52 #include <visp3/core/vpDebug.h>
53 #include <visp3/core/vpMath.h>
54 #include <visp3/core/vpRotationMatrix.h>
55 #include <visp3/core/vpThetaUVector.h>
56 #include <visp3/vision/vpHomography.h>
59 #include <visp3/core/vpDebug.h>
60 #include <visp3/core/vpHomogeneousMatrix.h>
61 #include <visp3/core/vpMath.h>
62 #include <visp3/core/vpPoint.h>
63 #include <visp3/io/vpParseArgv.h>
65 #define GETOPTARGS "h"
69 void usage(
const char *name,
const char *badparam);
70 bool getOptions(
int argc,
const char **argv);
81 void usage(
const char *name,
const char *badparam)
84 Test the HLM (Malis) homography estimation algorithm with a planar object.\n\
96 fprintf(stderr,
"ERROR: \n");
97 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
110 bool getOptions(
int argc,
const char **argv)
118 usage(argv[0],
nullptr);
123 usage(argv[0], optarg_);
129 if ((c == 1) || (c == -1)) {
131 usage(argv[0],
nullptr);
132 std::cerr <<
"ERROR: " << std::endl;
133 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
140 int main(
int argc,
const char **argv)
142 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
145 if (getOptions(argc, argv) ==
false) {
150 std::vector<double> xa(nbpt), ya(nbpt);
151 std::vector<double> xb(nbpt), yb(nbpt);
168 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++) {
178 xb[i] = P[i].
get_x();
179 yb[i] = P[i].
get_y();
181 std::cout <<
"-------------------------------" << std::endl;
182 std::cout <<
"aMb " << std::endl << aMb << std::endl;
183 std::cout <<
"-------------------------------" << std::endl;
189 std::cout <<
"aHb computed using the Malis paralax algorithm: \n" << aHb << std::endl;
195 std::cout <<
"-------------------------------" << std::endl;
196 std::cout <<
"extract R, T and n " << std::endl;
198 std::cout <<
"Rotation: aRb" << std::endl;
199 std::cout << aRb << std::endl;
200 std::cout <<
"Translation: aTb" << std::endl;
201 std::cout << (aTb).t() << std::endl;
202 std::cout <<
"Normal to the plane: n" << std::endl;
203 std::cout << (n).t() << std::endl;
205 std::cout <<
"-------------------------------" << std::endl;
206 std::cout <<
"Compare with built homography H = R + t/d " << std::endl;
209 std::cout <<
"aHb built from the displacement " << std::endl;
210 std::cout << std::endl << aHb_built / aHb_built[2][2] << std::endl;
212 aHb_built.computeDisplacement(aRb, aTb, n);
213 std::cout <<
"Rotation: aRb" << std::endl;
214 std::cout << aRb << std::endl;
215 std::cout <<
"Translation: aTb" << std::endl;
216 std::cout << (aTb).t() << std::endl;
217 std::cout <<
"Normal to the plane: n" << std::endl;
218 std::cout << (n).t() << std::endl;
220 std::cout <<
"-------------------------------" << std::endl;
221 std::cout <<
"test if ap = aHb bp" << std::endl;
223 for (
unsigned int i = 0; i < nbpt; i++) {
224 std::cout <<
"Point " << i << std::endl;
228 std::cout <<
") = (";
233 std::cout <<
"-------------------------------" << std::endl;
234 std::cout <<
"test displacement" << std::endl;
236 std::list<vpRotationMatrix> laRb;
237 std::list<vpTranslationVector> laTb;
238 std::list<vpColVector> lnb;
242 std::list<vpRotationMatrix>::const_iterator it_laRb = laRb.begin();
243 std::list<vpTranslationVector>::const_iterator it_laTb = laTb.begin();
244 std::list<vpColVector>::const_iterator it_lnb = lnb.begin();
247 while (it_lnb != lnb.end()) {
248 std::cout <<
"Solution " << k++ << std::endl;
253 std::cout <<
"Rotation: aRb" << std::endl;
254 std::cout << aRb << std::endl;
255 std::cout <<
"Translation: aTb" << std::endl;
256 std::cout << (aTb).t() << std::endl;
257 std::cout <<
"Normal to the plane: n" << std::endl;
258 std::cout << (n).t() << std::endl;
267 std::cout <<
"Catch an exception: " << e << std::endl;
273 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of an homography and operations on homographies.
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)
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
static double rad(double deg)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
This class defines the container for a plane geometrical structure.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_w() const
Get the point w coordinate in the image plane.
double get_y() const
Get the point y coordinate in the image plane.
double get_x() const
Get the point x coordinate in the image plane.
void setWorldCoordinates(double oX, double oY, double oZ)
Implementation of a rotation matrix and operations on such kind of matrices.
Class that consider the case of a translation vector.