// ------------------------------------------------------ // 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(); }