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