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"
70 void usage(
const char *name,
const char *badparam);
71 bool getOptions(
int argc,
const char **argv);
82 void usage(
const char *name,
const char *badparam)
85 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)
119 usage(argv[0],
nullptr);
124 usage(argv[0], optarg_);
130 if ((c == 1) || (c == -1)) {
132 usage(argv[0],
nullptr);
133 std::cerr <<
"ERROR: " << std::endl;
134 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
141 int main(
int argc,
const char **argv)
143 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
146 if (getOptions(argc, argv) ==
false) {
151 std::vector<double> xa(nbpt), ya(nbpt);
152 std::vector<double> xb(nbpt), yb(nbpt);
172 for (
unsigned int i = 0; i < nbpt; i++) {
175 xa[i] = P[i].
get_x();
176 ya[i] = P[i].
get_y();
179 for (
unsigned int i = 0; i < nbpt; i++) {
182 xb[i] = P[i].
get_x();
183 yb[i] = P[i].
get_y();
189 std::cout <<
"-------------------------------" << std::endl;
190 std::cout <<
"Compare with built homography H = R + t/d n " << std::endl;
193 std::cout <<
"aHb built from the displacement: \n" << aHb_built / aHb_built[2][2] << std::endl;
195 aHb_built.computeDisplacement(aRb, aTb, n);
196 std::cout <<
"Rotation: aRb" << std::endl;
197 std::cout << aRb << std::endl;
198 std::cout <<
"Translation: aTb" << std::endl;
199 std::cout << (aTb).t() << std::endl;
200 std::cout <<
"Normal to the plane: n" << std::endl;
201 std::cout << (n).t() << std::endl;
203 std::cout <<
"-------------------------------" << std::endl;
204 std::cout <<
"aMb " << std::endl << aMb << std::endl;
205 std::cout <<
"-------------------------------" << std::endl;
210 std::cout <<
"aHb computed using the Malis paralax algorithm" << std::endl;
212 std::cout << std::endl << aHb << std::endl;
214 std::cout <<
"-------------------------------" << std::endl;
215 std::cout <<
"extract R, T and n " << std::endl;
217 std::cout <<
"Rotation: aRb" << std::endl;
218 std::cout << aRb << std::endl;
219 std::cout <<
"Translation: aTb" << std::endl;
220 std::cout << (aTb).t() << std::endl;
221 std::cout <<
"Normal to the plane: n" << std::endl;
222 std::cout << (n).t() << std::endl;
224 std::cout <<
"-------------------------------" << std::endl;
225 std::cout <<
"test if ap = aHb bp" << std::endl;
227 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;
245 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.