/*!
\brief Compute the pose of a planar object using Lagrange approach.
\param cMo : Estimated pose. No initialisation is requested to estimate cMo.
\param coplanar_plane_type : Type of coplanar plane:
1: if plane x=cst
2: if plane y=cst
3: if plane z=cst
0: any other plane
*/
void
vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type)
{
#if (DEBUG_LEVEL1)
std::cout << "begin vpPose::PoseLagrange(...) " << std::endl ;
#endif
try
{
double s;
unsigned int i;
unsigned int k=0;
unsigned int nl=npt*2;
vpMatrix a(nl,3) ;
vpMatrix b(nl,6);
vpPoint P ;
i=0 ;
if (coplanar_plane_type == 1) { // plane ax=d
for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
{
P = *it ;
a[k][0] = -P.get_oY();
a[k][1] = 0.0;
a[k][2] = P.get_oY()*P.get_x();
a[k+1][0] = 0.0;
a[k+1][1] = -P.get_oY();
a[k+1][2] = P.get_oY()*P.get_y();
b[k][0] = -P.get_oZ();
b[k][1] = 0.0;
b[k][2] = P.get_oZ()*P.get_x();
b[k][3] = -1.0;
b[k][4] = 0.0;
b[k][5] = P.get_x();
b[k+1][0] = 0.0;
b[k+1][1] = -P.get_oZ();
b[k+1][2] = P.get_oZ()*P.get_y();
b[k+1][3] = 0.0;
b[k+1][4] = -1.0;
b[k+1][5] = P.get_y();
k += 2;
}
}
else if (coplanar_plane_type == 2) { // plane by=d
for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
{
P = *it ;
a[k][0] = -P.get_oX();
a[k][1] = 0.0;
a[k][2] = P.get_oX()*P.get_x();
a[k+1][0] = 0.0;
a[k+1][1] = -P.get_oX();
a[k+1][2] = P.get_oX()*P.get_y();
b[k][0] = -P.get_oZ();
b[k][1] = 0.0;
b[k][2] = P.get_oZ()*P.get_x();
b[k][3] = -1.0;
b[k][4] = 0.0;
b[k][5] = P.get_x();
b[k+1][0] = 0.0;
b[k+1][1] = -P.get_oZ();
b[k+1][2] = P.get_oZ()*P.get_y();
b[k+1][3] = 0.0;
b[k+1][4] = -1.0;
b[k+1][5] = P.get_y();
k += 2;
}
}
else { // plane cz=d or any other
for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
{
P = *it ;
a[k][0] = -P.get_oX();
a[k][1] = 0.0;
a[k][2] = P.get_oX()*P.get_x();
a[k+1][0] = 0.0;
a[k+1][1] = -P.get_oX();
a[k+1][2] = P.get_oX()*P.get_y();
b[k][0] = -P.get_oY();
b[k][1] = 0.0;
b[k][2] = P.get_oY()*P.get_x();
b[k][3] = -1.0;
b[k][4] = 0.0;
b[k][5] = P.get_x();
b[k+1][0] = 0.0;
b[k+1][1] = -P.get_oY();
b[k+1][2] = P.get_oY()*P.get_y();
b[k+1][3] = 0.0;
b[k+1][4] = -1.0;
b[k+1][5] = P.get_y();
k += 2;
}
}
vpColVector X1(3) ;
vpColVector X2(6) ;
#if (DEBUG_LEVEL2)
{
std::cout <<"a " << a << std::endl ;
std::cout <<"b " << b << std::endl ;
}
#endif
lagrange(a,b,X1,X2);
#if (DEBUG_LEVEL2)
{
std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
}
#endif
if (X2[5] < 0.0)
{ /* car Zo > 0 */
for (i=0;i<3;i++) X1[i] = -X1[i];
for (i=0;i<6;i++) X2[i] = -X2[i];
}
s = 0.0;
for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
s = 0.0;
for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
if (s<1e-10)
{
std::cout << "Points that produce an error: " << std::endl;
for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
{
std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
<< (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
}
vpERROR_TRACE( "division par zero ") ;
throw(vpException(vpException::divideByZeroError,
"division by zero ")) ;
}
s = 1.0/sqrt(s);
for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
calculTranslation (a, b, nl, 3, 3, X1, X2) ;
// if (err != OK)
{
// std::cout << "in (vpCalculPose_plan.cc)CalculTranslation returns " ;
// PrintError(err) ;
// return err ;
}
if (coplanar_plane_type == 1) { // plane ax=d
cMo[0][0] = (X1[1]*X2[2])-(X1[2]*X2[1]);
cMo[1][0] = (X1[2]*X2[0])-(X1[0]*X2[2]);
cMo[2][0] = (X1[0]*X2[1])-(X1[1]*X2[0]);
for (i=0;i<3;i++)
{ /* calcul de la matrice de passage */
cMo[i][1] = X1[i];
cMo[i][2] = X2[i];
cMo[i][3] = X2[i+3];
}
}
else if (coplanar_plane_type == 2) { // plane by=d
cMo[0][1] = (X1[1]*X2[2])-(X1[2]*X2[1]);
cMo[1][1] = (X1[2]*X2[0])-(X1[0]*X2[2]);
cMo[2][1] = (X1[0]*X2[1])-(X1[1]*X2[0]);
for (i=0;i<3;i++)
{ /* calcul de la matrice de passage */
cMo[i][0] = X1[i];
cMo[i][2] = X2[i];
cMo[i][3] = X2[i+3];
}
}
else { // plane cz=d or any other
cMo[0][2] = (X1[1]*X2[2])-(X1[2]*X2[1]);
cMo[1][2] = (X1[2]*X2[0])-(X1[0]*X2[2]);
cMo[2][2] = (X1[0]*X2[1])-(X1[1]*X2[0]);
for (i=0;i<3;i++)
{ /* calcul de la matrice de passage */
cMo[i][0] = X1[i];
cMo[i][1] = X2[i];
cMo[i][3] = X2[i+3];
}
}
}
catch(...)
{
vpERROR_TRACE(" ") ;
throw ;
}
#if (DEBUG_LEVEL1)
std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
#endif
// return(OK);
}
void
vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
{
if (DEBUG_LEVEL1)
std::cout << "begin CPose::PoseLagrange(...) " << std::endl ;
try{
double s;
int i;
int k=0;
int nl=npt*2;
vpMatrix a(nl,3) ;
vpMatrix b(nl,9);
b =0 ;
vpPoint P ;
listP.front() ;
i=0 ;
while (!listP.outside())
{
P= listP.value() ;
a[k][0] = -P.get_oX();
a[k][1] = 0.0;
a[k][2] = P.get_oX()*P.get_x();
a[k+1][0] = 0.0;
a[k+1][1] = -P.get_oX();
a[k+1][2] = P.get_oX()*P.get_y();
b[k][0] = -P.get_oY();
b[k][1] = 0.0;
b[k][2] = P.get_oY()*P.get_x();
b[k][3] = -P.get_oZ();
b[k][4] = 0.0;
b[k][5] = P.get_oZ()*P.get_x();
b[k][6] = -1.0;
b[k][7] = 0.0;
b[k][8] = P.get_x();
b[k+1][0] = 0.0;
b[k+1][1] = -P.get_oY();
b[k+1][2] = P.get_oY()*P.get_y();
b[k+1][3] = 0.0;
b[k+1][4] = -P.get_oZ();
b[k+1][5] = P.get_oZ()*P.get_y();
b[k+1][6] = 0.0;
b[k+1][7] = -1.0;
b[k+1][8] = P.get_y();
k += 2;
listP.next() ;
}
vpColVector X1(3) ;
vpColVector X2(9) ;
if (DEBUG_LEVEL2)
{
std::cout <<"a " << a << std::endl ;
std::cout <<"b " << b << std::endl ;
}
lagrange(a,b,X1,X2);
// if (err != OK)
{
// std::cout << "in (CLagrange.cc)Lagrange returns " ;
// PrintError(err) ;
// return err ;
}
if (DEBUG_LEVEL2)
{
std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
}
if (X2[8] < 0.0)
{ /* car Zo > 0 */
X1 *= -1 ;
X2 *= -1 ;
}
s = 0.0;
for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
s = 0.0;
for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
if (s<1e-10)
{
vpERROR_TRACE(" division par zero " ) ;
throw(vpException(vpException::divideByZeroError,
"division by zero ")) ;
}
s = 1.0/sqrt(s);
for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
X2[3] = (X1[1]*X2[2])-(X1[2]*X2[1]);
X2[4] = (X1[2]*X2[0])-(X1[0]*X2[2]);
X2[5] = (X1[0]*X2[1])-(X1[1]*X2[0]);
calculTranslation (a, b, nl, 3, 6, X1, X2) ;
for (i=0 ; i<3 ; i++)
{
cMo[i][0] = X1[i];
cMo[i][1] = X2[i];
cMo[i][2] = X2[i+3];
cMo[i][3] = X2[i+6];
}
}
catch(...)
{
vpERROR_TRACE(" ") ;
throw ;
}
if (DEBUG_LEVEL1)
std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
}
void
vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
{
#if (DEBUG_LEVEL1)
std::cout << "begin CPose::PoseLagrange(...) " << std::endl ;
#endif
try{
double s;
unsigned int i;
unsigned int k=0;
unsigned int nl=npt*2;
vpMatrix a(nl,3) ;
vpMatrix b(nl,9);
b =0 ;
vpPoint P ;
i=0 ;
for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
{
P = *it;
a[k][0] = -P.get_oX();
a[k][1] = 0.0;
a[k][2] = P.get_oX()*P.get_x();
a[k+1][0] = 0.0;
a[k+1][1] = -P.get_oX();
a[k+1][2] = P.get_oX()*P.get_y();
b[k][0] = -P.get_oY();
b[k][1] = 0.0;
b[k][2] = P.get_oY()*P.get_x();
b[k][3] = -P.get_oZ();
b[k][4] = 0.0;
b[k][5] = P.get_oZ()*P.get_x();
b[k][6] = -1.0;
b[k][7] = 0.0;
b[k][8] = P.get_x();
b[k+1][0] = 0.0;
b[k+1][1] = -P.get_oY();
b[k+1][2] = P.get_oY()*P.get_y();
b[k+1][3] = 0.0;
b[k+1][4] = -P.get_oZ();
b[k+1][5] = P.get_oZ()*P.get_y();
b[k+1][6] = 0.0;
b[k+1][7] = -1.0;
b[k+1][8] = P.get_y();
k += 2;
}
vpColVector X1(3) ;
vpColVector X2(9) ;
#if (DEBUG_LEVEL2)
{
std::cout <<"a " << a << std::endl ;
std::cout <<"b " << b << std::endl ;
}
#endif
lagrange(a,b,X1,X2);
// if (err != OK)
{
// std::cout << "in (CLagrange.cc)Lagrange returns " ;
// PrintError(err) ;
// return err ;
}
#if (DEBUG_LEVEL2)
{
std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
}
#endif
if (X2[8] < 0.0)
{ /* car Zo > 0 */
X1 *= -1 ;
X2 *= -1 ;
}
s = 0.0;
for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
//s = 0.0;
//for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
s = X2[0]*X2[0] + X2[1]*X2[1] + X2[2]*X2[2]; // To avoid a Coverity copy/past error
if (s<1e-10)
{
// std::cout << "Points that produce an error: " << std::endl;
// for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
// {
// std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
// << (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
// }
//vpERROR_TRACE(" division par zero " ) ;
throw(vpException(vpException::divideByZeroError,
"Division by zero in Lagrange pose computation (non planar plane case)")) ;
}
s = 1.0/sqrt(s);
for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
X2[3] = (X1[1]*X2[2])-(X1[2]*X2[1]);
X2[4] = (X1[2]*X2[0])-(X1[0]*X2[2]);
X2[5] = (X1[0]*X2[1])-(X1[1]*X2[0]);
calculTranslation (a, b, nl, 3, 6, X1, X2) ;
for (i=0 ; i<3 ; i++)
{
cMo[i][0] = X1[i];
cMo[i][1] = X2[i];
cMo[i][2] = X2[i+3];
cMo[i][3] = X2[i+6];
}
}
catch(vpException &e)
{
throw e;
}
#if (DEBUG_LEVEL1)
std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
#endif
}