// ------------------------------------------------------------ // Set the sensor pose // ------------------------------------------------------------ void exec_setPoseByIdx( mrpt::slam::CActionCollection *acts, mrpt::slam::CSensoryFrame *SF, int &changesCount ) { if (SF) if (SF->size()>idxToProcess) { CObservationPtr obs = SF->getObservationByIndex(idxToProcess); if (changeOnlyXYZ) { CPose3D tmpPose; obs->getSensorPose(tmpPose); tmpPose.setFromValues( sensorPoseToSet.x, sensorPoseToSet.y, sensorPoseToSet.z, tmpPose.yaw(), tmpPose.pitch(), tmpPose.roll() ); obs->setSensorPose( tmpPose ); } else { obs->setSensorPose( sensorPoseToSet ); } changesCount++; } }
void test_ExpLnEqual(double x1,double y1,double z1, double yaw1,double pitch1,double roll1) { const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1); const CPose3D p2 = CPose3D::exp( p1.ln() ); EXPECT_NEAR((p1.getAsVectorVal()-p2.getAsVectorVal()).array().abs().sum(),0, 1e-5 ) << "p1: " << p1 <<endl; }
/** Constructor from a CPose3D */ CPose3DQuat::CPose3DQuat(const CPose3D &p) { x() = p.x(); y() = p.y(); z() = p.z(); p.getAsQuaternion(m_quat); }
TEST_F(QuaternionTests, crossProduct) { CQuaternionDouble q1, q2, q3; // q1 = CQuaternionDouble(1,2,3,4); q1.normalize(); CPose3D p1(0, 0, 0, DEG2RAD(10), DEG2RAD(30), DEG2RAD(-20)); p1.getAsQuaternion(q1); CPose3D p2(0, 0, 0, DEG2RAD(30), DEG2RAD(-20), DEG2RAD(10)); p2.getAsQuaternion(q2); // q3 = q1 x q2 q3.crossProduct(q1, q2); const CPose3D p3 = p1 + p2; EXPECT_NEAR( 0, std::abs( (p3.getAsVectorVal() - CPose3D(q3, 0, 0, 0).getAsVectorVal()) .sum()), 1e-6) << "q1 = " << q1 << endl << "q1 as CPose3D = " << CPose3D(q1, 0, 0, 0) << endl << "q2 = " << q2 << endl << "q2 as CPose3D = " << CPose3D(q2, 0, 0, 0) << endl << "q3 = q1 * q2 = " << q3 << endl << "q3 as CPose3D = " << CPose3D(q3, 0, 0, 0) << endl << "Should be equal to p3 = p1 (+) p2 = " << p3 << endl; }
void ICPslamWrapper::updateSensorPose(std::string _frame_id) { CPose3D pose; tf::StampedTransform transform; try { listenerTF_.lookupTransform(base_frame_id, _frame_id, ros::Time(0), transform); tf::Vector3 translation = transform.getOrigin(); tf::Quaternion quat = transform.getRotation(); pose.x() = translation.x(); pose.y() = translation.y(); pose.z() = translation.z(); tf::Matrix3x3 Rsrc(quat); CMatrixDouble33 Rdes; for (int c = 0; c < 3; c++) for (int r = 0; r < 3; r++) Rdes(r, c) = Rsrc.getRow(r)[c]; pose.setRotationMatrix(Rdes); laser_poses_[_frame_id] = pose; } catch (tf::TransformException ex) { ROS_ERROR("%s", ex.what()); ros::Duration(1.0).sleep(); } }
bool CPose3DInterpolator::getPreviousPoseWithMinDistance( const mrpt::system::TTimeStamp &t, double distance, CPose3D &out_pose ) { if( m_path.size() == 0 || distance <=0 ) return false; CPose3D myPose; // Search for the desired timestamp iterator it = m_path.find(t); if( it != m_path.end() && it != m_path.begin() ) myPose = it->second; else return false; double d = 0.0; do { --it; d = myPose.distance2DTo( it->second.x(), it->second.y()); } while( d < distance && it != m_path.begin() ); if( d >= distance ) { out_pose = it->second; return true; } else return false; } // end getPreviousPose
CPose3D monoslamMRPT3DScene::getRobotState() { // Put the camera in the right place in the image display scene->get_motion_model()->func_xp(scene->get_xv()); ThreeD_Motion_Model *threed_motion_model = (ThreeD_Motion_Model *) scene->get_motion_model(); VW::Vector3D r = threed_motion_model->get_rRES(); VW::Quaternion qWR = threed_motion_model->get_qRES(); // q is qWR between world frame and Scene robot frame // What we need to plot though uses GL object frame O // Know qRO: pi rotation about y axis // qWO = qWR * qRO VW::Quaternion qRO(0.0, 1.0, 0.0, 0.0); VW::Quaternion qWO = qWR * qRO; CPose3D position; double yaw,pitch,roll; qWO.GetZYXEuler(yaw,pitch,roll); position.setFromValues(r._x,r._y,r._z,yaw,pitch,roll); return position; // if (display_trajectory_button->GetState()) { // trajectory_store.push_back(r.GetVNL3()); // if (trajectory_store.size() > 800) { // trajectory_store.erase(trajectory_store.begin()); // } // } // // // // image_threedtool->SetCameraPositionOrientation(r, qWO); }
// ------------------------------------------------------ // Benchmark Point Maps // ------------------------------------------------------ double pointmap_test_0(int a1, int a2) { // test 0: insert scan // ---------------------------------------- // prepare the laser scan: CObservation2DRangeScan scan1; scan1.aperture = M_PIf; scan1.rightToLeft = true; scan1.validRange.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); scan1.scan.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) ); memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) ); CSimplePointsMap pt_map; pt_map.insertionOptions.minDistBetweenLaserPoints = 0.03; CPose3D pose; CTicTac tictac; for (int n=0;n<a2;n++) { pt_map.clear(); for (long i=0;i<a1;i++) { pose.setFromValues( pose.x()+0.04, pose.y()+0.08,0, pose.yaw()+0.02); pt_map.insertObservation(&scan1, &pose); } } return tictac.Tac()/a2; }
void exec_setPoseByLabel( mrpt::slam::CActionCollection *acts, mrpt::slam::CSensoryFrame *SF, int &changesCount ) { if (SF) { for (CSensoryFrame::iterator it= SF->begin();it!=SF->end();++it) { CObservationPtr obs = *it; if ( obs->sensorLabel == labelToProcess) { if (changeOnlyXYZ) { CPose3D tmpPose; obs->getSensorPose(tmpPose); tmpPose.setFromValues( sensorPoseToSet.x, sensorPoseToSet.y, sensorPoseToSet.z, tmpPose.yaw(), tmpPose.pitch(), tmpPose.roll() ); obs->setSensorPose( tmpPose ); } else { obs->setSensorPose( sensorPoseToSet ); } changesCount++; } } // end for each obs. } }
static void func_jacob_Aexpe_D( const CArrayDouble<6> &eps, const TParams_func_jacob_Aexpe_D ¶ms, CArrayDouble<12> &Y) { CPose3D incr; CPose3D::exp(eps,incr); const CPose3D res = params.A + incr + params.D; res.getAs12Vector(Y); }
// tech report: // static void func_jacob_expe_D( const CArrayDouble<6> &eps, const CPose3D &D, CArrayDouble<12> &Y) { CPose3D incr; CPose3D::exp(eps,incr); const CPose3D expe_D = incr + D; expe_D.getAs12Vector(Y); }
CPose3DRotVec::CPose3DRotVec(const CPose3D &m) { m_coords[0] = m.x(); m_coords[1] = m.y(); m_coords[2] = m.z(); CMatrixDouble44 R; m.getHomogeneousMatrix( R ); m_rotvec = rotVecFromRotMat( R ); }
static void func_jacob_expe_e( const CArrayDouble<6> &x, const double &dummy, CArrayDouble<12> &Y) { MRPT_UNUSED_PARAM(dummy); const CPose3D p = CPose3D::exp(x); //const CMatrixDouble44 R = p.getHomogeneousMatrixVal(); p.getAs12Vector(Y); }
static void func_jacob_LnT_T( const CArrayDouble<12> &x, const double &dummy, CArrayDouble<6> &Y) { MRPT_UNUSED_PARAM(dummy); CPose3D p; p.setFrom12Vector(x); Y = p.ln(); }
void CEllipsoid_setFromPosePDF(CEllipsoid& self, CPose3DPDF& posePDF) { CPose3D meanPose; CMatrixDouble66 COV; posePDF.getCovarianceAndMean(COV, meanPose); CMatrixDouble33 COV3 = COV.block(0,0,3,3); self.setLocation(meanPose.x(), meanPose.y(), meanPose.z() + 0.001 ); self.setCovMatrix(COV3, COV3(2,2)==0 ? 2:3 ); }
double pointmap_test_1(int a1, int a2) { // test 1: insert scan // ---------------------------------------- // prepare the laser scan: CObservation2DRangeScan scan1; scan1.aperture = M_PIf; scan1.rightToLeft = true; scan1.validRange.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); scan1.scan.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) ); memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) ); CSimplePointsMap pt_map; pt_map.insertionOptions.minDistBetweenLaserPoints = 0.03; CPose3D pose; CTicTac tictac; for (long i=0;i<a1;i++) { pose.setFromValues( pose.x()+0.04, pose.y()+0.08,0, pose.yaw()+0.02); pt_map.insertObservation(&scan1, &pose); } const unsigned N_REPS = 25; if (a2==0) return tictac.Tac(); else if (a2==1) { // 2d kd-tree float x,y, dist2; tictac.Tic(); for (unsigned k=N_REPS;k!=0;--k) /*size_t idx = */ pt_map.kdTreeClosestPoint2D(5.0, 6.0, x,y, dist2); return tictac.Tac()/N_REPS; } else { // 3d kd-tree float x,y,z, dist2; tictac.Tic(); for (unsigned k=N_REPS;k!=0;--k) /*size_t idx =*/ pt_map.kdTreeClosestPoint3D(5.0, 6.0, 1.0, x,y,z, dist2); return tictac.Tac()/N_REPS; } }
void test_to_from_2d(double x,double y, double phi) { const CPose2D p2d = CPose2D(x,y,phi); const CPose3D p3d = CPose3D(p2d); const CPose2D p2d_bis = CPose2D(p3d); EXPECT_FLOAT_EQ( p2d.x(), p2d_bis.x() ) << "p2d: " << p2d << endl; EXPECT_FLOAT_EQ( p2d.y(), p2d_bis.y() ) << "p2d: " << p2d << endl; EXPECT_FLOAT_EQ( p2d.phi(), p2d_bis.phi() ) << "p2d: " << p2d << endl; EXPECT_FLOAT_EQ( p2d.phi(), p3d.yaw() ) << "p2d: " << p2d << endl; }
void guideLines(const CPose3D& base, CSetOfLines::Ptr& lines, float dist) { CPoint3D pDist = CPoint3D(dist, 0, 0); CPoint3D pps[4]; pps[0] = base + pDist; pps[1] = base + CPose3D(0, 0, 0, 0, -M_PI / 2, 0) + pDist; pps[2] = base + CPose3D(0, 0, 0, -M_PI / 2, 0, 0) + pDist; pps[3] = base + CPose3D(0, 0, 0, M_PI / 2, 0, 0) + pDist; for (size_t i = 0; i < 4; i++) lines->appendLine( base.x(), base.y(), base.z(), pps[i].x(), pps[i].y(), pps[i].z()); lines->setLineWidth(5); lines->setColor(0, 0, 1); }
void test_lnAndExpMatches(double yaw1,double pitch1,double roll1) { const CPose3D pp(0,0,0,yaw1,pitch1,roll1); CQuaternionDouble q1; pp.getAsQuaternion(q1); mrpt::vector_double q1_ln = q1.ln<mrpt::vector_double>(); const CQuaternionDouble q2 = CQuaternionDouble::exp(q1_ln); // q2 should be == q1 EXPECT_NEAR(0, (q1-q2).Abs().sumAll(), 1e-10 ) << "q1:\n" << q1 << endl << "q2:\n" << q2 << endl << "Error:\n" << (q1-q2) << endl; }
void test_invComposePointJacob_se3( const CPose3D &p, const TPoint3D x_g ) { CMatrixFixedNumeric<double,3,6> df_dse3; TPoint3D pp; p.inverseComposePoint(x_g.x,x_g.y,x_g.z, pp.x,pp.y,pp.z, NULL, NULL, &df_dse3 ); // Numerical approx: CMatrixFixedNumeric<double,3,6> num_df_dse3(UNINITIALIZED_MATRIX); { CArrayDouble<6> x_mean; for (int i=0;i<6;i++) x_mean[i]=0; CArrayDouble<3> P; for (int i=0;i<3;i++) P[i]=pp[i]; CArrayDouble<6> x_incrs; x_incrs.assign(1e-9); mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_invcompose_point_se3,x_incrs, P, num_df_dse3 ); } EXPECT_NEAR(0, (df_dse3-num_df_dse3).array().abs().sum(), 3e-3 ) << "p: " << p << endl << "x_g: " << x_g << endl << "Numeric approximation of df_dse3: " << endl <<num_df_dse3 << endl << "Implemented method: " << endl << df_dse3 << endl << "Error: " << endl << df_dse3-num_df_dse3 << endl; }
void generatePolygon(CPolyhedronPtr &poly,const vector<TPoint3D> &profile,const CPose3D &pose) { math::TPolygon3D p(profile.size()); for (size_t i=0;i<profile.size();i++) pose.composePoint(profile[i].x,profile[i].y,profile[i].z,p[i].x,p[i].y,p[i].z); vector<math::TPolygon3D> convexPolys; if (!math::splitInConvexComponents(p,convexPolys)) convexPolys.push_back(p); poly=CPolyhedron::Create(convexPolys); }
double poses_test_compose3D2(int a1, int a2) { const long N = 100000; CTicTac tictac; CPose3D a(1.0,2.0,3.0,DEG2RAD(10),DEG2RAD(50),DEG2RAD(-30)); CPose3D b(8.0,-5.0,-1.0,DEG2RAD(-40),DEG2RAD(10),DEG2RAD(-45)); CPose3D 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; }
CPose3D CPose2D::operator -(const CPose3D& b) const { CMatrixDouble44 B_INV(UNINITIALIZED_MATRIX); b.getInverseHomogeneousMatrix( B_INV ); CMatrixDouble44 HM(UNINITIALIZED_MATRIX); this->getHomogeneousMatrix(HM); CMatrixDouble44 RES(UNINITIALIZED_MATRIX); RES.multiply(B_INV,HM); return CPose3D( RES ); }
/*--------------------------------------------------------------- changeCoordinatesReference ---------------------------------------------------------------*/ void CPointPDFGaussian::changeCoordinatesReference(const CPose3D &newReferenceBase ) { const CMatrixDouble33 &M = newReferenceBase.getRotationMatrix(); // The mean: mean = newReferenceBase + mean; // The covariance: M.multiply_HCHt(CMatrixDouble33(cov), cov); // save in cov }
/** A pseudo-Logarithm map in SE(3), where the output = [X,Y,Z, Ln(ROT)], that * is, the normal * SO(3) logarithm is used for the rotation components, but the translation is * left unmodified. */ void SE_traits<3>::pseudo_ln(const CPose3D& P, array_t& x) { x[0] = P.m_coords[0]; x[1] = P.m_coords[1]; x[2] = P.m_coords[2]; CArrayDouble<3> ln_rot = P.ln_rotation(); x[3] = ln_rot[0]; x[4] = ln_rot[1]; x[5] = ln_rot[2]; }
/*--------------------------------------------------------------- changeCoordinatesReference ---------------------------------------------------------------*/ void CPoint2DPDFGaussian::changeCoordinatesReference(const CPose3D &newReferenceBase ) { // Clip the 3x3 rotation matrix const CMatrixDouble22 M = newReferenceBase.getRotationMatrix().block(0,0,2,2); // The mean: mean = CPoint2D( newReferenceBase + mean ); // The covariance: cov = M*cov*M.transpose(); }
double pointmap_test_3(int a1, int a2) { // test 3: query2D // ---------------------------------------- // prepare the laser scan: CObservation2DRangeScan scan1; scan1.aperture = M_PIf; scan1.rightToLeft = true; scan1.validRange.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); scan1.scan.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) ); memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) ); CSimplePointsMap pt_map; pt_map.insertionOptions.minDistBetweenLaserPoints = 0.03; CPose3D pose; for (long i=0;i<a1;i++) { pose.setFromValues( pose.x()+0.04, pose.y()+0.08,0, pose.yaw()+0.02); pt_map.insertObservation(&scan1, &pose); } CTicTac tictac; float x0=-5; float y0=-4; float sq; float x,y; for (long i=0;i<a2;i++) { pt_map.kdTreeClosestPoint2D(x0,y0,x,y,sq); x0+=0.05; y0+=0.05; if (x0>20) x0=-10; if (y0>20) y0=-10; } return tictac.Tac()/a2; }
void test_composeFrom(double x1,double y1,double z1, double yaw1,double pitch1,double roll1, double x2,double y2,double z2, double yaw2,double pitch2,double roll2 ) { const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1); const CPose3D p2(x2,y2,z2,yaw2,pitch2,roll2); const CPose3D p1_plus_p2 = p1 + p2; { CPose3D p1_plus_p2bis; p1_plus_p2bis.composeFrom(p1,p2); EXPECT_NEAR(0, (p1_plus_p2bis.getAsVectorVal()-p1_plus_p2.getAsVectorVal()).array().abs().sum(), 1e-5) << "p2 : " << p2 << endl << "p1 : " << p1 << endl << "p1_plus_p2 : " << p1_plus_p2 << endl << "p1_plus_p2bis : " << p1_plus_p2bis<< endl; } { CPose3D p1_plus_p2bis = p1; p1_plus_p2bis.composeFrom(p1_plus_p2bis,p2); EXPECT_NEAR(0, (p1_plus_p2bis.getAsVectorVal()-p1_plus_p2.getAsVectorVal()).array().abs().sum(), 1e-5) << "p2 : " << p2 << endl << "p1 : " << p1 << endl << "p1_plus_p2 : " << p1_plus_p2 << endl << "p1_plus_p2bis : " << p1_plus_p2bis<< endl; } { CPose3D p1_plus_p2bis = p2; p1_plus_p2bis.composeFrom(p1,p1_plus_p2bis); EXPECT_NEAR(0, (p1_plus_p2bis.getAsVectorVal()-p1_plus_p2.getAsVectorVal()).array().abs().sum(), 1e-5) << "p2 : " << p2 << endl << "p1 : " << p1 << endl << "p1_plus_p2 : " << p1_plus_p2 << endl << "p1_plus_p2bis : " << p1_plus_p2bis<< endl; } }
void ICPslamWrapper::publishMapPose() { // get currently builded map metric_map_ = mapBuilder.getCurrentlyBuiltMetricMap(); // publish map if (metric_map_->m_gridMaps.size()) { nav_msgs::OccupancyGrid _msg; // if we have new map for current sensor update it mrpt_bridge::convert(*metric_map_->m_gridMaps[0], _msg); pub_map_.publish(_msg); pub_metadata_.publish(_msg.info); } if (metric_map_->m_pointsMaps.size()) { sensor_msgs::PointCloud _msg; std_msgs::Header header; header.stamp = ros::Time(0); header.frame_id = global_frame_id; // if we have new map for current sensor update it mrpt_bridge::point_cloud::mrpt2ros(*metric_map_->m_pointsMaps[0], header, _msg); pub_point_cloud_.publish(_msg); } CPose3D robotPose; mapBuilder.getCurrentPoseEstimation()->getMean(robotPose); // publish pose // geometry_msgs::PoseStamped pose; pose.header.frame_id = global_frame_id; // the pose pose.pose.position.x = robotPose.x(); pose.pose.position.y = robotPose.y(); pose.pose.position.z = 0.0; pose.pose.orientation = tf::createQuaternionMsgFromYaw(robotPose.yaw()); pub_pose_.publish(pose); }
/*--------------------------------------------------------------- point3D = point3D - pose3D ---------------------------------------------------------------*/ CPoint3D CPoint3D::operator - (const CPose3D& b) const { // JLBC: 7-FEB-2008: Why computing the whole matrix multiplication?? ;-) // 5.7us -> 4.1us -> 3.1us (with optimization of HM matrices by reference) // JLBC: 10-APR-2009: Usage of fixed-size 4x4 matrix, should be even faster now. CMatrixDouble44 B_INV(UNINITIALIZED_MATRIX); b.getInverseHomogeneousMatrix( B_INV ); return CPoint3D( B_INV.get_unsafe(0,0) * m_coords[0] + B_INV.get_unsafe(0,1) * m_coords[1] + B_INV.get_unsafe(0,2) * m_coords[2] + B_INV.get_unsafe(0,3), B_INV.get_unsafe(1,0) * m_coords[0] + B_INV.get_unsafe(1,1) * m_coords[1] + B_INV.get_unsafe(1,2) * m_coords[2] + B_INV.get_unsafe(1,3), B_INV.get_unsafe(2,0) * m_coords[0] + B_INV.get_unsafe(2,1) * m_coords[1] + B_INV.get_unsafe(2,2) * m_coords[2] + B_INV.get_unsafe(2,3) ); }