vpWireFrameSimulator.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Wire frame simulator
 *
 * Authors:
 * Nicolas Melchior
 *
 *****************************************************************************/

#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <vector>
#include <visp3/robot/vpWireFrameSimulator.h>

#include "vpArit.h"
#include "vpBound.h"
#include "vpClipping.h"
#include "vpCoreDisplay.h"
#include "vpKeyword.h"
#include "vpLex.h"
#include "vpMy.h"
#include "vpParser.h"
#include "vpProjection.h"
#include "vpRfstack.h"
#include "vpScene.h"
#include "vpTmstack.h"
#include "vpToken.h"
#include "vpView.h"
#include "vpVwstack.h"

#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPoint.h>

extern Point2i *point2i;
extern Point2i *listpoint2i;

/*
  Copy the scene corresponding to the registeresd parameters in the image.
*/
void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color)
{
  // extern Bound *clipping_Bound ();
  Bound *bp, *bend;
  Bound *clip; /* surface apres clipping */
  Byte b = (Byte)*get_rfstack();
  Matrix m;

  // bcopy ((char *) mat, (char *) m, sizeof (Matrix));
  memmove((char *)m, (char *)mat, sizeof(Matrix));
  View_to_Matrix(get_vwstack(), *(get_tmstack()));
  postmult_matrix(m, *(get_tmstack()));
  bp = sc.bound.ptr;
  bend = bp + sc.bound.nbr;
  for (; bp < bend; bp++) {
    if ((clip = clipping_Bound(bp, m)) != NULL) {
      Face *fp = clip->face.ptr;
      Face *fend = fp + clip->face.nbr;

      set_Bound_face_display(clip, b); // regarde si is_visible

      point_3D_2D(clip->point.ptr, clip->point.nbr, (int)I.getWidth(), (int)I.getHeight(), point2i);
      for (; fp < fend; fp++) {
        if (fp->is_visible) {
          wireframe_Face(fp, point2i);
          Point2i *pt = listpoint2i;
          for (int i = 1; i < fp->vertex.nbr; i++) {
            vpDisplay::displayLine(I, vpImagePoint((pt)->y, (pt)->x), vpImagePoint((pt + 1)->y, (pt + 1)->x), color,
                                   thickness_);
            pt++;
          }
          if (fp->vertex.nbr > 2) {
            vpDisplay::displayLine(I, vpImagePoint((listpoint2i)->y, (listpoint2i)->x), vpImagePoint((pt)->y, (pt)->x),
                                   color, thickness_);
          }
        }
      }
    }
  }
}

/*
  Copy the scene corresponding to the registeresd parameters in the image.
*/
void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I,
                                         const vpColor &color)
{
  // extern Bound *clipping_Bound ();

  Bound *bp, *bend;
  Bound *clip; /* surface apres clipping */
  Byte b = (Byte)*get_rfstack();
  Matrix m;

  // bcopy ((char *) mat, (char *) m, sizeof (Matrix));
  memmove((char *)m, (char *)mat, sizeof(Matrix));
  View_to_Matrix(get_vwstack(), *(get_tmstack()));
  postmult_matrix(m, *(get_tmstack()));
  bp = sc.bound.ptr;
  bend = bp + sc.bound.nbr;
  for (; bp < bend; bp++) {
    if ((clip = clipping_Bound(bp, m)) != NULL) {
      Face *fp = clip->face.ptr;
      Face *fend = fp + clip->face.nbr;

      set_Bound_face_display(clip, b); // regarde si is_visible

      point_3D_2D(clip->point.ptr, clip->point.nbr, (int)I.getWidth(), (int)I.getHeight(), point2i);
      for (; fp < fend; fp++) {
        if (fp->is_visible) {
          wireframe_Face(fp, point2i);
          Point2i *pt = listpoint2i;
          for (int i = 1; i < fp->vertex.nbr; i++) {
            vpDisplay::displayLine(I, vpImagePoint((pt)->y, (pt)->x), vpImagePoint((pt + 1)->y, (pt + 1)->x), color,
                                   thickness_);
            pt++;
          }
          if (fp->vertex.nbr > 2) {
            vpDisplay::displayLine(I, vpImagePoint((listpoint2i)->y, (listpoint2i)->x), vpImagePoint((pt)->y, (pt)->x),
                                   color, thickness_);
          }
        }
      }
    }
  }
}

/*************************************************************************************************************/

vpWireFrameSimulator::vpWireFrameSimulator()
  : scene(), desiredScene(), camera(), objectImage(), fMo(), fMc(), camMf(), refMo(), cMo(), cdMo(), object(PLATE),
    desiredObject(D_STANDARD), camColor(vpColor::green), camTrajColor(vpColor::green), curColor(vpColor::blue),
    desColor(vpColor::red), sceneInitialized(false), displayCameraTrajectory(true), cameraTrajectory(), poseList(),
    fMoList(), nbrPtLimit(1000), old_iPr(), old_iPz(), old_iPt(), blockedr(false), blockedz(false), blockedt(false),
    blocked(false), camMf2(), f2Mf(), px_int(1), py_int(1), px_ext(1), py_ext(1), displayObject(false),
    displayDesiredObject(false), displayCamera(false), displayImageSimulator(false), cameraFactor(1.),
    camTrajType(CT_LINE), extCamChanged(false), rotz(), thickness_(1), scene_dir()
{
  // set scene_dir from #define VISP_SCENE_DIR if it exists
  // VISP_SCENES_DIR may contain multiple locations separated by ";"
  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
  bool sceneDirExists = false;
  for (size_t i = 0; i < scene_dirs.size(); i++)
    if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
      scene_dir = scene_dirs[i];
      sceneDirExists = true;
      break;
    }
  if (!sceneDirExists) {
    try {
      scene_dir = vpIoTools::getenv("VISP_SCENES_DIR");
      std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir << std::endl;
    } catch (...) {
      std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
    }
  }

  open_display();
  open_clipping();

  old_iPr = vpImagePoint(-1, -1);
  old_iPz = vpImagePoint(-1, -1);
  old_iPt = vpImagePoint(-1, -1);

  rotz.buildFrom(0, 0, 0, 0, 0, vpMath::rad(180));

  scene.name = NULL;
  scene.bound.ptr = NULL;
  scene.bound.nbr = 0;

  desiredScene.name = NULL;
  desiredScene.bound.ptr = NULL;
  desiredScene.bound.nbr = 0;

  camera.name = NULL;
  camera.bound.ptr = NULL;
  camera.bound.nbr = 0;
}

vpWireFrameSimulator::~vpWireFrameSimulator()
{
  if (sceneInitialized) {
    if (displayObject)
      free_Bound_scene(&(this->scene));
    if (displayCamera) {
      free_Bound_scene(&(this->camera));
    }
    if (displayDesiredObject)
      free_Bound_scene(&(this->desiredScene));
  }
  close_clipping();
  close_display();

  cameraTrajectory.clear();
  poseList.clear();
  fMoList.clear();
}

void vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object)
{
  char name_cam[FILENAME_MAX];
  char name[FILENAME_MAX];

  object = obj;
  this->desiredObject = desired_object;

  const char *scene_dir_ = scene_dir.c_str();
  if (strlen(scene_dir_) >= FILENAME_MAX) {
    throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the camera name"));
  }

  strcpy(name_cam, scene_dir_);
  if (desiredObject != D_TOOL) {
    strcat(name_cam, "/camera.bnd");
    set_scene(name_cam, &camera, cameraFactor);
  } else {
    strcat(name_cam, "/tool.bnd");
    set_scene(name_cam, &(this->camera), 1.0);
  }

  strcpy(name, scene_dir_);
  switch (obj) {
  case THREE_PTS: {
    strcat(name, "/3pts.bnd");
    break;
  }
  case CUBE: {
    strcat(name, "/cube.bnd");
    break;
  }
  case PLATE: {
    strcat(name, "/plate.bnd");
    break;
  }
  case SMALL_PLATE: {
    strcat(name, "/plate_6cm.bnd");
    break;
  }
  case RECTANGLE: {
    strcat(name, "/rectangle.bnd");
    break;
  }
  case SQUARE_10CM: {
    strcat(name, "/square10cm.bnd");
    break;
  }
  case DIAMOND: {
    strcat(name, "/diamond.bnd");
    break;
  }
  case TRAPEZOID: {
    strcat(name, "/trapezoid.bnd");
    break;
  }
  case THREE_LINES: {
    strcat(name, "/line.bnd");
    break;
  }
  case ROAD: {
    strcat(name, "/road.bnd");
    break;
  }
  case TIRE: {
    strcat(name, "/circles2.bnd");
    break;
  }
  case PIPE: {
    strcat(name, "/pipe.bnd");
    break;
  }
  case CIRCLE: {
    strcat(name, "/circle.bnd");
    break;
  }
  case SPHERE: {
    strcat(name, "/sphere.bnd");
    break;
  }
  case CYLINDER: {
    strcat(name, "/cylinder.bnd");
    break;
  }
  case PLAN: {
    strcat(name, "/plan.bnd");
    break;
  }
  case POINT_CLOUD: {
    strcat(name, "/point_cloud.bnd");
    break;
  }
  }
  set_scene(name, &(this->scene), 1.0);

  scene_dir_ = scene_dir.c_str();
  if (strlen(scene_dir_) >= FILENAME_MAX) {
    throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the desired object name"));
  }

  switch (desiredObject) {
  case D_STANDARD: {
    break;
  }
  case D_CIRCLE: {
    strcpy(name, scene_dir_);
    strcat(name, "/circle_sq2.bnd");
    break;
  }
  case D_TOOL: {
    strcpy(name, scene_dir_);
    strcat(name, "/tool.bnd");
    break;
  }
  }
  set_scene(name, &(this->desiredScene), 1.0);

  if (obj == PIPE)
    load_rfstack(IS_INSIDE);
  else
    add_rfstack(IS_BACK);

  add_vwstack("start", "depth", 0.0, 100.0);
  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
  add_vwstack("start", "type", PERSPECTIVE);

  sceneInitialized = true;
  displayObject = true;
  displayDesiredObject = true;
  displayCamera = true;
  displayImageSimulator = true;
}

void vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object,
                                     const std::list<vpImageSimulator> &imObj)
{
  initScene(obj, desired_object);
  objectImage = imObj;
  displayImageSimulator = true;
}

void vpWireFrameSimulator::initScene(const char *obj, const char *desired_object)
{
  char name_cam[FILENAME_MAX];
  char name[FILENAME_MAX];

  object = THREE_PTS;
  this->desiredObject = D_STANDARD;

  const char *scene_dir_ = scene_dir.c_str();
  if (strlen(scene_dir_) >= FILENAME_MAX) {
    throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the camera name"));
  }

  strcpy(name_cam, scene_dir_);
  strcat(name_cam, "/camera.bnd");
  set_scene(name_cam, &camera, cameraFactor);

  if (strlen(obj) >= FILENAME_MAX) {
    throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the name"));
  }

  strcpy(name, obj);
  Model_3D model;
  model = getExtension(obj);
  if (model == BND_MODEL)
    set_scene(name, &(this->scene), 1.0);
  else if (model == WRL_MODEL)
    set_scene_wrl(name, &(this->scene), 1.0);
  else if (model == UNKNOWN_MODEL) {
    vpERROR_TRACE("Unknown file extension for the 3D model");
  }

  if (strlen(desired_object) >= FILENAME_MAX) {
    throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the camera name"));
  }

  strcpy(name, desired_object);
  model = getExtension(desired_object);
  if (model == BND_MODEL)
    set_scene(name, &(this->desiredScene), 1.0);
  else if (model == WRL_MODEL)
    set_scene_wrl(name, &(this->desiredScene), 1.0);
  else if (model == UNKNOWN_MODEL) {
    vpERROR_TRACE("Unknown file extension for the 3D model");
  }

  add_rfstack(IS_BACK);

  add_vwstack("start", "depth", 0.0, 100.0);
  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
  add_vwstack("start", "type", PERSPECTIVE);

  sceneInitialized = true;
  displayObject = true;
  displayDesiredObject = true;
  displayCamera = true;
}

void vpWireFrameSimulator::initScene(const char *obj, const char *desired_object,
                                     const std::list<vpImageSimulator> &imObj)
{
  initScene(obj, desired_object);
  objectImage = imObj;
  displayImageSimulator = true;
}

void vpWireFrameSimulator::initScene(const vpSceneObject &obj)
{
  char name_cam[FILENAME_MAX];
  char name[FILENAME_MAX];

  object = obj;

  const char *scene_dir_ = scene_dir.c_str();
  if (strlen(scene_dir_) >= FILENAME_MAX) {
    throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the camera name"));
  }

  strcpy(name_cam, scene_dir_);
  strcat(name_cam, "/camera.bnd");
  set_scene(name_cam, &camera, cameraFactor);

  strcpy(name, scene_dir_);
  switch (obj) {
  case THREE_PTS: {
    strcat(name, "/3pts.bnd");
    break;
  }
  case CUBE: {
    strcat(name, "/cube.bnd");
    break;
  }
  case PLATE: {
    strcat(name, "/plate.bnd");
    break;
  }
  case SMALL_PLATE: {
    strcat(name, "/plate_6cm.bnd");
    break;
  }
  case RECTANGLE: {
    strcat(name, "/rectangle.bnd");
    break;
  }
  case SQUARE_10CM: {
    strcat(name, "/square10cm.bnd");
    break;
  }
  case DIAMOND: {
    strcat(name, "/diamond.bnd");
    break;
  }
  case TRAPEZOID: {
    strcat(name, "/trapezoid.bnd");
    break;
  }
  case THREE_LINES: {
    strcat(name, "/line.bnd");
    break;
  }
  case ROAD: {
    strcat(name, "/road.bnd");
    break;
  }
  case TIRE: {
    strcat(name, "/circles2.bnd");
    break;
  }
  case PIPE: {
    strcat(name, "/pipe.bnd");
    break;
  }
  case CIRCLE: {
    strcat(name, "/circle.bnd");
    break;
  }
  case SPHERE: {
    strcat(name, "/sphere.bnd");
    break;
  }
  case CYLINDER: {
    strcat(name, "/cylinder.bnd");
    break;
  }
  case PLAN: {
    strcat(name, "/plan.bnd");
    break;
  }
  case POINT_CLOUD: {
    strcat(name, "/point_cloud.bnd");
    break;
  }
  }
  set_scene(name, &(this->scene), 1.0);

  if (obj == PIPE)
    load_rfstack(IS_INSIDE);
  else
    add_rfstack(IS_BACK);

  add_vwstack("start", "depth", 0.0, 100.0);
  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
  add_vwstack("start", "type", PERSPECTIVE);

  sceneInitialized = true;
  displayObject = true;
  displayCamera = true;

  displayDesiredObject = false;
  displayImageSimulator = false;
}

void vpWireFrameSimulator::initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj)
{
  initScene(obj);
  objectImage = imObj;
  displayImageSimulator = true;
}

void vpWireFrameSimulator::initScene(const char *obj)
{
  char name_cam[FILENAME_MAX];
  char name[FILENAME_MAX];

  object = THREE_PTS;

  const char *scene_dir_ = scene_dir.c_str();
  if (strlen(scene_dir_) >= FILENAME_MAX) {
    throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the camera name"));
  }

  strcpy(name_cam, scene_dir_);
  strcat(name_cam, "/camera.bnd");
  set_scene(name_cam, &camera, cameraFactor);

  if (strlen(obj) >= FILENAME_MAX) {
    throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the name"));
  }

  strcpy(name, obj);
  Model_3D model;
  model = getExtension(obj);
  if (model == BND_MODEL)
    set_scene(name, &(this->scene), 1.0);
  else if (model == WRL_MODEL)
    set_scene_wrl(name, &(this->scene), 1.0);
  else if (model == UNKNOWN_MODEL) {
    vpERROR_TRACE("Unknown file extension for the 3D model");
  }

  add_rfstack(IS_BACK);

  add_vwstack("start", "depth", 0.0, 100.0);
  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
  add_vwstack("start", "type", PERSPECTIVE);

  sceneInitialized = true;
  displayObject = true;
  displayCamera = true;
}

void vpWireFrameSimulator::initScene(const char *obj, const std::list<vpImageSimulator> &imObj)
{
  initScene(obj);
  objectImage = imObj;
  displayImageSimulator = true;
}

void vpWireFrameSimulator::getInternalImage(vpImage<vpRGBa> &I)
{
  if (!sceneInitialized)
    throw(vpException(vpException::notInitialized, "The scene has to be initialized"));

  double u;
  double v;
  // if(px_int != 1 && py_int != 1)
  // we assume px_int and py_int > 0
  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
      (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
    u = (double)I.getWidth() / (2 * px_int);
    v = (double)I.getHeight() / (2 * py_int);
  } else {
    u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
    v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
  }

  float o44c[4][4], o44cd[4][4], x, y, z;
  Matrix id = IDENTITY_MATRIX;

  vp2jlc_matrix(cMo.inverse(), o44c);
  vp2jlc_matrix(cdMo.inverse(), o44cd);

  if (displayImageSimulator) {
    I = 255;

    for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
      vpImageSimulator *imSim = &(*it);
      imSim->setCameraPosition(rotz * cMo);
      imSim->getImage(I, getInternalCameraParameters(I));
    }

    if (I.display != NULL)
      vpDisplay::display(I);
  }

  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
  x = o44c[2][0] + o44c[3][0];
  y = o44c[2][1] + o44c[3][1];
  z = o44c[2][2] + o44c[3][2];
  add_vwstack("start", "vrp", x, y, z);
  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
  add_vwstack("start", "window", -u, u, -v, v);
  if (displayObject)
    display_scene(id, this->scene, I, curColor);

  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
  x = o44cd[2][0] + o44cd[3][0];
  y = o44cd[2][1] + o44cd[3][1];
  z = o44cd[2][2] + o44cd[3][2];
  add_vwstack("start", "vrp", x, y, z);
  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
  add_vwstack("start", "window", -u, u, -v, v);
  if (displayDesiredObject) {
    if (desiredObject == D_TOOL)
      display_scene(o44cd, desiredScene, I, vpColor::red);
    else
      display_scene(id, desiredScene, I, desColor);
  }
}

void vpWireFrameSimulator::getExternalImage(vpImage<vpRGBa> &I)
{
  bool changed = false;
  vpHomogeneousMatrix displacement = navigation(I, changed);

  // if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] !=
  // 0*/)
  if (std::fabs(displacement[2][3]) >
      std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
    camMf2 = camMf2 * displacement;

  f2Mf = camMf2.inverse() * camMf;

  camMf = camMf2 * displacement * f2Mf;

  double u;
  double v;
  // if(px_ext != 1 && py_ext != 1)
  // we assume px_ext and py_ext > 0
  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
      (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
    u = (double)I.getWidth() / (2 * px_ext);
    v = (double)I.getHeight() / (2 * py_ext);
  } else {
    u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
    v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
  }

  float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;

  vp2jlc_matrix(camMf.inverse(), w44cext);
  vp2jlc_matrix(fMc, w44c);
  vp2jlc_matrix(fMo, w44o);

  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
  x = w44cext[2][0] + w44cext[3][0];
  y = w44cext[2][1] + w44cext[3][1];
  z = w44cext[2][2] + w44cext[3][2];
  add_vwstack("start", "vrp", x, y, z);
  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
  add_vwstack("start", "window", -u, u, -v, v);
  if ((object == CUBE) || (object == SPHERE)) {
    add_vwstack("start", "type", PERSPECTIVE);
  }

  if (displayImageSimulator) {
    I = 255;

    for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
      vpImageSimulator *imSim = &(*it);
      imSim->setCameraPosition(rotz * camMf * fMo);
      imSim->getImage(I, getInternalCameraParameters(I));
    }

    if (I.display != NULL)
      vpDisplay::display(I);
  }

  if (displayObject)
    display_scene(w44o, this->scene, I, curColor);

  if (displayCamera)
    display_scene(w44c, camera, I, camColor);

  if (displayCameraTrajectory) {
    vpImagePoint iP;
    vpImagePoint iP_1;
    poseList.push_back(cMo);
    fMoList.push_back(fMo);

    int iter = 0;

    if (changed || extCamChanged) {
      cameraTrajectory.clear();
      std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
      std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();

      while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end())) {
        iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
        cameraTrajectory.push_back(iP);
        if (camTrajType == CT_LINE) {
          if (iter != 0)
            vpDisplay::displayLine(I, iP_1, iP, camTrajColor, thickness_);
        } else if (camTrajType == CT_POINT)
          vpDisplay::displayPoint(I, iP, camTrajColor);
        ++iter_poseList;
        ++iter_fMoList;
        iter++;
        iP_1 = iP;
      }
      extCamChanged = false;
    } else {
      iP = projectCameraTrajectory(I, cMo, fMo);
      cameraTrajectory.push_back(iP);

      for (std::list<vpImagePoint>::const_iterator it = cameraTrajectory.begin(); it != cameraTrajectory.end(); ++it) {
        if (camTrajType == CT_LINE) {
          if (iter != 0)
            vpDisplay::displayLine(I, iP_1, *it, camTrajColor, thickness_);
        } else if (camTrajType == CT_POINT)
          vpDisplay::displayPoint(I, *it, camTrajColor);
        iter++;
        iP_1 = *it;
      }
    }

    if (poseList.size() > nbrPtLimit) {
      poseList.pop_front();
    }
    if (fMoList.size() > nbrPtLimit) {
      fMoList.pop_front();
    }
    if (cameraTrajectory.size() > nbrPtLimit) {
      cameraTrajectory.pop_front();
    }
  }
}

void vpWireFrameSimulator::getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cam_Mf)
{
  float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;

  vpHomogeneousMatrix camMft = rotz * cam_Mf;

  double u;
  double v;
  // if(px_ext != 1 && py_ext != 1)
  // we assume px_ext and py_ext > 0
  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
      (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
    u = (double)I.getWidth() / (2 * px_ext);
    v = (double)I.getHeight() / (2 * py_ext);
  } else {
    u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
    v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
  }

  vp2jlc_matrix(camMft.inverse(), w44cext);
  vp2jlc_matrix(fMo * cMo.inverse(), w44c);
  vp2jlc_matrix(fMo, w44o);

  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
  x = w44cext[2][0] + w44cext[3][0];
  y = w44cext[2][1] + w44cext[3][1];
  z = w44cext[2][2] + w44cext[3][2];
  add_vwstack("start", "vrp", x, y, z);
  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
  add_vwstack("start", "window", -u, u, -v, v);

  if (displayImageSimulator) {
    I = 255;

    for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
      vpImageSimulator *imSim = &(*it);
      imSim->setCameraPosition(rotz * cam_Mf * fMo);
      imSim->getImage(I, getInternalCameraParameters(I));
    }

    if (I.display != NULL)
      vpDisplay::display(I);
  }

  if (displayObject)
    display_scene(w44o, this->scene, I, curColor);
  if (displayCamera)
    display_scene(w44c, camera, I, camColor);
}

void vpWireFrameSimulator::getInternalImage(vpImage<unsigned char> &I)
{
  if (!sceneInitialized)
    throw(vpException(vpException::notInitialized, "The scene has to be initialized"));

  double u;
  double v;
  // if(px_int != 1 && py_int != 1)
  // we assume px_int and py_int > 0
  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
      (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
    u = (double)I.getWidth() / (2 * px_int);
    v = (double)I.getHeight() / (2 * py_int);
  } else {
    u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
    v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
  }

  float o44c[4][4], o44cd[4][4], x, y, z;
  Matrix id = IDENTITY_MATRIX;

  vp2jlc_matrix(cMo.inverse(), o44c);
  vp2jlc_matrix(cdMo.inverse(), o44cd);

  if (displayImageSimulator) {
    I = 255;

    for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
      vpImageSimulator *imSim = &(*it);
      imSim->setCameraPosition(rotz * camMf * fMo);
      imSim->getImage(I, getInternalCameraParameters(I));
    }

    if (I.display != NULL)
      vpDisplay::display(I);
  }

  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
  x = o44c[2][0] + o44c[3][0];
  y = o44c[2][1] + o44c[3][1];
  z = o44c[2][2] + o44c[3][2];
  add_vwstack("start", "vrp", x, y, z);
  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
  add_vwstack("start", "window", -u, u, -v, v);
  if (displayObject)
    display_scene(id, this->scene, I, curColor);

  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
  x = o44cd[2][0] + o44cd[3][0];
  y = o44cd[2][1] + o44cd[3][1];
  z = o44cd[2][2] + o44cd[3][2];
  add_vwstack("start", "vrp", x, y, z);
  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
  add_vwstack("start", "window", -u, u, -v, v);
  if (displayDesiredObject) {
    if (desiredObject == D_TOOL)
      display_scene(o44cd, desiredScene, I, vpColor::red);
    else
      display_scene(id, desiredScene, I, desColor);
  }
}

void vpWireFrameSimulator::getExternalImage(vpImage<unsigned char> &I)
{
  bool changed = false;
  vpHomogeneousMatrix displacement = navigation(I, changed);

  // if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] !=
  // 0*/)
  if (std::fabs(displacement[2][3]) >
      std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
    camMf2 = camMf2 * displacement;

  f2Mf = camMf2.inverse() * camMf;

  camMf = camMf2 * displacement * f2Mf;

  double u;
  double v;
  // if(px_ext != 1 && py_ext != 1)
  // we assume px_ext and py_ext > 0
  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
      (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
    u = (double)I.getWidth() / (2 * px_ext);
    v = (double)I.getHeight() / (2 * py_ext);
  } else {
    u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
    v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
  }

  float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;

  vp2jlc_matrix(camMf.inverse(), w44cext);
  vp2jlc_matrix(fMc, w44c);
  vp2jlc_matrix(fMo, w44o);

  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
  x = w44cext[2][0] + w44cext[3][0];
  y = w44cext[2][1] + w44cext[3][1];
  z = w44cext[2][2] + w44cext[3][2];
  add_vwstack("start", "vrp", x, y, z);
  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
  add_vwstack("start", "window", -u, u, -v, v);
  if ((object == CUBE) || (object == SPHERE)) {
    add_vwstack("start", "type", PERSPECTIVE);
  }

  if (displayImageSimulator) {
    I = 255;
    for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
      vpImageSimulator *imSim = &(*it);
      imSim->setCameraPosition(rotz * camMf * fMo);
      imSim->getImage(I, getInternalCameraParameters(I));
    }
    if (I.display != NULL)
      vpDisplay::display(I);
  }

  if (displayObject)
    display_scene(w44o, this->scene, I, curColor);

  if (displayCamera)
    display_scene(w44c, camera, I, camColor);

  if (displayCameraTrajectory) {
    vpImagePoint iP;
    vpImagePoint iP_1;
    poseList.push_back(cMo);
    fMoList.push_back(fMo);

    int iter = 0;

    if (changed || extCamChanged) {
      cameraTrajectory.clear();
      std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
      std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();

      while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end())) {
        iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
        cameraTrajectory.push_back(iP);
        // vpDisplay::displayPoint(I,cameraTrajectory.value(),vpColor::green);
        if (camTrajType == CT_LINE) {
          if (iter != 0)
            vpDisplay::displayLine(I, iP_1, iP, camTrajColor, thickness_);
        } else if (camTrajType == CT_POINT)
          vpDisplay::displayPoint(I, iP, camTrajColor);
        ++iter_poseList;
        ++iter_fMoList;
        iter++;
        iP_1 = iP;
      }
      extCamChanged = false;
    } else {
      iP = projectCameraTrajectory(I, cMo, fMo);
      cameraTrajectory.push_back(iP);

      for (std::list<vpImagePoint>::const_iterator it = cameraTrajectory.begin(); it != cameraTrajectory.end(); ++it) {
        if (camTrajType == CT_LINE) {
          if (iter != 0)
            vpDisplay::displayLine(I, iP_1, *it, camTrajColor, thickness_);
        } else if (camTrajType == CT_POINT)
          vpDisplay::displayPoint(I, *it, camTrajColor);
        iter++;
        iP_1 = *it;
      }
    }

    if (poseList.size() > nbrPtLimit) {
      poseList.pop_front();
    }
    if (fMoList.size() > nbrPtLimit) {
      fMoList.pop_front();
    }
    if (cameraTrajectory.size() > nbrPtLimit) {
      cameraTrajectory.pop_front();
    }
  }
}

void vpWireFrameSimulator::getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &cam_Mf)
{
  float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;

  vpHomogeneousMatrix camMft = rotz * cam_Mf;

  double u;
  double v;
  // if(px_ext != 1 && py_ext != 1)
  // we assume px_ext and py_ext > 0
  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
      (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
    u = (double)I.getWidth() / (2 * px_ext);
    v = (double)I.getHeight() / (2 * py_ext);
  } else {
    u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
    v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
  }

  vp2jlc_matrix(camMft.inverse(), w44cext);
  vp2jlc_matrix(fMo * cMo.inverse(), w44c);
  vp2jlc_matrix(fMo, w44o);

  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
  x = w44cext[2][0] + w44cext[3][0];
  y = w44cext[2][1] + w44cext[3][1];
  z = w44cext[2][2] + w44cext[3][2];
  add_vwstack("start", "vrp", x, y, z);
  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
  add_vwstack("start", "window", -u, u, -v, v);

  if (displayImageSimulator) {
    I = 255;
    for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
      vpImageSimulator *imSim = &(*it);
      imSim->setCameraPosition(rotz * cam_Mf * fMo);
      imSim->getImage(I, getInternalCameraParameters(I));
    }
    if (I.display != NULL)
      vpDisplay::display(I);
  }

  if (displayObject)
    display_scene(w44o, this->scene, I, curColor);
  if (displayCamera)
    display_scene(w44c, camera, I, camColor);
}

void vpWireFrameSimulator::displayTrajectory(const vpImage<unsigned char> &I,
                                             const std::list<vpHomogeneousMatrix> &list_cMo,
                                             const std::list<vpHomogeneousMatrix> &list_fMo,
                                             const vpHomogeneousMatrix &cMf)
{
  if (list_cMo.size() != list_fMo.size())
    throw(vpException(vpException::dimensionError, "The two lists must have the same size"));

  vpImagePoint iP;
  vpImagePoint iP_1;
  int iter = 0;

  std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
  std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();

  while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end())) {
    iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
    if (camTrajType == CT_LINE) {
      if (iter != 0)
        vpDisplay::displayLine(I, iP_1, iP, camTrajColor, thickness_);
    } else if (camTrajType == CT_POINT)
      vpDisplay::displayPoint(I, iP, camTrajColor);
    ++it_cMo;
    ++it_fMo;
    iter++;
    iP_1 = iP;
  }
}

void vpWireFrameSimulator::displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
                                             const std::list<vpHomogeneousMatrix> &list_fMo,
                                             const vpHomogeneousMatrix &cMf)
{
  if (list_cMo.size() != list_fMo.size())
    throw(vpException(vpException::dimensionError, "The two lists must have the same size"));

  vpImagePoint iP;
  vpImagePoint iP_1;
  int iter = 0;

  std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
  std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();

  while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end())) {
    iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
    if (camTrajType == CT_LINE) {
      if (iter != 0)
        vpDisplay::displayLine(I, iP_1, iP, camTrajColor, thickness_);
    } else if (camTrajType == CT_POINT)
      vpDisplay::displayPoint(I, iP, camTrajColor);
    ++it_cMo;
    ++it_fMo;
    iter++;
    iP_1 = iP;
  }
}

vpHomogeneousMatrix vpWireFrameSimulator::navigation(const vpImage<vpRGBa> &I, bool &changed)
{
  double width = vpMath::minimum(I.getWidth(), I.getHeight());
  vpImagePoint iP;
  vpImagePoint trash;
  bool clicked = false;
  bool clickedUp = false;
  vpMouseButton::vpMouseButtonType b = vpMouseButton::button1;

  vpHomogeneousMatrix mov(0, 0, 0, 0, 0, 0);
  changed = false;

  // if(!blocked) vpDisplay::getClickUp(I,trash, b,false);

  if (!blocked)
    clicked = vpDisplay::getClick(I, trash, b, false);

  if (blocked)
    clickedUp = vpDisplay::getClickUp(I, trash, b, false);

  if (clicked) {
    if (b == vpMouseButton::button1)
      blockedr = true;
    if (b == vpMouseButton::button2)
      blockedz = true;
    if (b == vpMouseButton::button3)
      blockedt = true;
    blocked = true;
  }
  if (clickedUp) {
    if (b == vpMouseButton::button1) {
      old_iPr = vpImagePoint(-1, -1);
      blockedr = false;
    }
    if (b == vpMouseButton::button2) {
      old_iPz = vpImagePoint(-1, -1);
      blockedz = false;
    }
    if (b == vpMouseButton::button3) {
      old_iPt = vpImagePoint(-1, -1);
      blockedt = false;
    }
    if (!(blockedr || blockedz || blockedt)) {
      blocked = false;
      while (vpDisplay::getClick(I, trash, b, false)) {
      };
    }
  }

  vpDisplay::getPointerPosition(I, iP);

  if (old_iPr != vpImagePoint(-1, -1) && blockedr) {
    double diffi = iP.get_i() - old_iPr.get_i();
    double diffj = iP.get_j() - old_iPr.get_j();
    // cout << "delta :" << diffj << endl;;
    double anglei = diffi * 360 / width;
    double anglej = diffj * 360 / width;
    mov.buildFrom(0, 0, 0, vpMath::rad(-anglei), vpMath::rad(anglej), 0);
    changed = true;
  }

  if (blockedr)
    old_iPr = iP;

  if (old_iPz != vpImagePoint(-1, -1) && blockedz) {
    double diffi = iP.get_i() - old_iPz.get_i();
    mov.buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
    changed = true;
  }

  if (blockedz)
    old_iPz = iP;

  if (old_iPt != vpImagePoint(-1, -1) && blockedt) {
    double diffi = iP.get_i() - old_iPt.get_i();
    double diffj = iP.get_j() - old_iPt.get_j();
    mov.buildFrom(diffj * 0.01, diffi * 0.01, 0, 0, 0, 0);
    changed = true;
  }

  if (blockedt)
    old_iPt = iP;

  return mov;
}

vpHomogeneousMatrix vpWireFrameSimulator::navigation(const vpImage<unsigned char> &I, bool &changed)
{
  double width = vpMath::minimum(I.getWidth(), I.getHeight());
  vpImagePoint iP;
  vpImagePoint trash;
  bool clicked = false;
  bool clickedUp = false;
  vpMouseButton::vpMouseButtonType b = vpMouseButton::button1;

  vpHomogeneousMatrix mov(0, 0, 0, 0, 0, 0);
  changed = false;

  // if(!blocked) vpDisplay::getClickUp(I,trash, b,false);

  if (!blocked)
    clicked = vpDisplay::getClick(I, trash, b, false);

  if (blocked)
    clickedUp = vpDisplay::getClickUp(I, trash, b, false);

  if (clicked) {
    if (b == vpMouseButton::button1)
      blockedr = true;
    if (b == vpMouseButton::button2)
      blockedz = true;
    if (b == vpMouseButton::button3)
      blockedt = true;
    blocked = true;
  }
  if (clickedUp) {
    if (b == vpMouseButton::button1) {
      old_iPr = vpImagePoint(-1, -1);
      blockedr = false;
    }
    if (b == vpMouseButton::button2) {
      old_iPz = vpImagePoint(-1, -1);
      blockedz = false;
    }
    if (b == vpMouseButton::button3) {
      old_iPt = vpImagePoint(-1, -1);
      blockedt = false;
    }
    if (!(blockedr || blockedz || blockedt)) {
      blocked = false;
      while (vpDisplay::getClick(I, trash, b, false)) {
      };
    }
  }

  vpDisplay::getPointerPosition(I, iP);

  // std::cout << "point : " << iP << std::endl;

  if (old_iPr != vpImagePoint(-1, -1) && blockedr) {
    double diffi = iP.get_i() - old_iPr.get_i();
    double diffj = iP.get_j() - old_iPr.get_j();
    // cout << "delta :" << diffj << endl;;
    double anglei = diffi * 360 / width;
    double anglej = diffj * 360 / width;
    mov.buildFrom(0, 0, 0, vpMath::rad(-anglei), vpMath::rad(anglej), 0);
    changed = true;
  }

  if (blockedr)
    old_iPr = iP;

  if (old_iPz != vpImagePoint(-1, -1) && blockedz) {
    double diffi = iP.get_i() - old_iPz.get_i();
    mov.buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
    changed = true;
  }

  if (blockedz)
    old_iPz = iP;

  if (old_iPt != vpImagePoint(-1, -1) && blockedt) {
    double diffi = iP.get_i() - old_iPt.get_i();
    double diffj = iP.get_j() - old_iPt.get_j();
    mov.buildFrom(diffj * 0.01, diffi * 0.01, 0, 0, 0, 0);
    changed = true;
  }

  if (blockedt)
    old_iPt = iP;

  return mov;
}

vpImagePoint vpWireFrameSimulator::projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
                                                           const vpHomogeneousMatrix &fMo_)
{
  vpPoint point;
  point.setWorldCoordinates(0, 0, 0);

  point.track(rotz * (camMf * fMo_ * cMo_.inverse()));

  vpImagePoint iP;

  vpMeterPixelConversion::convertPoint(getExternalCameraParameters(I), point.get_x(), point.get_y(), iP);

  return iP;
}

vpImagePoint vpWireFrameSimulator::projectCameraTrajectory(const vpImage<unsigned char> &I,
                                                           const vpHomogeneousMatrix &cMo_,
                                                           const vpHomogeneousMatrix &fMo_)
{
  vpPoint point;
  point.setWorldCoordinates(0, 0, 0);

  point.track(rotz * (camMf * fMo_ * cMo_.inverse()));

  vpImagePoint iP;

  vpMeterPixelConversion::convertPoint(getExternalCameraParameters(I), point.get_x(), point.get_y(), iP);

  return iP;
}

vpImagePoint vpWireFrameSimulator::projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
                                                           const vpHomogeneousMatrix &fMo_,
                                                           const vpHomogeneousMatrix &cMf)
{
  vpPoint point;
  point.setWorldCoordinates(0, 0, 0);

  point.track(rotz * (cMf * fMo_ * cMo_.inverse()));

  vpImagePoint iP;

  vpMeterPixelConversion::convertPoint(getExternalCameraParameters(I), point.get_x(), point.get_y(), iP);

  return iP;
}

vpImagePoint vpWireFrameSimulator::projectCameraTrajectory(const vpImage<unsigned char> &I,
                                                           const vpHomogeneousMatrix &cMo_,
                                                           const vpHomogeneousMatrix &fMo_,
                                                           const vpHomogeneousMatrix &cMf)
{
  vpPoint point;
  point.setWorldCoordinates(0, 0, 0);

  point.track(rotz * (cMf * fMo_ * cMo_.inverse()));

  vpImagePoint iP;

  vpMeterPixelConversion::convertPoint(getExternalCameraParameters(I), point.get_x(), point.get_y(), iP);

  return iP;
}