#include <visp3/vision/vpHomography.h>
#include <visp3/core/vpMeterPixelConversion.h>
int main()
{
double L = 0.1;
std::vector<vpPoint> oP;
oP.push_back(
vpPoint(2 * L, -L, 0));
oP.push_back(
vpPoint(L, 3 * L, 0));
oP.push_back(
vpPoint(-L, 4 * L, 0));
std::vector<vpPoint> aP(4), bP(4);
std::vector<double> xa(4), ya(4), xb(4), yb(4);
for (unsigned int i = 0; i < 4; i++) {
oP[i].project(aMo);
xa[i] = oP[i].get_x();
ya[i] = oP[i].get_y();
oP[i].project(bMo);
xb[i] = oP[i].get_x();
yb[i] = oP[i].get_y();
}
std::cout << "Estimated homography using DLT:\n" << aHb / aHb[2][2] << std::endl;
std::cout << "Estimated homography using HLM:\n" << aHb / aHb[2][2] << std::endl;
std::cout << "\nEstimated displacement:" << std::endl;
std::cout <<
" atb: " << atb.
t() << std::endl;
std::cout << " athetaub: ";
for (unsigned int i = 0; i < 3; i++)
std::cout << std::endl;
std::cout <<
" n: " << n.
t() << std::endl;
std::cout << "Ground truth: Point 3 in pixels in frame b: " << iPb << std::endl;
std::cout << "Ground truth: Point 3 in pixels in frame a: " << iPa << std::endl;
std::cout <<
"Estimation from homography: Point 3 in pixels in frame a: " <<
vpHomography::project(cam, aHb, iPb)
<< std::endl;
}