#include <stdio.h>
#include <stdlib.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpThetaUVector.h>
#include <visp3/vision/vpHomography.h>
bool test(
const std::string &s,
const vpHomography &H,
const std::vector<double> &bench)
{
static unsigned int cpt = 0;
std::cout << "** Test " << ++cpt << std::endl;
std::cout << s <<
"(" << H.
getRows() <<
"," << H.
getCols() <<
") = \n[" << H <<
"]" << std::endl;
if (bench.size() != H.
size()) {
std::cout << "Test fails: bad size wrt bench" << std::endl;
return false;
}
for (
unsigned int i = 0; i < H.
size(); i++) {
if (std::fabs(H.
data[i] - bench[i]) > std::fabs(H.
data[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
return false;
}
}
return true;
}
int main()
{
try {
{
std::vector<double> bench(9, 0);
bench[0] = bench[4] = bench[8] = 1.;
int err = 1;
if (test("H", H, bench) == false)
return err;
if (test("H", H / H[2][2], bench) == false)
return err;
}
{
std::cout << "Initialization " << std::endl;
std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl;
std::cout << "M" << std::endl << M << std::endl;
std::cout << "H" << std::endl << H << std::endl;
std::cout << "R" << std::endl << R;
std::cout <<
"T" << std::endl << T.
t() << std::endl;
std::cout <<
"n" << std::endl << n.
t() << std::endl;
}
std::cout << "------------------------------------------------------" << std::endl;
{
std::cout << "Initialization " << std::endl;
std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl;
M[0][3] = 0.21;
M[1][3] = 0.31;
M[2][3] = 0.5;
std::cout << "M" << std::endl << M << std::endl;
std::cout << "H" << std::endl << H << std::endl;
std::cout << "R" << std::endl << R;
std::cout <<
"T" << std::endl << T.
t() << std::endl;
std::cout <<
"n" << std::endl << n.
t() << std::endl;
}
std::cout << "------------------------------------------------------" << std::endl;
{
M[0][3] = 0.21;
M[1][3] = -0.31;
M[2][3] = 0.5;
std::cout << "M" << std::endl << M << std::endl;
std::cout << "H" << std::endl << H << std::endl;
std::cout << "R" << std::endl << R;
std::cout <<
"T" << std::endl << T.
t() << std::endl;
std::cout <<
"n" << std::endl << n.
t() << std::endl;
std::cout << "H" << std::endl << H << std::endl;
}
std::cout << "All tests succeed" << std::endl;
return 0;
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}