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