53 #include <visp3/core/vpMath.h> 54 #include <visp3/core/vpRotationMatrix.h> 55 #include <visp3/vision/vpHomography.h> 56 #include <visp3/core/vpDebug.h> 57 #include <visp3/core/vpThetaUVector.h> 59 #include <visp3/core/vpPoint.h> 60 #include <visp3/core/vpMath.h> 61 #include <visp3/core/vpHomogeneousMatrix.h> 62 #include <visp3/core/vpDebug.h> 63 #include <visp3/io/vpParseArgv.h> 66 #define GETOPTARGS "h" 71 void usage(
const char *name,
const char *badparam);
72 bool getOptions(
int argc,
const char **argv);
83 void usage(
const char *name,
const char *badparam)
86 Test the HLM (Malis) homography estimation algorithm with a 3D object.\n\ 97 fprintf(stderr,
"ERROR: \n" );
98 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
111 bool getOptions(
int argc,
const char **argv)
118 case 'h': usage(argv[0], NULL);
return false;
break;
121 usage(argv[0], optarg_);
126 if ((c == 1) || (c == -1)) {
128 usage(argv[0], NULL);
129 std::cerr <<
"ERROR: " << std::endl;
130 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
139 main(
int argc,
const char ** argv)
143 if (getOptions(argc, argv) ==
false) {
148 std::vector<double> xa(nbpt), ya(nbpt) ;
149 std::vector<double> xb(nbpt), yb(nbpt) ;
169 for(
unsigned int i=0 ; i < nbpt ; i++)
173 xa[i] = P[i].
get_x() ;
174 ya[i] = P[i].
get_y() ;
177 for(
unsigned int i=0 ; i < nbpt ; i++)
181 xb[i] = P[i].
get_x() ;
182 yb[i] = P[i].
get_y() ;
188 std::cout <<
"-------------------------------" << std::endl ;
189 std::cout <<
"Compare with built homography H = R + t/d n " << std::endl ;
192 std::cout <<
"aHb built from the displacement: \n" << aHb_built/aHb_built[2][2] << std::endl ;
194 aHb_built.computeDisplacement(aRb, aTb, n) ;
195 std::cout <<
"Rotation: aRb" <<std::endl ;
196 std::cout << aRb << std::endl ;
197 std::cout <<
"Translation: aTb" <<std::endl;
198 std::cout << (aTb).t() <<std::endl ;
199 std::cout <<
"Normal to the plane: n" <<std::endl;
200 std::cout << (n).t() <<std::endl ;
202 std::cout <<
"-------------------------------" <<std::endl ;
203 std::cout <<
"aMb "<<std::endl <<aMb << std::endl ;
204 std::cout <<
"-------------------------------" <<std::endl ;
209 std::cout <<
"aHb computed using the Malis paralax algorithm" << std::endl ;
211 std::cout << std::endl << aHb<< std::endl ;
213 std::cout <<
"-------------------------------" <<std::endl ;
214 std::cout <<
"extract R, T and n " << std::endl ;
216 std::cout <<
"Rotation: aRb" <<std::endl ;
217 std::cout << aRb << std::endl ;
218 std::cout <<
"Translation: aTb" <<std::endl;
219 std::cout << (aTb).t() <<std::endl ;
220 std::cout <<
"Normal to the plane: n" <<std::endl;
221 std::cout << (n).t() <<std::endl ;
223 std::cout <<
"-------------------------------" <<std::endl ;
224 std::cout <<
"test if ap = aHb bp" << std::endl ;
226 for(
unsigned int i=0 ; i < nbpt ; i++)
228 std::cout <<
"Point "<< i<< std::endl ;
232 std::cout <<
") = (" ;
239 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.