示例#1
0
/*!
  compute the homography using a displacement matrix.

  the homography is given by:

  \f$ {}^cH_{c_0} = {}^cR_{c_0} + \frac{{}^cT_{c_0} . {}^tN}{d_0} \f$

  Several internal variables are computed (dt, cRc0_0n)

  \param _cTc0 : the displacement matrix of the camera between the initial position of the camera and the current camera position
  \param _cHc0 : the homography of the plane
*/
void
vpMbtDistanceKltPoints::computeHomography(const vpHomogeneousMatrix& _cTc0, vpHomography& _cHc0)
{
  vpRotationMatrix cRc0;
  vpTranslationVector ctransc0;

  _cTc0.extract(cRc0);
  _cTc0.extract(ctransc0);
  vpMatrix cHc0 = _cHc0.convert();

//   vpGEMM(cRc0, 1.0, invd0, cRc0, -1.0, _cHc0, VP_GEMM_A_T);
  vpGEMM(ctransc0, N, -invd0, cRc0, 1.0, cHc0, VP_GEMM_B_T);
  cHc0 /= cHc0[2][2];

  H = cHc0;

//   vpQuaternionVector NQuat(N[0], N[1], N[2], 0.0);
//   vpQuaternionVector RotQuat(cRc0);
//   vpQuaternionVector RotQuatConj(-RotQuat.x(), -RotQuat.y(), -RotQuat.z(), RotQuat.w());
//   vpQuaternionVector partial = RotQuat * NQuat;
//   vpQuaternionVector resQuat = (partial * RotQuatConj);
//
//   cRc0_0n = vpColVector(3);
//   cRc0_0n[0] = resQuat.x();
//   cRc0_0n[1] = resQuat.y();
//   cRc0_0n[2] = resQuat.z();

  cRc0_0n = cRc0*N;

//   vpPlane p(corners[0], corners[1], corners[2]);
//   vpColVector Ncur = p.getNormal();
//   Ncur.normalize();
  N_cur = cRc0_0n;
  dt = 0.0;
  for (unsigned int i = 0; i < 3; i += 1){
    dt += ctransc0[i] * (N_cur[i]);
  }
}
示例#2
0
/*!
  Set the pose to be used in entry (as guess) of the next call to the track() function.
  This pose will be just used once.

  \warning This functionnality is not available when tracking cylinders.

  \param I : image corresponding to the desired pose.
  \param cdMo : Pose to affect.
*/
void
vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo)
{
  if((int)(kltCylinders.size()) != 0)
  {
    std::cout << "WARNING: Cannot set pose when model contains cylinder(s). This feature is not implemented yet." << std::endl;
    std::cout << "Tracker will be reinitialized with the given pose." << std::endl;
    cMo = cdMo;
    init(I);
  }
  else
  {
    vpMbtDistanceKltPoints *kltpoly;

#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
    std::vector<cv::Point2f> init_pts;
    std::vector<long> init_ids;
    std::vector<cv::Point2f> guess_pts;
#else
    unsigned int nbp = 0;
    for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it) {
      kltpoly = *it;
      if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2)
        nbp += (*it)->getCurrentNumberPoints();
    }

    CvPoint2D32f* init_pts = NULL;
    init_pts = (CvPoint2D32f*)cvAlloc(tracker.getMaxFeatures()*sizeof(init_pts[0]));
    long *init_ids = (long*)cvAlloc((unsigned int)tracker.getMaxFeatures()*sizeof(long));
    unsigned int iter_pts = 0;

    CvPoint2D32f* guess_pts = NULL;
    guess_pts = (CvPoint2D32f*)cvAlloc(tracker.getMaxFeatures()*sizeof(guess_pts[0]));
#endif

    vpHomogeneousMatrix cdMc = cdMo * cMo.inverse();
    vpHomogeneousMatrix cMcd = cdMc.inverse();

    vpRotationMatrix cdRc;
    vpTranslationVector cdtc;

    cdMc.extract(cdRc);
    cdMc.extract(cdtc);

    unsigned int nbCur = 0;

    for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it) {
      kltpoly = *it;

      if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
        kltpoly->polygon->changeFrame(cdMo);

        //Get the normal to the face at the current state cMo
        vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
        plan.changeFrame(cMcd);

        vpColVector Nc = plan.getNormal();
        Nc.normalize();

        double invDc = 1.0 / plan.getD();

        //Create the homography
        vpMatrix cdHc;
        vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
        cdHc /= cdHc[2][2];

        //Create the 2D homography
        vpMatrix cdGc = cam.get_K() * cdHc * cam.get_K_inverse();

        //Points displacement
        std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
        nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
        for( ; iter != kltpoly->getCurrentPoints().end(); iter++){
          vpColVector cdp(3);
          cdp[0] = iter->second.get_j(); cdp[1] = iter->second.get_i(); cdp[2] = 1.0;

#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
          cv::Point2f p((float)cdp[0], (float)cdp[1]);
          init_pts.push_back(p);
          init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
#else
          init_pts[iter_pts].x = (float)cdp[0];
          init_pts[iter_pts].y = (float)cdp[1];
          init_ids[iter_pts] = (kltpoly->getCurrentPointsInd())[(size_t)iter->first];
#endif

          double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];

          if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
            cdp[0] = 0.0;
            cdp[1] = 0.0;
            throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
          }

          cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
          cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;

          //Set value to the KLT tracker
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
          cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
          guess_pts.push_back(p_guess);
#else
          guess_pts[iter_pts].x = (float)cdp[0];
          guess_pts[iter_pts++].y = (float)cdp[1];
#endif
        }
      }
    }

    vpImageConvert::convert(I, cur);

#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
    tracker.setInitialGuess(init_pts, guess_pts, init_ids);
#else
    tracker.setInitialGuess(&init_pts, &guess_pts, init_ids, iter_pts);

    if(init_pts) cvFree(&init_pts);
    init_pts = NULL;

    if(guess_pts) cvFree(&guess_pts);
    guess_pts = NULL;

    if(init_ids)cvFree(&init_ids);
    init_ids = NULL;
#endif

    bool reInitialisation = false;
    if(!useOgre)
      faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
    else{
#ifdef VISP_HAVE_OGRE
      faces.setVisibleOgre(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
#else
      faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
#endif
    }

    cam.computeFov(I.getWidth(), I.getHeight());

    if(useScanLine){
      faces.computeClippedPolygons(cdMo,cam);
      faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
    }

    for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
      kltpoly = *it;
      if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2){
        kltpoly->polygon->computePolygonClipped(cam);
        kltpoly->init(tracker);
      }
    }

    cMo = cdMo;
    c0Mo = cMo;
    ctTc0.eye();
  }
}
示例#3
0
int
main()
{
  try {
    int err = 1;
    {
      vpColVector c(6, 1);
      vpRowVector r(6, 1);
      std::vector<double> bench(6, 1);
      vpMatrix M1(c);
      if (test("M1", M1, bench) == false)
        return err;
      vpMatrix M2(r);
      if (test("M2", M2, bench) == false)
        return err;
    }
    {
      vpMatrix M(4,5);
      int val = 0;
      for(unsigned int i=0; i<M.getRows(); i++) {
        for(unsigned int j=0; j<M.getCols(); j++) {
          M[i][j] = val++;
        }
      }
      std::cout <<"M ";
      M.print (std::cout, 4);

      vpMatrix N;
      N.init(M, 0, 1, 2, 3);
      std::cout <<"N ";
      N.print (std::cout, 4);
      std::string header("My 4-by-5 matrix\nwith a second line");

      // Save matrix in text format
      if (vpMatrix::saveMatrix("matrix.mat", M, false, header.c_str()))
        std::cout << "Matrix saved in matrix.mat file" << std::endl;
      else
        return err;

      // Load matrix in text format
      vpMatrix M1;
      char header_[100];
      if (vpMatrix::loadMatrix("matrix.mat", M1, false, header_))
        std::cout << "Matrix loaded from matrix.mat file with header \"" << header_ << "\": \n" << M1 << std::endl;
      else
        return err;
      if (header != std::string(header_)) {
        std::cout << "Bad header in matrix.mat" << std::endl;
        return err;
      }

      // Save matrix in binary format
      if (vpMatrix::saveMatrix("matrix.bin", M, true, header.c_str()))
        std::cout << "Matrix saved in matrix.bin file" << std::endl;
      else
        return err;

      // Load matrix in binary format
      if (vpMatrix::loadMatrix("matrix.bin", M1, true, header_))
        std::cout << "Matrix loaded from matrix.bin file with header \"" << header_ << "\": \n" << M1 << std::endl;
      else
        return err;
      if (header != std::string(header_)) {
        std::cout << "Bad header in matrix.bin" << std::endl;
        return err;
      }

      // Save matrix in YAML format
      if (vpMatrix::saveMatrixYAML("matrix.yml", M, header.c_str()))
        std::cout << "Matrix saved in matrix.yml file" << std::endl;
      else
        return err;

      // Read matrix in YAML format
      vpMatrix M2;
      if (vpMatrix::loadMatrixYAML("matrix.yml", M2, header_))
        std::cout << "Matrix loaded from matrix.yml file with header \"" << header_ << "\": \n" << M2 << std::endl;
      else
        return err;
      if (header != std::string(header_)) {
        std::cout << "Bad header in matrix.mat" << std::endl;
        return err;
      }
    }

    {
      vpRotationMatrix R(vpMath::rad(10), vpMath::rad(20), vpMath::rad(30));
      std::cout << "R: \n" << R << std::endl;
      vpMatrix M1(R);
      std::cout << "M1: \n" << M1 << std::endl;
      vpMatrix M2(M1);
      std::cout << "M2: \n" << M2 << std::endl;
      vpMatrix M3 = R;
      std::cout << "M3: \n" << M3 << std::endl;
      vpMatrix M4 = M1;
      std::cout << "M4: \n" << M4 << std::endl;
    }
    {

      std::cout << "------------------------" << std::endl;
      std::cout << "--- TEST PRETTY PRINT---" << std::endl;
      std::cout << "------------------------" << std::endl;
      vpMatrix M ;
      M.eye(4);

      std::cout << "call std::cout << M;" << std::endl;
      std::cout << M << std::endl;

      std::cout << "call M.print (std::cout, 4);" << std::endl;
      M.print (std::cout, 4);

      std::cout << "------------------------" << std::endl;
      M.resize(3,3) ;
      M.eye(3);
      M[1][0]=1.235;
      M[1][1]=12.345;
      M[1][2]=.12345;
      std::cout << "call std::cout << M;" << std::endl;
      std::cout << M;
      std::cout << "call M.print (std::cout, 6);" << std::endl;
      M.print (std::cout, 6);
      std::cout << std::endl;

      std::cout << "------------------------" << std::endl;
      M[0][0]=-1.235;
      M[1][0]=-12.235;

      std::cout << "call std::cout << M;" << std::endl;
      std::cout << M << std::endl;

      std::cout << "call M.print (std::cout, 10);" << std::endl;
      M.print (std::cout, 10);
      std::cout << std::endl;

      std::cout << "call M.print (std::cout, 2);" << std::endl;
      M.print (std::cout, 2);
      std::cout << std::endl;

      std::cout << "------------------------" << std::endl;
      M.resize(3,3) ;
      M.eye(3);
      M[0][2]=-0.0000000876;
      std::cout << "call std::cout << M;" << std::endl;
      std::cout << M << std::endl;

      std::cout << "call M.print (std::cout, 4);" << std::endl;
      M.print (std::cout, 4);
      std::cout << std::endl;
      std::cout << "call M.print (std::cout, 10, \"M\");" << std::endl;
      M.print (std::cout, 10, "M");
      std::cout << std::endl;
      std::cout << "call M.print (std::cout, 20, \"M\");" << std::endl;
      M.print (std::cout, 20, "M");
      std::cout << std::endl;


      std::cout << "------------------------" << std::endl;
      std::cout << "--- TEST RESIZE --------" << std::endl;
      std::cout << "------------------------" << std::endl;
      std::cout <<  "5x5" << std::endl;
      M.resize(5,5,false);
      std::cout << M << std::endl;
      std::cout << "3x2" << std::endl;
      M.resize(3,2,false);
      std::cout << M << std::endl;
      std::cout << "2x2" << std::endl;
      M.resize(2,2,false);
      std::cout << M << std::endl;
      std::cout << "------------------------" << std::endl;

      vpVelocityTwistMatrix vMe;
      vpMatrix A(1,6),B;

      A=1.0;
      //vMe=1.0;
      B=A*vMe;

      std::cout << "------------------------" << std::endl;
      std::cout << "--- TEST vpRowVector * vpColVector" << std::endl;
      std::cout << "------------------------" << std::endl;
      vpRowVector r(3);
      r[0] = 2;
      r[1] = 3;
      r[2] = 4;

      vpColVector c(3);
      c[0] = 1;
      c[1] = 2;
      c[2] = -1;

      double rc = r * c;

      r.print(std::cout, 2, "r");
      c.print(std::cout, 2, "c");
      std::cout << "r * c = " << rc << std::endl;

      std::cout << "------------------------" << std::endl;
      std::cout << "--- TEST vpRowVector * vpMatrix" << std::endl;
      std::cout << "------------------------" << std::endl;
      M.resize(3,3) ;
      M.eye(3);

      M[1][0] = 1.5;
      M[2][0] = 2.3;

      vpRowVector rM = r * M;

      r.print(std::cout, 2, "r");
      M.print(std::cout, 10, "M");
      std::cout << "r * M = " << rM << std::endl;

      std::cout << "------------------------" << std::endl;
      std::cout << "--- TEST vpGEMM " << std::endl;
      std::cout << "------------------------" << std::endl;
      M.resize(3,3) ;
      M.eye(3);
      vpMatrix N(3, 3);
      N[0][0] = 2;
      N[1][0] = 1.2;
      N[1][2] = 0.6;
      N[2][2] = 0.25;

      vpMatrix C(3, 3);
      C.eye(3);

      vpMatrix D;

      //realise the operation D = 2 * M^T * N + 3 C
      vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
      std::cout << D << std::endl;

      std::cout << "All tests succeed" << std::endl;
      return 0;
    }
  }
  catch(vpException &e) {
    std::cout << "Catch an exception: " << e << std::endl;
    return 1;
  }
}