Compute the pose of a 3D object using the Dementhon, Lagrange and Non-Linear approach.
#include <visp/vpPose.h>
#include <visp/vpPoint.h>
#include <visp/vpMath.h>
#include <visp/vpTranslationVector.h>
#include <visp/vpRxyzVector.h>
#include <visp/vpRotationMatrix.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpDebug.h>
#include <visp/vpParseArgv.h>
#include <stdlib.h>
#include <stdio.h>
#define GETOPTARGS "h"
#define L 0.035
void usage(const char *name, const char *badparam)
{
fprintf(stdout, "\n\
Compute the pose of a 3D object using the Dementhon, Lagrange and\n\
Non-Linear approach.\n\
\n\
SYNOPSIS\n\
%s [-h]\n", name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
-h\n\
Print the help.\n");
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv)
{
const char *optarg;
int c;
switch (c) {
case 'h': usage(argv[0], NULL); return false; break;
default:
usage(argv[0], optarg);
return false; break;
}
}
if ((c == 1) || (c == -1)) {
usage(argv[0], NULL);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg << std::endl << std::endl;
return false;
}
return true;
}
int
main(int argc, const char ** argv)
{
if (getOptions(argc, argv) == false) {
exit (-1);
}
int i ;
for(i=0 ; i < 4 ; i++)
P[i].project(cMo_ref) ;
for (i=0 ; i < 4 ; i++)
{
}
std::cout <<"-------------------------------------------------"<<std::endl ;
std::cout << cMo << std::endl ;
std::cout <<
"Lagrange residual term: " << pose.
computeResidual(cMo) <<std::endl ;
std::cout << "\nPose Lagrange "
<< "(residual: " << residual << ")\n "
<< "cdto[0] = " << cto[0] << ";\n "
<< "cdto[1] = " << cto[1] << ";\n "
<< "cdto[2] = " << cto[2] << ";\n "
<<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
<<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
<<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
<< std::endl;
std::cout <<"--------------------------------------------------"<<std::endl ;
std::cout << cMo << std::endl ;
std::cout <<
"Lowe residual term: " <<pose.
computeResidual(cMo) <<std::endl ;
std::cout << "\nPose Lowe "
<< "(residual: " << residual << ")\n "
<< "cdto[0] = " << cto[0] << ";\n "
<< "cdto[1] = " << cto[1] << ";\n "
<< "cdto[2] = " << cto[2] << ";\n "
<<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
<<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
<<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
<< std::endl;
std::cout <<"--------------------------------------------------"<<std::endl ;
std::cout << cMo << std::endl ;
<< std::endl ;
std::cout << "\nPose Dementhon "
<< "(residual: " << residual << ")\n "
<< "cdto[0] = " << cto[0] << ";\n "
<< "cdto[1] = " << cto[1] << ";\n "
<< "cdto[2] = " << cto[2] << ";\n "
<<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
<<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
<<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
<< std::endl;
std::cout <<"--------------------------------------------------"<<std::endl ;
std::cout <<
"Lowe residual term: " <<pose.
computeResidual(cMo) <<std::endl ;
std::cout << "\nPose lowe "
<< "(residual: " << residual << ")\n "
<< "cdto[0] = " << cto[0] << ";\n "
<< "cdto[1] = " << cto[1] << ";\n "
<< "cdto[2] = " << cto[2] << ";\n "
<<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
<<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
<<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
<< std::endl;
std::cout <<std::endl << std::endl ;
std::cout <<"--------------------------------------------------"<<std::endl ;
std::cout << "Virtual Visual servoing " << std::endl ;
std::cout <<"--------------------------------------------------"<<std::endl ;
std::cout << cMo << std::endl ;
<<std::endl ;
std::cout << "\nPose Lagrange "
<< "(residual: " << residual << ")\n "
<< "cdto[0] = " << cto[0] << ";\n "
<< "cdto[1] = " << cto[1] << ";\n "
<< "cdto[2] = " << cto[2] << ";\n "
<<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
<<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
<<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
<< std::endl;
std::cout <<"--------------------------------------------------"<<std::endl ;
std::cout << cMo << std::endl ;
std::cout <<
"vvs residual term: " <<pose.
computeResidual(cMo) <<std::endl ;
std::cout << "\nPose VVS "
<< "(residual: " << residual << ")\n "
<< "cdto[0] = " << cto[0] << ";\n "
<< "cdto[1] = " << cto[1] << ";\n "
<< "cdto[2] = " << cto[2] << ";\n "
<<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
<<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
<<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
<< std::endl;
std::cout <<"-------------------------------------------------"<<std::endl ;
std::cout << cMo << std::endl ;
<<std::endl ;
std::cout << "\nPose Dementhon "
<< "(residual: " << residual << ")\n "
<< "cdto[0] = " << cto[0] << ";\n "
<< "cdto[1] = " << cto[1] << ";\n "
<< "cdto[2] = " << cto[2] << ";\n "
<<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
<<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
<<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
<< std::endl;
std::cout <<"-------------------------------------------------"<<std::endl ;
std::cout <<
"vvs residual term: " <<pose.
computeResidual(cMo) <<std::endl ;
std::cout << "\nPose VVS "
<< "(residual: " << residual << ")\n "
<< "cdto[0] = " << cto[0] << ";\n "
<< "cdto[1] = " << cto[1] << ";\n "
<< "cdto[2] = " << cto[2] << ";\n "
<<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
<<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
<<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
<< std::endl;
return 0;
}