/*! 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]); } }
/*! 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(); } }
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; } }