// ------------------------------------------------------
// Generate both sets of points
// ------------------------------------------------------
CPose3DQuat generate_points( TPoints &pA, TPoints &pB )
{
const double Dx = 0.5;
const double Dy = 1.5;
const double Dz = 0.75;
const double yaw = DEG2RAD(10);
const double pitch = DEG2RAD(20);
const double roll = DEG2RAD(5);
pA.resize( 5 ); // A set of points at "A" reference system
pB.resize( 5 ); // A set of points at "B" reference system
pA[0].resize(3); pA[0][0] = 0.0; pA[0][1] = 0.5; pA[0][2] = 0.4;
pA[1].resize(3); pA[1][0] = 1.0; pA[1][1] = 1.5; pA[1][2] = -0.1;
pA[2].resize(3); pA[2][0] = 1.2; pA[2][1] = 1.1; pA[2][2] = 0.9;
pA[3].resize(3); pA[3][0] = 0.7; pA[3][1] = 0.3; pA[3][2] = 3.4;
pA[4].resize(3); pA[4][0] = 1.9; pA[4][1] = 2.5; pA[4][2] = -1.7;
CPose3DQuat qPose = CPose3DQuat(CPose3D( Dx, Dy, Dz, yaw, pitch, roll ));
for( unsigned int i = 0; i < 5; ++i )
{
pB[i].resize( 3 );
qPose.inverseComposePoint( pA[i][0], pA[i][1], pA[i][2], pB[i][0], pB[i][1], pB[i][2] );
}
return qPose;
} // end generate_points
/** Save as a config block:
*/
void TStereoCamera::saveToConfigFile(const std::string §ion, mrpt::utils::CConfigFileBase &cfg ) const
{
// [<SECTION>_LEFT]
// ...
// [<SECTION>_RIGHT]
// ...
// [<SECTION>_LEFT2RIGHT_POSE]
// pose_quaternion = [x y z qr qx qy qz]
leftCamera.saveToConfigFile(section+string("_LEFT"), cfg);
rightCamera.saveToConfigFile(section+string("_RIGHT"), cfg);
const CPose3DQuat q = CPose3DQuat(rightCameraPose); // Don't remove this line, it's a safe against future possible bugs if rightCameraPose changes!
cfg.write(section+string("_LEFT2RIGHT_POSE"), "pose_quaternion",q.asString() );
}
double poses_test_compose3DQuat2(int a1, int a2)
{
const long N = 100000;
CTicTac tictac;
CPose3DQuat a(CPose3D(1.0,2.0,3.0,DEG2RAD(10),DEG2RAD(50),DEG2RAD(-30)));
CPose3DQuat b(CPose3D(8.0,-5.0,-1.0,DEG2RAD(-40),DEG2RAD(10),DEG2RAD(-45)));
CPose3DQuat p;
for (long i=0;i<N;i++)
{
p.composeFrom(a,b);
}
double T = tictac.Tac()/N;
dummy_do_nothing_with_string( mrpt::format("%f",p.x()) );
return T;
}
/*---------------------------------------------------------------
HornMethod
---------------------------------------------------------------*/
double scanmatching::HornMethod(
const vector_double &inVector,
vector_double &outVector, // The output vector
bool forceScaleToUnity )
{
MRPT_START
vector_double input;
input.resize( inVector.size() );
input = inVector;
outVector.resize( 7 );
// Compute the centroids
TPoint3D cL(0,0,0), cR(0,0,0);
const size_t nMatches = input.size()/6;
ASSERT_EQUAL_(input.size()%6, 0)
for( unsigned int i = 0; i < nMatches; i++ )
{
cL.x += input[i*6+3];
cL.y += input[i*6+4];
cL.z += input[i*6+5];
cR.x += input[i*6+0];
cR.y += input[i*6+1];
cR.z += input[i*6+2];
}
ASSERT_ABOVE_(nMatches,0)
const double F = 1.0/nMatches;
cL *= F;
cR *= F;
CMatrixDouble33 S; // S.zeros(); // Zeroed by default
// Substract the centroid and compute the S matrix of cross products
for( unsigned int i = 0; i < nMatches; i++ )
{
input[i*6+3] -= cL.x;
input[i*6+4] -= cL.y;
input[i*6+5] -= cL.z;
input[i*6+0] -= cR.x;
input[i*6+1] -= cR.y;
input[i*6+2] -= cR.z;
S.get_unsafe(0,0) += input[i*6+3]*input[i*6+0];
S.get_unsafe(0,1) += input[i*6+3]*input[i*6+1];
S.get_unsafe(0,2) += input[i*6+3]*input[i*6+2];
S.get_unsafe(1,0) += input[i*6+4]*input[i*6+0];
S.get_unsafe(1,1) += input[i*6+4]*input[i*6+1];
S.get_unsafe(1,2) += input[i*6+4]*input[i*6+2];
S.get_unsafe(2,0) += input[i*6+5]*input[i*6+0];
S.get_unsafe(2,1) += input[i*6+5]*input[i*6+1];
S.get_unsafe(2,2) += input[i*6+5]*input[i*6+2];
}
// Construct the N matrix
CMatrixDouble44 N; // N.zeros(); // Zeroed by default
N.set_unsafe( 0,0,S.get_unsafe(0,0) + S.get_unsafe(1,1) + S.get_unsafe(2,2) );
N.set_unsafe( 0,1,S.get_unsafe(1,2) - S.get_unsafe(2,1) );
N.set_unsafe( 0,2,S.get_unsafe(2,0) - S.get_unsafe(0,2) );
N.set_unsafe( 0,3,S.get_unsafe(0,1) - S.get_unsafe(1,0) );
N.set_unsafe( 1,0,N.get_unsafe(0,1) );
N.set_unsafe( 1,1,S.get_unsafe(0,0) - S.get_unsafe(1,1) - S.get_unsafe(2,2) );
N.set_unsafe( 1,2,S.get_unsafe(0,1) + S.get_unsafe(1,0) );
N.set_unsafe( 1,3,S.get_unsafe(2,0) + S.get_unsafe(0,2) );
N.set_unsafe( 2,0,N.get_unsafe(0,2) );
N.set_unsafe( 2,1,N.get_unsafe(1,2) );
N.set_unsafe( 2,2,-S.get_unsafe(0,0) + S.get_unsafe(1,1) - S.get_unsafe(2,2) );
N.set_unsafe( 2,3,S.get_unsafe(1,2) + S.get_unsafe(2,1) );
N.set_unsafe( 3,0,N.get_unsafe(0,3) );
N.set_unsafe( 3,1,N.get_unsafe(1,3) );
N.set_unsafe( 3,2,N.get_unsafe(2,3) );
N.set_unsafe( 3,3,-S.get_unsafe(0,0) - S.get_unsafe(1,1) + S.get_unsafe(2,2) );
// q is the quaternion correspondent to the greatest eigenvector of the N matrix (last column in Z)
CMatrixDouble44 Z, D;
vector_double v;
N.eigenVectors( Z, D );
Z.extractCol( Z.getColCount()-1, v );
ASSERTDEB_( fabs( sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3] ) - 1.0 ) < 0.1 );
// Make q_r > 0
if( v[0] < 0 ){ v[0] *= -1; v[1] *= -1; v[2] *= -1; v[3] *= -1; }
CPose3DQuat q; // Create a pose rotation with the quaternion
for(unsigned int i = 0; i < 4; i++ ) // insert the quaternion part
outVector[i+3] = q[i+3] = v[i];
// Compute scale
double num = 0.0;
double den = 0.0;
for( unsigned int i = 0; i < nMatches; i++ )
{
num += square( input[i*6+0] ) + square( input[i*6+1] ) + square( input[i*6+2] );
den += square( input[i*6+3] ) + square( input[i*6+4] ) + square( input[i*6+5] );
} // end-for
// The scale:
double s = std::sqrt( num/den );
// Enforce scale to be 1
if( forceScaleToUnity )
s = 1.0;
TPoint3D pp;
q.composePoint( cL.x, cL.y, cL.z, pp.x, pp.y, pp.z );
pp*=s;
outVector[0] = cR.x - pp.x; // X
outVector[1] = cR.y - pp.y; // Y
outVector[2] = cR.z - pp.z; // Z
return s; // return scale
MRPT_END
}
void mrpt::math::slerp(
const CPose3DQuat & q0,
const CPose3DQuat & q1,
const double t,
CPose3DQuat & q)
{
// The quaternion part (this will raise exception on t not in [0,1])
mrpt::math::slerp(q0.quat(), q1.quat(),t, q.quat());
// XYZ:
q.x( (1-t)*q0.x()+t*q1.x() );
q.y( (1-t)*q0.y()+t*q1.y() );
q.z( (1-t)*q0.z()+t*q1.z() );
}
/*---------------------------------------------------------------
jacobiansPoseComposition
---------------------------------------------------------------*/
void CPose3DQuatPDF::jacobiansPoseComposition(
const CPose3DQuat &x,
const CPose3DQuat &u,
CMatrixDouble77 &df_dx,
CMatrixDouble77 &df_du,
CPose3DQuat *out_x_oplus_u
)
{
// For the derivation of the formulas, see the tech. report cited in the header file.
const double qr = x.quat().r();
const double qx = x.quat().x(); const double qx2 = square(qx);
const double qy = x.quat().y(); const double qy2 = square(qy);
const double qz = x.quat().z(); const double qz2 = square(qz);
const double ax = u.x();
const double ay = u.y();
const double az = u.z();
const double q2r = u.quat().r();
const double q2x = u.quat().x();
const double q2y = u.quat().y();
const double q2z = u.quat().z();
CPose3DQuat x_plus_u = x + u; // needed for the normalization Jacobian:
CMatrixDouble44 norm_jacob(UNINITIALIZED_MATRIX);
x_plus_u.quat().normalizationJacobian(norm_jacob);
CMatrixDouble44 norm_jacob_x(UNINITIALIZED_MATRIX);
x.quat().normalizationJacobian(norm_jacob_x);
// df_dx ===================================================
df_dx.zeros();
// first part 3x7: df_{qr} / dp
df_dx.set_unsafe(0,0, 1);
df_dx.set_unsafe(1,1, 1);
df_dx.set_unsafe(2,2, 1);
MRPT_ALIGN16 const double vals2[3*4] = {
2*(-qz*ay +qy*az ),
2*(qy*ay + qz*az ),
2*(-2*qy*ax + qx*ay +qr*az ),
2*(-2*qz*ax - qr*ay +qx*az ),
2*(qz*ax - qx*az ),
2*(qy*ax - 2*qx*ay -qr*az ),
2*(qx*ax +qz*az ),
2*(qr*ax - 2*qz*ay +qy*az ),
2*(-qy*ax + qx*ay ),
2*( qz*ax + qr*ay - 2*qx*az ),
2*(-qr*ax + qz*ay - 2*qy*az ),
2*( qx*ax + qy*ay )
};
// df_dx(0:3,3:7) = vals2 * NORM_JACOB
df_dx.block(0,3, 3,4).noalias() = (CMatrixFixedNumeric<double,3,4>(vals2) * norm_jacob_x).eval();
// second part:
{
MRPT_ALIGN16 const double aux44_data[4*4] = {
q2r,-q2x,-q2y,-q2z,
q2x, q2r, q2z,-q2y,
q2y,-q2z, q2r, q2x,
q2z, q2y,-q2x, q2r };
df_dx.block(3,3, 4,4).noalias() = (norm_jacob * CMatrixFixedNumeric<double,4,4>(aux44_data)).eval();
}
// df_du ===================================================
df_du.zeros();
// first part 3x3: df_{qr} / da
df_du.set_unsafe(0,0, 1-2*(qy2+qz2) );
df_du.set_unsafe(0,1, 2*(qx*qy - qr*qz ) );
df_du.set_unsafe(0,2, 2*(qr*qy + qx*qz ) );
df_du.set_unsafe(1,0, 2*(qr*qz + qx*qy ) );
df_du.set_unsafe(1,1, 1 - 2*( qx2+qz2) );
df_du.set_unsafe(1,2, 2*(qy*qz - qr*qx ) );
df_du.set_unsafe(2,0, 2*(qx*qz - qr*qy ) );
df_du.set_unsafe(2,1, 2*(qr*qx + qy*qz ) );
df_du.set_unsafe(2,2, 1-2*(qx2+qy2) );
// Second part:
{
MRPT_ALIGN16 const double aux44_data[4*4] = {
qr,-qx,-qy,-qz,
qx, qr,-qz, qy,
qy, qz, qr,-qx,
qz,-qy, qx, qr };
df_du.block(3,3, 4,4).noalias() = (norm_jacob * CMatrixFixedNumeric<double,4,4>(aux44_data)).eval();
}
if (out_x_oplus_u)
*out_x_oplus_u = x_plus_u;
}
/** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with all its arguments multiplied by "-1") */
CPose3DQuat mrpt::poses::operator -(const CPose3DQuat &p)
{
CPose3DQuat ret = p;
ret.inverse();
return ret;
}
TPoint3D mrpt::poses::operator-(const TPoint3D& G, const CPose3DQuat& p)
{
mrpt::math::TPoint3D L;
p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
return L;
}
bool mrpt::poses::operator==(const CPose3DQuat& p1, const CPose3DQuat& p2)
{
return p1.quat() == p2.quat() && p1.x() == p2.x() && p1.y() == p2.y() &&
p1.z() == p2.z();
}
