55#include <visp3/core/vpDebug.h>
56#include <visp3/core/vpMath.h>
57#include <visp3/core/vpRotationMatrix.h>
58#include <visp3/core/vpThetaUVector.h>
59#include <visp3/vision/vpHomography.h>
62#include <visp3/core/vpDebug.h>
63#include <visp3/core/vpHomogeneousMatrix.h>
64#include <visp3/core/vpMath.h>
65#include <visp3/core/vpPoint.h>
66#include <visp3/io/vpParseArgv.h>
72void usage(
const char *name,
const char *badparam);
73bool getOptions(
int argc,
const char **argv);
84void usage(
const char *name,
const char *badparam)
87Test the HLM (Malis) homography estimation algorithm with a planar object.\n\
98 fprintf(stderr,
"ERROR: \n");
99 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
112bool getOptions(
int argc,
const char **argv)
120 usage(argv[0], NULL);
125 usage(argv[0], optarg_);
131 if ((c == 1) || (c == -1)) {
133 usage(argv[0], NULL);
134 std::cerr <<
"ERROR: " << std::endl;
135 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
142int main(
int argc,
const char **argv)
144#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
147 if (getOptions(argc, argv) ==
false) {
152 std::vector<double> xa(nbpt), ya(nbpt);
153 std::vector<double> xb(nbpt), yb(nbpt);
170 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++) {
180 xb[i] = P[i].
get_x();
181 yb[i] = P[i].
get_y();
183 std::cout <<
"-------------------------------" << std::endl;
184 std::cout <<
"aMb " << std::endl << aMb << std::endl;
185 std::cout <<
"-------------------------------" << std::endl;
191 std::cout <<
"aHb computed using the Malis paralax algorithm: \n" << aHb << std::endl;
197 std::cout <<
"-------------------------------" << std::endl;
198 std::cout <<
"extract R, T and n " << std::endl;
200 std::cout <<
"Rotation: aRb" << std::endl;
201 std::cout << aRb << std::endl;
202 std::cout <<
"Translation: aTb" << std::endl;
203 std::cout << (aTb).t() << std::endl;
204 std::cout <<
"Normal to the plane: n" << std::endl;
205 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++) {
226 std::cout <<
"Point " << i << std::endl;
230 std::cout <<
") = (";
235 std::cout <<
"-------------------------------" << std::endl;
236 std::cout <<
"test displacement" << std::endl;
238 std::list<vpRotationMatrix> laRb;
239 std::list<vpTranslationVector> laTb;
240 std::list<vpColVector> lnb;
244 std::list<vpRotationMatrix>::const_iterator it_laRb = laRb.begin();
245 std::list<vpTranslationVector>::const_iterator it_laTb = laTb.begin();
246 std::list<vpColVector>::const_iterator it_lnb = lnb.begin();
249 while (it_lnb != lnb.end()) {
250 std::cout <<
"Solution " << k++ << std::endl;
255 std::cout <<
"Rotation: aRb" << std::endl;
256 std::cout << aRb << std::endl;
257 std::cout <<
"Translation: aTb" << std::endl;
258 std::cout << (aTb).t() << std::endl;
259 std::cout <<
"Normal to the plane: n" << std::endl;
260 std::cout << (n).t() << std::endl;
268 std::cout <<
"Catch an exception: " << e << std::endl;
274 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 emited 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.