55 #include <visp3/core/vpMath.h> 56 #include <visp3/core/vpRotationMatrix.h> 57 #include <visp3/vision/vpHomography.h> 58 #include <visp3/core/vpDebug.h> 59 #include <visp3/core/vpThetaUVector.h> 61 #include <visp3/core/vpPoint.h> 62 #include <visp3/core/vpMath.h> 63 #include <visp3/core/vpHomogeneousMatrix.h> 64 #include <visp3/core/vpDebug.h> 65 #include <visp3/io/vpParseArgv.h> 68 #define GETOPTARGS "h" 72 void usage(
const char *name,
const char *badparam);
73 bool getOptions(
int argc,
const char **argv);
84 void usage(
const char *name,
const char *badparam)
87 Test the HLM (Malis) homography estimation algorithm with a planar object.\n\ 98 fprintf(stderr,
"ERROR: \n" );
99 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
112 bool getOptions(
int argc,
const char **argv)
119 case 'h': usage(argv[0], NULL);
return false;
break;
122 usage(argv[0], optarg_);
127 if ((c == 1) || (c == -1)) {
129 usage(argv[0], NULL);
130 std::cerr <<
"ERROR: " << std::endl;
131 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
140 main(
int argc,
const char ** argv)
144 if (getOptions(argc, argv) ==
false) {
149 std::vector<double> xa(nbpt), ya(nbpt) ;
150 std::vector<double> xb(nbpt), yb(nbpt) ;
167 for(
unsigned int i=0 ; i < nbpt ; i++)
171 xa[i] = P[i].
get_x() ;
172 ya[i] = P[i].
get_y() ;
175 for(
unsigned int i=0 ; i < nbpt ; i++)
179 xb[i] = P[i].
get_x() ;
180 yb[i] = P[i].
get_y() ;
182 std::cout <<
"-------------------------------" <<std::endl ;
183 std::cout <<
"aMb "<<std::endl <<aMb << std::endl ;
184 std::cout <<
"-------------------------------" <<std::endl ;
190 std::cout <<
"aHb computed using the Malis paralax algorithm: \n" << aHb<< std::endl ;
196 std::cout <<
"-------------------------------" <<std::endl ;
197 std::cout <<
"extract R, T and n " << std::endl;
199 std::cout <<
"Rotation: aRb" <<std::endl ;
200 std::cout << aRb << std::endl ;
201 std::cout <<
"Translation: aTb" <<std::endl;
202 std::cout << (aTb).t() <<std::endl ;
203 std::cout <<
"Normal to the plane: n" <<std::endl;
204 std::cout << (n).t() <<std::endl ;
207 std::cout <<
"-------------------------------" <<std::endl ;
208 std::cout <<
"Compare with built homography H = R + t/d " << std::endl ;
211 std::cout <<
"aHb built from the displacement " << std::endl ;
212 std::cout << std::endl <<aHb_built/aHb_built[2][2] << std::endl ;
214 aHb_built.computeDisplacement(aRb, aTb, n) ;
215 std::cout <<
"Rotation: aRb" <<std::endl ;
216 std::cout << aRb << std::endl ;
217 std::cout <<
"Translation: aTb" <<std::endl;
218 std::cout << (aTb).t() <<std::endl ;
219 std::cout <<
"Normal to the plane: n" <<std::endl;
220 std::cout << (n).t() <<std::endl ;
222 std::cout <<
"-------------------------------" << std::endl ;
223 std::cout <<
"test if ap = aHb bp" << std::endl ;
225 for(
unsigned int i=0 ; i < nbpt ; i++)
227 std::cout <<
"Point "<< i<< std::endl ;
231 std::cout <<
") = (" ;
236 std::cout <<
"-------------------------------" <<std::endl ;
237 std::cout <<
"test displacement" << std::endl ;
239 std::list<vpRotationMatrix> laRb ;
240 std::list<vpTranslationVector> laTb ;
241 std::list<vpColVector> lnb ;
246 std::list<vpRotationMatrix>::const_iterator it_laRb = laRb.begin();
247 std::list<vpTranslationVector>::const_iterator it_laTb = laTb.begin();
248 std::list<vpColVector>::const_iterator it_lnb = lnb.begin();
251 while (it_lnb != lnb.end())
253 std::cout <<
"Solution " << k++ << std::endl ;
258 std::cout <<
"Rotation: aRb" <<std::endl ;
259 std::cout << aRb << std::endl ;
260 std::cout <<
"Translation: aTb" <<std::endl;
261 std::cout << (aTb).t() <<std::endl ;
262 std::cout <<
"Normal to the plane: n" <<std::endl;
263 std::cout << (n).t() <<std::endl ;
272 std::cout <<
"Catch an exception: " << e << std::endl;
Implementation of an homogeneous matrix and operations on such kind of matrices.
error that can be emited by ViSP classes.
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines what is a point.
static void HLM(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, bool isplanar, vpHomography &aHb)
Implementation of a rotation matrix and operations on such kind of matrices.
double get_w() const
Get the point w coordinate in the image plane.
Implementation of an homography and operations on homographies.
static double rad(double deg)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Implementation of column vector and the associated operations.
double get_x() const
Get the point x coordinate in the image plane.
double get_y() const
Get the point y coordinate in the image plane.
This class defines the container for a plane geometrical structure.
Class that consider the case of a translation vector.