// ------------------------------------------------------
// 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;
}
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);
}
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.
}
}
// ------------------------------------------------------------
// 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++;
}
}
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;
}
}
/*---------------------------------------------------------------
drawSingleSample
---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::drawSingleSample(CPose3D& outPart) const
{
MRPT_UNUSED_PARAM(outPart);
MRPT_START
CMatrixDouble66 cov(UNINITIALIZED_MATRIX);
this->cov_inv.inv(cov);
CVectorDouble v;
getRandomGenerator().drawGaussianMultivariate(v, cov);
outPart.setFromValues(
mean.x() + v[0], mean.y() + v[1], mean.z() + v[2], mean.yaw() + v[3],
mean.pitch() + v[4], mean.roll() + v[5]);
MRPT_END_WITH_CLEAN_UP(
cov_inv.saveToTextFile(
"__DEBUG_EXC_DUMP_drawSingleSample_COV_INV.txt"););
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;
}
double pointmap_test_5(int a1, int a2)
{
// test 5: boundingBox
// ----------------------------------------
// 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,x1, y0,y1, z0,z1;
for (long i=0;i<a2;i++)
{
// Modify the map so the bounding box cache is invalidated:
pt_map.setPoint(0, 0,0,0);
pt_map.boundingBox(x0,x1, y0,y1, z0,z1);
}
return tictac.Tac()/a2;
}
/*---------------------------------------------------------------
getMean
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed
as a weighted average over all m_particles.
---------------------------------------------------------------*/
void CPose3DPDFParticles::getMean(CPose3D &p) const
{
MRPT_START
double X=0,Y=0,Z=0,YAW=0,PITCH=0,ROLL=0;
double ang,w,W=0;
double W_yaw_R=0,W_yaw_L=0;
double yaw_R=0,yaw_L=0;
double W_roll_R=0,W_roll_L=0;
double roll_R=0,roll_L=0;
// First: XYZ
// -----------------------------------
for (CPose3DPDFParticles::CParticleList::const_iterator it=m_particles.begin();it!=m_particles.end();++it)
{
w = exp(it->log_w);
W += w;
X += w * it->d->x();
Y += w * it->d->y();
Z += w * it->d->z();
PITCH += w * it->d->pitch();
// Angles Yaw and Roll are especial!:
ang = it->d->yaw();
if (fabs( ang )>0.5*M_PI)
{
// LEFT HALF: 0,2pi
if (ang<0) ang = (M_2PI + ang);
yaw_L += ang * w;
W_yaw_L += w;
}
else
{
// RIGHT HALF: -pi,pi
yaw_R += ang * w;
W_yaw_R += w;
}
// Angles Yaw and Roll are especial!:
ang = it->d->roll();
if (fabs( ang )>0.5*M_PI)
{
// LEFT HALF: 0,2pi
if (ang<0) ang = (M_2PI + ang);
roll_L += ang * w;
W_roll_L += w;
}
else
{
// RIGHT HALF: -pi,pi
roll_R += ang * w;
W_roll_R += w;
}
}
if (W==0) {
p.setFromValues(0,0,0,0,0,0);
return;
}
X /= W;
Y /= W;
Z /= W;
PITCH /= W;
// Next: Yaw and Roll:
// -----------------------------------
// The mean value from each side:
if (W_yaw_L>0) yaw_L /= W_yaw_L; // [0,2pi]
if (W_yaw_R>0) yaw_R /= W_yaw_R; // [-pi,pi]
// Left side to [-pi,pi] again:
if (yaw_L>M_PI) yaw_L = yaw_L - M_2PI;
// The mean value from each side:
if (W_roll_L>0) roll_L /= W_roll_L; // [0,2pi]
if (W_roll_R>0) roll_R /= W_roll_R; // [-pi,pi]
// Left side to [-pi,pi] again:
if (roll_L>M_PI) roll_L = roll_L - M_2PI;
// The total means:
YAW = ((yaw_L * W_yaw_L + yaw_R * W_yaw_R )/(W_yaw_L+W_yaw_R));
ROLL = ((roll_L * W_roll_L + roll_R * W_roll_R )/(W_roll_L+W_roll_R));
p.setFromValues(X,Y,Z,YAW,PITCH,ROLL);
MRPT_END
}
