Ejemplo n.º 1
0
std::vector<glm::mat4> Animation::getKeyPose(const unsigned int& keyFrame, const std::vector<unsigned int>& roots, const std::vector<Joint>& hierarchy) const
{
	std::vector<glm::mat4> pose(timeLine[0].poses.size(), glm::mat4(1.f));
	for (unsigned int i = 0; i < roots.size() && keyFrame < timeLine.size(); i++)
		computePose(keyFrame, pose, glm::mat4(1.f), roots[i], hierarchy);
	return pose;
}
Ejemplo n.º 2
0
void
  Cylinder::computePose(Eigen::Vector3f origin, std::vector<std::vector<Eigen::Vector3f> >& contours_3d)
  {
    Eigen::Affine3f t;
    pcl::getTransformationFromTwoUnitVectorsAndOrigin(sym_axis_, sym_axis_.unitOrthogonal(), origin, t);
    Eigen::Vector3f z_cyl = t * contours_3d[0][0];
    z_cyl(1) = 0;
    Eigen::Vector3f z_axis = t.inverse().rotation() * z_cyl;
    computePose(origin, z_axis.normalized());
  }
Ejemplo n.º 3
0
//	Protected functions
void Animation::computePose(const unsigned int& keyFrame, std::vector<glm::mat4>& pose, const glm::mat4& parentPose, unsigned int joint, const std::vector<Joint>& hierarchy) const
{
	glm::mat4 t = glm::translate(timeLine[keyFrame].poses[joint].position);
	glm::mat4 r = glm::toMat4(timeLine[keyFrame].poses[joint].rotation);
	glm::mat4 s = glm::scale(timeLine[keyFrame].poses[joint].scale);

	pose[joint] = parentPose *t * r * s;
	for (unsigned int i = 0; i < hierarchy[joint].sons.size(); i++)
		computePose(keyFrame, pose, pose[joint], hierarchy[joint].sons[i], hierarchy);
}
int main()
{
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && (defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_CMU1394))
  vpImage<unsigned char> I;

#if defined(VISP_HAVE_DC1394_2)
  vp1394TwoGrabber g;
#elif defined(VISP_HAVE_CMU1394)
  vp1394CMUGrabber g;
#endif
  g.open(I);

  // Parameters of our camera
  vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);

  // The pose container
  vpHomogeneousMatrix cMo;

  std::vector<vpDot2> dot(4);
  std::vector<vpPoint> point(4);
  double L = 0.06;
  point[0].setWorldCoordinates(-L, -L, 0);
  point[1].setWorldCoordinates( L, -L, 0);
  point[2].setWorldCoordinates( L,  L, 0);
  point[3].setWorldCoordinates(-L,  L, 0);

  bool init = true;
#if defined(VISP_HAVE_X11)
  vpDisplayX d(I);
#elif defined(VISP_HAVE_GDI)
  vpDisplayGDI d(I);
#endif

  while(1){
    // Image Acquisition
    g.acquire(I);
    vpDisplay::display(I);
    track(I, dot, init);
    computePose(point, dot, cam, init, cMo);
    vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
    vpDisplay::flush(I);
    if (init) init = false; // turn off the initialisation specific stuff

    if (vpDisplay::getClick(I, false))
        break;
  }
#endif
}
Ejemplo n.º 5
0
  void
  Cylinder::updateAttributes(const Eigen::Vector3f& sym_axis, const Eigen::Vector3f& origin, const Eigen::Vector3f& z_axis)
  {
    //origin_= new_origin;
    sym_axis_ = sym_axis;
    if (sym_axis_[2] < 0 )
    {
      sym_axis_ = sym_axis_ * -1;
    }
    computePose(origin, z_axis.normalized());
    //d_ = fabs(pose_.translation().dot(normal_));
    //normal_ = new_normal;

    /*Eigen::Affine3f transform_from_plane_to_world;
  getTransformationFromPlaneToWorld(sym_axis,normal,origin_,transform_from_plane_to_world);
  transform_from_world_to_plane=transform_from_plane_to_world.inverse();*/
  }
Ejemplo n.º 6
0
 Cylinder::Cylinder(unsigned int id,
                    Eigen::Vector3f origin,
                    Eigen::Vector3f sym_axis,
                    double radius,
                    std::vector<std::vector<Eigen::Vector3f> >& contours_3d,
                    std::vector<bool> holes,
                    std::vector<float> color)
  : Polygon()
 {
   //Cylinder();
   id_ = id;
   sym_axis_ = sym_axis;
   r_ = radius;
   holes_ = holes;
   color_ = color;
   if (sym_axis_[2] < 0 )
   {
     sym_axis_ = sym_axis_ * -1;
   }
   computePose(origin, contours_3d);
   setContours3D(contours_3d);
   computeHeight();
 }
Ejemplo n.º 7
0
 Cylinder::Cylinder(unsigned int id,
                    Eigen::Vector3f origin,
                    Eigen::Vector3f sym_axis,
                    double radius,
                    std::vector<pcl::PointCloud<pcl::PointXYZ> >& contours_3d,
                    std::vector<bool> holes,
                    std::vector<float> color)
 : Polygon()
 {
   std::vector<std::vector<Eigen::Vector3f> > contours_eigen;
   for(unsigned int i = 0; i < contours_3d.size(); i++)
   {
     std::vector<Eigen::Vector3f> c_eigen;
     for(unsigned int j = 0; j < contours_3d[i].size(); j++)
     {
       Eigen::Vector3f pt = contours_3d[i].points[j].getVector3fMap();
       c_eigen.push_back(pt);
     }
     contours_eigen.push_back(c_eigen);
   }
   id_ = id;
   sym_axis_ = sym_axis;
   r_ = radius;
   //std::cout << "origin " << origin << std::endl;
   //std::cout << "r " << r_ << std::endl;
   holes_ = holes;
   color_ = color;
   if (sym_axis_[2] < 0 )
   {
     sym_axis_ = sym_axis_ * -1;
   }
   computePose(origin, contours_eigen);
   setContours3D(contours_eigen);
   computeHeight();
   //Cylinder(id, origin, sym_axis, radius, contours_eigen, holes, color);
 }
Ejemplo n.º 8
0
/*!
  Compute the calibration according to the desired method using one pose.

  \param method : Method that will be used to estimate the parameters.
  \param cMo_est : estimated homogeneous matrix that defines the pose.
  \param cam_est : estimated intrinsic camera parameters.
  \param verbose : set at true if information about the residual at each loop
  of the algorithm is hoped.

  \return 0 if the calibration computation succeed.
*/
int vpCalibration::computeCalibration(vpCalibrationMethodType method,
                                      vpHomogeneousMatrix &cMo_est,
                                      vpCameraParameters &cam_est,
                                      bool verbose)
{
  try{
    computePose(cam_est,cMo_est);
    switch (method)
    {
    case CALIB_LAGRANGE :
    case CALIB_LAGRANGE_VIRTUAL_VS :
      {
        calibLagrange(cam_est, cMo_est);
      }
      break;
    case CALIB_VIRTUAL_VS:
    case CALIB_VIRTUAL_VS_DIST:
    case CALIB_LAGRANGE_VIRTUAL_VS_DIST:
    default:
      break;
    }

    switch (method)
    {
    case CALIB_VIRTUAL_VS:
    case CALIB_VIRTUAL_VS_DIST:
    case CALIB_LAGRANGE_VIRTUAL_VS:
    case CALIB_LAGRANGE_VIRTUAL_VS_DIST:
      {
        if (verbose){std::cout << "start calibration without distortion"<< std::endl;}
        calibVVS(cam_est, cMo_est, verbose);
      }
      break ;
    case CALIB_LAGRANGE:
    default:
      break;
    }
    this->cMo = cMo_est;
    this->cMo_dist = cMo_est;

    //Print camera parameters
    if(verbose){
      //       std::cout << "Camera parameters without distortion :" << std::endl;
      cam_est.printParameters();
    }

    this->cam = cam_est;

    switch (method)
    {
    case CALIB_VIRTUAL_VS_DIST:
    case CALIB_LAGRANGE_VIRTUAL_VS_DIST:
      {
        if (verbose){std::cout << "start calibration with distortion"<< std::endl;}
        calibVVSWithDistortion(cam_est, cMo_est, verbose);
      }
      break ;
    case CALIB_LAGRANGE:
    case CALIB_VIRTUAL_VS:
    case CALIB_LAGRANGE_VIRTUAL_VS:
    default:
      break;
    }
    //Print camera parameters
    if(verbose){
      //       std::cout << "Camera parameters without distortion :" << std::endl;
      this->cam.printParameters();
      //       std::cout << "Camera parameters with distortion :" << std::endl;
      cam_est.printParameters();
    }

    this->cam_dist = cam_est ;

    this->cMo_dist = cMo_est;
    return 0 ;
  }
  catch(...){
    throw;
  }
}
int main()
{
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  try {  vpImage<unsigned char> I;

#if defined(VISP_HAVE_DC1394)
    vp1394TwoGrabber g;
    g.open(I);
#elif defined(VISP_HAVE_CMU1394)
    vp1394CMUGrabber g;
    g.open(I);
#elif defined(VISP_HAVE_OPENCV)
    cv::VideoCapture g(0); // open the default camera
    if(!g.isOpened()) { // check if we succeeded
      std::cout << "Failed to open the camera" << std::endl;
      return -1;
    }
    cv::Mat frame;
    g >> frame; // get a new frame from camera
	vpImageConvert::convert(frame, I);
#endif

    // Parameters of our camera
    vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);

    // The pose container
    vpHomogeneousMatrix cMo;

    std::vector<vpDot2> dot(4);
    std::vector<vpPoint> point(4);
    double L = 0.06;
    point[0].setWorldCoordinates(-L, -L, 0);
    point[1].setWorldCoordinates( L, -L, 0);
    point[2].setWorldCoordinates( L,  L, 0);
    point[3].setWorldCoordinates(-L,  L, 0);

    bool init = true;
#if defined(VISP_HAVE_X11)
    vpDisplayX d(I);
#elif defined(VISP_HAVE_GDI)
    vpDisplayGDI d(I);
#elif defined(VISP_HAVE_OPENCV)
    vpDisplayOpenCV d(I);
#endif

    while(1){
      // Image Acquisition
#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394)
      g.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
	  g >> frame;
      vpImageConvert::convert(frame, I);
#endif

      vpDisplay::display(I);
      track(I, dot, init);
      computePose(point, dot, cam, init, cMo);
      vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
      vpDisplay::flush(I);
      if (init) init = false; // turn off the initialisation specific stuff

      if (vpDisplay::getClick(I, false))
        break;
    }
  }
  catch(vpException e) {
    std::cout << "Catch an exception: " << e << std::endl;
  }
#endif
}
int main()
{
#if defined(VISP_HAVE_ZBAR)
  try {
    vpImage<unsigned char> I;
    vpImageIo::read(I, "bar-code.pgm");

#if defined(VISP_HAVE_X11)
    vpDisplayX d(I);
#elif defined(VISP_HAVE_GDI)
    vpDisplayGDI d(I);
#elif defined(VISP_HAVE_OPENCV)
    vpDisplayOpenCV d(I);
#endif

    // Camera parameters should be adapted to your camera
    vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);

    // 3D model of the QRcode: here we consider a 12cm by 12cm QRcode
    std::vector<vpPoint> point;
    point.push_back( vpPoint(-0.06, -0.06, 0) ); // QCcode point 0 3D coordinates in plane Z=0
    point.push_back( vpPoint( 0.06, -0.06, 0) ); // QCcode point 1 3D coordinates in plane Z=0
    point.push_back( vpPoint( 0.06,  0.06, 0) ); // QCcode point 2 3D coordinates in plane Z=0
    point.push_back( vpPoint(-0.06,  0.06, 0) ); // QCcode point 3 3D coordinates in plane Z=0

    vpHomogeneousMatrix cMo;
    bool init = true;

    vpDetectorQRCode detector;

    while(1) {
      vpImageIo::read(I, "bar-code.pgm");
      vpDisplay::display(I);

      bool status = detector.detect(I);

      std::ostringstream legend;
      legend << detector.getNbObjects() << " bar code detected";
      vpDisplay::displayText(I, (int)I.getHeight()-30, 10, legend.str(), vpColor::red);

      if (status) { // true if at least one QRcode is detected
        for(size_t i=0; i < detector.getNbObjects(); i++) {

          std::vector<vpImagePoint> p = detector.getPolygon(i); // get the four corners location in the image

          for(size_t j=0; j < p.size(); j++) {
            vpDisplay::displayCross(I, p[j], 14, vpColor::red, 3);
            std::ostringstream number;
            number << j;
            vpDisplay::displayText(I, p[j]+vpImagePoint(15,5), number.str(), vpColor::blue);
          }

          computePose(point, p, cam, init, cMo); // resulting pose is available in cMo var
          std::cout << "Pose translation (meter): " << cMo.getTranslationVector().t() << std::endl
                    << "Pose rotation (quaternion): " << vpQuaternionVector(cMo.getRotationMatrix()).t() << std::endl;
          vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
        }
      }
      vpDisplay::displayText(I, (int)I.getHeight()-15, 10, "A click to quit...", vpColor::red);
      vpDisplay::flush(I);

      if (vpDisplay::getClick(I, false)) break;

      vpTime::wait(40);
    }
  }
  catch(const vpException &e) {
    std::cout << "Catch an exception: " << e.getMessage() << std::endl;
  }
#else
  std::cout << "ViSP is not build with zbar 3rd party." << std::endl;
#endif
}
Ejemplo n.º 11
0
int main(int argc, const char** argv)
{
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100) && (defined(VISP_HAVE_ZBAR) || defined(VISP_HAVE_DMTX))
  int opt_device = 0;
  int opt_barcode = 0; // 0=QRCode, 1=DataMatrix
  std::string opt_ip = "198.18.0.1";

  for (unsigned int i=0; i<argc; i++) {
    if (std::string(argv[i]) == "--ip")
      opt_ip = argv[i+1];
    else if (std::string(argv[i]) == "--code-type")
      opt_barcode = atoi(argv[i+1]);
    else if (std::string(argv[i]) == "--help")
      std::cout << "Usage: " << argv[0]
                << " [--ip <robot ip address>] [--code-type <0 for QRcode | 1 for DataMatrix>] [--help]"
                << std::endl;
  }
  try {

    vpNaoqiGrabber g;
    if (! opt_ip.empty())
      g.setRobotIp(opt_ip);

    g.setCamera(0);
    g.open();

    vpImage<unsigned char> I; // for gray images
    g.acquire(I);
    vpDisplayX d(I);

    vpDetectorBase *detector;
    if (opt_barcode == 0)
      detector = new vpDetectorQRCode;
#ifdef VISP_HAVE_DMTX
    else
      detector = new vpDetectorDataMatrixCode;
#endif

    vpTemplateTrackerWarpAffine warp;
    vpTemplateTrackerSSDInverseCompositional tracker(&warp);
    tracker.setSampling(2,2);
    tracker.setLambda(0.001);
    tracker.setIterationMax(5);
    tracker.setPyramidal(2, 1);

    state_t state = detection;
    vpTemplateTrackerZone zone_ref, zone_cur;
    double area_zone_ref, area_zone_cur, area_zone_prev;
    vpColVector p; // Estimated parameters
    std::vector<vpImagePoint> corners_detected;
    std::vector<vpImagePoint> corners_tracked;
    std::vector<int> corners_tracked_index;
    bool target_found = false;
    vpRect target_bbox; // BBox of the tracked qrcode

    vpCameraParameters cam = g.getCameraParameters();
    vpHomogeneousMatrix cMo;

    std::vector<vpPoint> P(4);
    double qrcode_size = 0.035;
    P[0].setWorldCoordinates( qrcode_size/2,  qrcode_size/2, 0); //                             / \ x
    P[1].setWorldCoordinates(-qrcode_size/2,  qrcode_size/2, 0); //                              |
    P[2].setWorldCoordinates(-qrcode_size/2, -qrcode_size/2, 0); // small dot on the qrcode y <--|
    P[3].setWorldCoordinates( qrcode_size/2, -qrcode_size/2, 0);


    for(;;) {
      double t = vpTime::measureTimeMs();
      g.acquire(I);
      vpDisplay::display(I);

      target_found = false;

      if (state == detection) {
        vpDisplay::displayCharString(I, 10,10, "state: detection", vpColor::red);
        bool status = detector->detect(I);
        if (status) {
          vpDisplay::displayText(I, 20, 10, "Bar code detected", vpColor::green);
          for (size_t i=0; i < detector->getNbObjects(); i++) {
            if (detector->getMessage(i) == "romeo_left_arm") {

              corners_detected = detector->getPolygon(i);
              state = init_tracking;
              target_found = true;

              vpDisplay::displayText(I, I.getHeight()-20, 10, "Bar code message: " + detector->getMessage(i), vpColor::green);
              for(size_t j=0; j < corners_detected.size(); j++) {
                std::ostringstream s;
                s << j;
                vpDisplay::displayText(I, corners_detected[j]+vpImagePoint(-20,-20), s.str(), vpColor::green);
                vpDisplay::displayCross(I, corners_detected[j], 25, vpColor::green, 2);
              }
            }
          }
        }
      }
      if (target_found && state == init_tracking) {
        vpDisplay::displayCharString(I, 10,10, "state: init tracking", vpColor::red);
        try {
          tracker.resetTracker();
          tracker.initFromPoints(I, corners_detected, true);
          tracker.track(I);
          //tracker.display(I, vpColor::green);
          zone_ref = tracker.getZoneRef();
          area_zone_ref = zone_ref.getArea();
          p = tracker.getp();
          warp.warpZone(zone_ref, p, zone_cur);
          area_zone_prev = area_zone_cur = zone_cur.getArea();
          corners_tracked = getTemplateTrackerCorners(zone_cur);
          corners_tracked_index = computedTemplateTrackerCornersIndexes(corners_detected, corners_tracked);
          corners_tracked = orderPointsFromIndexes(corners_tracked_index, corners_tracked);

          computePose(P, corners_tracked, cam, true, cMo);
          vpDisplay::displayFrame(I, cMo, cam, 0.04, vpColor::none, 3);

          state = tracking;
        }
        catch(...) {
          std::cout << "Exception init tracking" << std::endl;
          state = detection;
        }
      }
      else if (state == tracking) {
        try {
          vpDisplay::displayCharString(I, 10,10, "state: tracking", vpColor::red);
          tracker.track(I);

          //tracker.display(I, vpColor::blue);
          {
            // Instantiate and get the reference zone
            p = tracker.getp();
            warp.warpZone(zone_ref, p, zone_cur);
            area_zone_cur = zone_cur.getArea();

            double size_percent = 0.9;
            double max_target_size = I.getSize()/4;
            if (area_zone_cur/area_zone_prev < size_percent || area_zone_cur/area_zone_prev > (1+size_percent)) {
              std::cout << "reinit caused by size" << std::endl;
              state = detection;
            }
            else if(zone_cur.getBoundingBox().getSize() > max_target_size) {
              std::cout << "reinit caused by size area" << std::endl;
              state = detection;
            }
            else {
              corners_tracked = getTemplateTrackerCorners(zone_cur);
              corners_tracked = orderPointsFromIndexes(corners_tracked_index, corners_tracked);

              computePose(P, corners_tracked, cam, false, cMo);
              vpDisplay::displayFrame(I, cMo, cam, 0.04, vpColor::none, 3);

              for(unsigned int j=0; j<corners_tracked.size(); j++) {
                std::ostringstream s;
                s << corners_tracked_index[j];
                vpDisplay::displayText(I, corners_tracked[j]+vpImagePoint(-10,-10), s.str(), vpColor::blue);
                vpDisplay::displayCross(I, corners_tracked[j], 15, vpColor::green, 2);
              }


              target_found = true;
            }

            area_zone_prev = area_zone_cur;
          }
        }
        catch(...) {
          std::cout << "Exception tracking" << std::endl;
          state = detection;
        }
      }

      //      if (target_found)
      //        vpDisplay::displayRectangle(I, target_bbox, vpColor::red, false, 2);

      vpDisplay::flush(I);
      if (vpDisplay::getClick(I, false)) // a click to exit
        break;
      std::cout << "Loop time: " << vpTime::measureTimeMs() - t << std::endl;
    }
  }
  catch(vpException e) {
    std::cout << "Catch an exception: " << e << std::endl;
  }
#endif
}
bool vpBlobsTargetTracker::track(const cv::Mat &cvI, const vpImage<unsigned char> &I )
{

  if (m_state == detection || m_force_detection) {
    //std::cout << "STATE: DETECTION "<< std::endl;

    bool obj_found = false;
    if (!m_manual_blob_init && !m_full_manual)
      obj_found = m_colBlob.detect(cvI);
    // Delete previuos list of blobs
    m_blob_list.clear();
    m_blob_list.resize(4);
    m_initPose = true;
    m_target_found = false;

    if (obj_found || m_manual_blob_init || m_full_manual) {

      try{


        if (m_full_manual)
        {
          std::cout << "Full manual" << std::endl;
          vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Click on the 4 blobs", vpColor::red);

          vpDisplay::flush(I);
          for(std::list<vpDot2>::iterator it=m_blob_list.begin(); it != m_blob_list.end(); ++it)
          {
            (*it).setGraphics(true);
            (*it).setGraphicsThickness(1);
            (*it).initTracking(I);
            (*it).track(I);
            vpDisplay::flush(I);

          }
          m_state = tracking;
          m_force_detection = false;
        }


        else {

          // std::cout << "TARGET FOUND" << std::endl;

          vpDot2 blob;
          blob.setGraphics(true);
          blob.setGraphicsThickness(1);
          blob.setEllipsoidShapePrecision(0.9);
          if (m_manual_blob_init)
          {
            vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Click on the colored blob", vpColor::red);
            vpDisplay::flush(I);
            blob.initTracking(I);
            m_manual_blob_init = false;
          }
          else
          {
            vpImagePoint cog = m_colBlob.getCog(0);
            vpDisplay::displayCross(I,cog,10, vpColor::red,2 );
            blob.initTracking(I,cog);
          }
          blob.track(I);

//          printf("Dot characteristics: \n");
//          printf("  width : %lf\n", blob.getWidth());
//          printf("  height: %lf\n", blob.getHeight());
//          printf("  area: %lf\n", blob.getArea());
//          printf("  gray level min: %d\n", blob.getGrayLevelMin());
//          printf("  gray level max: %d\n", blob.getGrayLevelMax());
//          printf("  grayLevelPrecision: %lf\n", blob.getGrayLevelPrecision());
//          printf("  sizePrecision: %lf\n", blob.getSizePrecision());
//          printf("  ellipsoidShapePrecision: %lf\n", blob.getEllipsoidShapePrecision());

          vpDot2 black_blob = blob;
          black_blob.setGrayLevelMax(m_grayLevelMaxBlob);
          black_blob.setGrayLevelMin(m_grayLevelMinBlob);

          int i,j,aj,ai;

          if(m_left_hand_target)
          {
            i = blob.getCog().get_i()-blob.getHeight()*2.3;
            j = blob.getCog().get_j()-blob.getWidth()*3.3;
            ai = blob.getHeight()*5;
            aj = blob.getWidth()*5;
          }
          else
          {
            i = blob.getCog().get_i()-blob.getHeight();
            j = blob.getCog().get_j()-blob.getWidth()*2.0;
            ai = blob.getHeight()*4.5;
            aj = blob.getWidth()*4;

          }

          //search similar blobs in the image and store them in blob_list
          //black_blob.searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), m_blob_list);
          //vpDisplay::displayRectangle(I, i, j, ai, aj, vpColor::red, false, 1);
          black_blob.searchDotsInArea(I, j, i, ai, aj, m_blob_list);

          //        vpDisplay::flush(I);
          //        vpDisplay::getClick(I,true);

          m_blob_list.insert(m_blob_list.begin(),blob);
          std::cout << "SIZE: " << m_blob_list.size() << std::endl;

          if(m_blob_list.size() == m_numBlobs)
          {
            for(std::list<vpDot2>::iterator it = m_blob_list.begin(); it != m_blob_list.end(); ++it)
            {
              //it->setEllipsoidShapePrecision(0.8);
              it->setEllipsoidShapePrecision(0.75);
            }
            if (1){
              for(std::list<vpDot2>::iterator it = m_blob_list.begin(); it != m_blob_list.end(); ++it)
              {
                it->initTracking(I, it->getCog());

              }
            }
            m_state = tracking;
            m_force_detection = false;
          }
          else
            std::cout << "Number blobs found is "<< m_blob_list.size() << ". Expected number: " << m_numBlobs << std::endl;
        }
      }
      catch(vpException &e) {
        std::cout << "Exception tracking detection: " << e.getStringMessage() << std::endl;
        m_target_found = false;
      }


    }
  }
  else if (m_state == tracking) {
    // std::cout << "STATE: TRACKING "<< std::endl;
    try {

      m_cog.set_uv(0.0,0.0);
      for(std::list<vpDot2>::iterator it = m_blob_list.begin(); it != m_blob_list.end(); ++it)
      {
        it->track(I);
        m_cog += it->getCog();
      }


      m_cog /= m_blob_list.size();

      // Display the ACTUAL center of gravity of the object
      //vpDisplay::displayCross(I,cog_tot,10, vpColor::blue,2 );

      //      std::vector<vpImagePoint> corners(m_blob_list.begin(),m_blob_list.end());



      // Now we create a map of VpPoint in order to order the points

      std::map< double,vpImagePoint> poly_verteces;

      double theta;
      for(std::list<vpDot2>::iterator it=m_blob_list.begin(); it != m_blob_list.end(); ++it)
      {
        // Get the cog of the blob
        vpImagePoint cog = it->getCog();

        theta = atan2(cog.get_v() - m_cog.get_v(), cog.get_u() - m_cog.get_u());
        // Insert the vertexes in the map (ordered)
        poly_verteces.insert ( std::pair<double,vpImagePoint>(theta,cog) );

      }

      // Now we create a Vector containing the ordered vertexes

      std::vector<vpImagePoint> poly_vert;
      int index_first= 0;
      unsigned int count = 0;

      for( std::map<double,vpImagePoint>::iterator it = poly_verteces.begin();  it!=poly_verteces.end(); ++it )
      {

        poly_vert.push_back( it->second );
        if (m_blob_list.front().getCog() == it->second )
          index_first = count;
        count++;

      }

      std::rotate(poly_vert.begin(), poly_vert.begin() + index_first, poly_vert.end());
      //std::cout << "---------------------------------------" << std::endl;
      for(unsigned int j = 0; j<poly_vert.size();j++)
      {
        std::ostringstream s;
        s << j;
        vpDisplay::displayText(I, poly_vert[j], s.str(), vpColor::green);
        //std::cout << "Cog blob " << j << " :" << poly_vert[j] << std::endl;
      }
      computePose(m_P, poly_vert, m_cam, m_initPose, m_cMo);




      bool duplicate = false;
      for(unsigned int i = 0; i < poly_vert.size()-1; i++)
      {
        for(unsigned int j = i+1; j < poly_vert.size(); j++)
        {
          //std::cout << "Distance " << i <<"-" << j << " :" << vpImagePoint::sqrDistance(poly_vert[i],poly_vert[j] )<< std::endl;
          if (vpImagePoint::sqrDistance(poly_vert[i],poly_vert[j]) < 5.0)
            duplicate = true;
        }
      }

      //std::cout << "---------------------------------------" << std::endl;

      // std::cout << "Number blobs found is "<< poly_vert.size() << ". Expected number: " << m_numBlobs << std::endl;

      //      int i = m_blob_list.front().getCog().get_i()-m_blob_list.front().getHeight()*2.3;
      //      int j = m_blob_list.front().getCog().get_j()-m_blob_list.front().getWidth()*3.3;
      //      unsigned int ai = m_blob_list.front().getHeight()*5;
      //      unsigned int aj =m_blob_list.front().getWidth()*5;
      //      vpDisplay::displayRectangle(I,i, j, ai,aj, vpColor::red,false,1);


      if (duplicate)
      {
        m_target_found = false;
        m_state = detection;
        std::cout << "PROBLEM: tracking failed " << m_numBlobs << std::endl;

      }
      else if (poly_vert.size() != m_numBlobs)
      {
        m_target_found = false;
        m_state = detection;
        std::cout << "PROBLEM: Expected number: " << m_numBlobs << std::endl;
      }

      else
        m_target_found = true;

    }
    catch(vpException &e) {
      std::cout << "Exception tracking: " << e.getStringMessage() << std::endl;
      m_state = detection;
      m_target_found = false;
    }
  }
  return m_target_found;
}