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++;
}
}
void test_default_values(const CPose3D &p, const std::string & label)
{
EXPECT_EQ(p.x(),0);
EXPECT_EQ(p.y(),0);
EXPECT_EQ(p.z(),0);
EXPECT_EQ(p.yaw(),0);
EXPECT_EQ(p.pitch(),0);
EXPECT_EQ(p.roll(),0);
for (size_t i=0;i<4;i++)
for (size_t j=0;j<4;j++)
EXPECT_NEAR(p.getHomogeneousMatrixVal()(i,j), i==j ? 1.0 : 0.0, 1e-8 )
<< "Failed for (i,j)=" << i << "," << j << endl
<< "Matrix is: " << endl << p.getHomogeneousMatrixVal() << endl
<< "case was: " << label << endl;
}
/*---------------------------------------------------------------
saveInterpolatedToTextFile
---------------------------------------------------------------*/
bool CPose3DInterpolator::saveInterpolatedToTextFile(const std::string &s, double period) const
{
try
{
CFileOutputStream f(s);
if (m_path.empty()) return true;
const TTimeStamp t_ini = m_path.begin()->first;
const TTimeStamp t_end = m_path.rbegin()->first;
TTimeStamp At = mrpt::system::secondsToTimestamp(period);
//cout << "Interp: " << t_ini << " " << t_end << " " << At << endl;
CPose3D p;
bool valid;
for (TTimeStamp t=t_ini;t<=t_end;t+=At)
{
this->interpolate( t, p, valid );
if (!valid) continue;
int r = f.printf("%.06f %.06f %.06f %.06f %.06f %.06f %.06f\n",
mrpt::system::timestampTotime_t(t),
p.x(),p.y(),p.z(),
p.yaw(),p.pitch(),p.roll());
ASSERT_(r>0);
}
return true;
}
catch(...)
{
return false;
}
}
CPose3DPDFPtr CICP::ICP3D_Method_Classic(
const mrpt::maps::CMetricMap *m1,
const mrpt::maps::CMetricMap *mm2,
const CPose3DPDFGaussian &initialEstimationPDF,
TReturnInfo &outInfo )
{
MRPT_START
// The result can be either a Gaussian or a SOG:
CPose3DPDFPtr resultPDF;
CPose3DPDFGaussianPtr gaussPdf;
size_t nCorrespondences=0;
bool keepApproaching;
CPose3D grossEst = initialEstimationPDF.mean;
mrpt::utils::TMatchingPairList correspondences,old_correspondences;
CPose3D lastMeanPose;
// Assure the class of the maps:
ASSERT_(mm2->GetRuntimeClass()->derivedFrom(CLASS_ID(CPointsMap)));
const CPointsMap *m2 = (CPointsMap*)mm2;
// Asserts:
// -----------------
ASSERT_( options.ALFA>0 && options.ALFA<1 );
// The algorithm output auxiliar info:
// -------------------------------------------------
outInfo.nIterations = 0;
outInfo.goodness = 1;
outInfo.quality = 0;
// The gaussian PDF to estimate:
// ------------------------------------------------------
gaussPdf = CPose3DPDFGaussian::Create();
// First gross approximation:
gaussPdf->mean = grossEst;
// Initial thresholds:
TMatchingParams matchParams;
TMatchingExtraResults matchExtraResults;
matchParams.maxDistForCorrespondence = options.thresholdDist; // Distance threshold
matchParams.maxAngularDistForCorrespondence = options.thresholdAng; // Angular threshold
matchParams.onlyKeepTheClosest = options.onlyClosestCorrespondences;
matchParams.onlyUniqueRobust = options.onlyUniqueRobust;
matchParams.decimation_other_map_points = options.corresponding_points_decimation;
// Asure maps are not empty!
// ------------------------------------------------------
if ( !m2->isEmpty() )
{
matchParams.offset_other_map_points = 0;
// ------------------------------------------------------
// The ICP loop
// ------------------------------------------------------
do
{
matchParams.angularDistPivotPoint = TPoint3D(gaussPdf->mean.x(),gaussPdf->mean.y(),gaussPdf->mean.z());
// ------------------------------------------------------
// Find the matching (for a points map)
// ------------------------------------------------------
m1->determineMatching3D(
m2, // The other map
gaussPdf->mean, // The other map pose
correspondences,
matchParams, matchExtraResults);
nCorrespondences = correspondences.size();
if ( !nCorrespondences )
{
// Nothing we can do !!
keepApproaching = false;
}
else
{
// Compute the estimated pose, using Horn's method.
// ----------------------------------------------------------------------
mrpt::poses::CPose3DQuat estPoseQuat;
double transf_scale;
mrpt::tfest::se3_l2(correspondences, estPoseQuat, transf_scale, false /* dont force unit scale */ );
gaussPdf->mean = estPoseQuat;
// If matching has not changed, decrease the thresholds:
// --------------------------------------------------------
keepApproaching = true;
if (!(fabs(lastMeanPose.x()-gaussPdf->mean.x())>options.minAbsStep_trans ||
fabs(lastMeanPose.y()-gaussPdf->mean.y())>options.minAbsStep_trans ||
fabs(lastMeanPose.z()-gaussPdf->mean.z())>options.minAbsStep_trans ||
fabs(math::wrapToPi(lastMeanPose.yaw()-gaussPdf->mean.yaw()))>options.minAbsStep_rot ||
fabs(math::wrapToPi(lastMeanPose.pitch()-gaussPdf->mean.pitch()))>options.minAbsStep_rot ||
fabs(math::wrapToPi(lastMeanPose.roll()-gaussPdf->mean.roll()))>options.minAbsStep_rot ))
{
matchParams.maxDistForCorrespondence *= options.ALFA;
matchParams.maxAngularDistForCorrespondence *= options.ALFA;
if (matchParams.maxDistForCorrespondence < options.smallestThresholdDist )
keepApproaching = false;
if (++matchParams.offset_other_map_points>=options.corresponding_points_decimation)
matchParams.offset_other_map_points=0;
}
lastMeanPose = gaussPdf->mean;
} // end of "else, there are correspondences"
// Next iteration:
outInfo.nIterations++;
if (outInfo.nIterations >= options.maxIterations && matchParams.maxDistForCorrespondence>options.smallestThresholdDist)
{
matchParams.maxDistForCorrespondence *= options.ALFA;
}
} while ( (keepApproaching && outInfo.nIterations<options.maxIterations) ||
(outInfo.nIterations >= options.maxIterations && matchParams.maxDistForCorrespondence>options.smallestThresholdDist) );
// -------------------------------------------------
// Obtain the covariance matrix of the estimation
// -------------------------------------------------
if (!options.skip_cov_calculation && nCorrespondences)
{
// ...
}
//
outInfo.goodness = matchExtraResults.correspondencesRatio;
} // end of "if m2 is not empty"
// Return the gaussian distribution:
resultPDF = gaussPdf;
return resultPDF;
MRPT_END
}
int main(int argc, char ** argv)
{
try
{
bool showHelp = argc>1 && !os::_strcmp(argv[1],"--help");
bool showVersion = argc>1 && !os::_strcmp(argv[1],"--version");
printf(" simul-landmarks - Part of the MRPT\n");
printf(" MRPT C++ Library: %s - BUILD DATE %s\n", MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str());
if (showVersion)
return 0; // Program end
// Process arguments:
if (argc<2 || showHelp )
{
printf("Usage: %s <config_file.ini>\n\n",argv[0]);
if (!showHelp)
{
mrpt::system::pause();
return -1;
}
else return 0;
}
string INI_FILENAME = std::string( argv[1] );
ASSERT_FILE_EXISTS_(INI_FILENAME)
CConfigFile ini( INI_FILENAME );
const int random_seed = ini.read_int("Params","random_seed",0);
if (random_seed!=0)
randomGenerator.randomize(random_seed);
else randomGenerator.randomize();
// Set default values:
unsigned int nLandmarks = 3;
unsigned int nSteps = 100;
std::string outFile("out.rawlog");
std::string outDir("OUT");
float min_x =-5;
float max_x =5;
float min_y =-5;
float max_y =5;
float min_z =0;
float max_z =0;
float odometryNoiseXY_std = 0.001f;
float odometryNoisePhi_std_deg = 0.01f;
float odometryNoisePitch_std_deg = 0.01f;
float odometryNoiseRoll_std_deg = 0.01f;
float minSensorDistance = 0;
float maxSensorDistance = 10;
float fieldOfView_deg= 180.0f;
float sensorPose_x = 0;
float sensorPose_y = 0;
float sensorPose_z = 0;
float sensorPose_yaw_deg = 0;
float sensorPose_pitch_deg = 0;
float sensorPose_roll_deg = 0;
float stdRange = 0.01f;
float stdYaw_deg = 0.1f;
float stdPitch_deg = 0.1f;
bool sensorDetectsIDs=true;
bool circularPath = true;
bool random6DPath = false;
size_t squarePathLength=40;
// Load params from INI:
MRPT_LOAD_CONFIG_VAR(outFile,string, ini,"Params");
MRPT_LOAD_CONFIG_VAR(outDir,string, ini,"Params");
MRPT_LOAD_CONFIG_VAR(nSteps,int, ini,"Params");
MRPT_LOAD_CONFIG_VAR(circularPath,bool, ini,"Params");
MRPT_LOAD_CONFIG_VAR(random6DPath,bool, ini,"Params");
MRPT_LOAD_CONFIG_VAR(squarePathLength,int,ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorDetectsIDs,bool, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryNoiseXY_std,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryNoisePhi_std_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryNoisePitch_std_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryNoiseRoll_std_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorPose_x,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorPose_y,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorPose_z,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorPose_yaw_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorPose_pitch_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorPose_roll_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(minSensorDistance,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(maxSensorDistance,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(fieldOfView_deg,float, ini,"Params");
bool show_in_3d = false;
MRPT_LOAD_CONFIG_VAR(show_in_3d,bool, ini,"Params");
MRPT_LOAD_CONFIG_VAR(stdRange,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(stdYaw_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(stdPitch_deg,float, ini,"Params");
float stdYaw = DEG2RAD(stdYaw_deg);
float stdPitch = DEG2RAD(stdPitch_deg);
float odometryNoisePhi_std = DEG2RAD(odometryNoisePhi_std_deg);
float odometryNoisePitch_std = DEG2RAD(odometryNoisePitch_std_deg);
float odometryNoiseRoll_std = DEG2RAD(odometryNoiseRoll_std_deg);
float fieldOfView = DEG2RAD(fieldOfView_deg);
CPose3D sensorPoseOnRobot(
sensorPose_x,
sensorPose_y,
sensorPose_z,
DEG2RAD( sensorPose_yaw_deg ),
DEG2RAD( sensorPose_pitch_deg ),
DEG2RAD( sensorPose_roll_deg ) );
// Create out dir:
mrpt::system::createDirectory(outDir);
// ---------------------------------------------
// Create the point-beacons:
// ---------------------------------------------
printf("Creating landmark map...");
mrpt::maps::CLandmarksMap landmarkMap;
int randomSetCount = 0;
int uniqueIds = 1;
// Process each of the "RANDOMSET_%i" found:
do
{
string sectName = format("RANDOMSET_%i",++randomSetCount);
nLandmarks = 0;
MRPT_LOAD_CONFIG_VAR(nLandmarks,uint64_t, ini,sectName);
MRPT_LOAD_CONFIG_VAR(min_x,float, ini,sectName);
MRPT_LOAD_CONFIG_VAR(max_x,float, ini,sectName);
MRPT_LOAD_CONFIG_VAR(min_y,float, ini,sectName);
MRPT_LOAD_CONFIG_VAR(max_y,float, ini,sectName);
MRPT_LOAD_CONFIG_VAR(min_z,float, ini,sectName);
MRPT_LOAD_CONFIG_VAR(max_z,float, ini,sectName);
for (size_t i=0; i<nLandmarks; i++)
{
CLandmark LM;
CPointPDFGaussian pt3D;
// Random coordinates:
pt3D.mean = CPoint3D(
randomGenerator.drawUniform(min_x,max_x),
randomGenerator.drawUniform(min_y,max_y),
randomGenerator.drawUniform(min_z,max_z) );
// Add:
LM.createOneFeature();
LM.features[0]->type = featBeacon;
LM.ID = uniqueIds++;
LM.setPose( pt3D );
landmarkMap.landmarks.push_back(LM);
}
if (nLandmarks) cout << nLandmarks << " generated for the 'randomset' " << randomSetCount << endl;
}
while (nLandmarks);
landmarkMap.saveToTextFile( format("%s/%s_ground_truth.txt",outDir.c_str(),outFile.c_str()) );
printf("Done!\n");
// ---------------------------------------------
// Simulate:
// ---------------------------------------------
size_t nWarningsNoSight=0;
CActionRobotMovement2D::TMotionModelOptions opts;
opts.modelSelection = CActionRobotMovement2D::mmGaussian;
opts.gausianModel.a1=0;
opts.gausianModel.a2=0;
opts.gausianModel.a3=0;
opts.gausianModel.a4=0;
opts.gausianModel.minStdXY = odometryNoiseXY_std;
opts.gausianModel.minStdPHI = odometryNoisePhi_std;
// Output rawlog, gz-compressed.
CFileGZOutputStream fil( format("%s/%s",outDir.c_str(),outFile.c_str()));
CPose3D realPose;
const size_t N_STEPS_STOP_AT_THE_BEGINNING = 4;
CMatrixDouble GT_path;
for (size_t i=0; i<nSteps; i++)
{
cout << "Generating step " << i << "...\n";
CSensoryFrame SF;
CActionCollection acts;
CPose3D incPose3D;
bool incPose_is_3D = random6DPath;
if (i>=N_STEPS_STOP_AT_THE_BEGINNING)
{
if (random6DPath)
{ // 3D path
const double Ar = DEG2RAD(3);
TPose3D Ap = TPose3D(0.20*cos(Ar),0.20*sin(Ar),0,Ar,0,0);
//Ap.z += randomGenerator.drawGaussian1D(0,0.05);
Ap.yaw += randomGenerator.drawGaussian1D(0,DEG2RAD(0.2));
Ap.pitch += randomGenerator.drawGaussian1D(0,DEG2RAD(2));
Ap.roll += randomGenerator.drawGaussian1D(0,DEG2RAD(4));
incPose3D = CPose3D(Ap);
}
else
{ // 2D path:
if (circularPath)
{
// Circular path:
float Ar = DEG2RAD(5);
incPose3D = CPose3D(CPose2D(0.20f*cos(Ar),0.20f*sin(Ar),Ar));
}
else
{
// Square path:
if ( (i % squarePathLength) > (squarePathLength-5) )
incPose3D = CPose3D(CPose2D(0,0,DEG2RAD(90.0f/4)));
else incPose3D = CPose3D(CPose2D(0.20f,0,0));
}
}
}
else
{
// Robot is still at the beginning:
incPose3D = CPose3D(0,0,0,0,0,0);
}
// Simulate observations:
CObservationBearingRangePtr obs=CObservationBearingRange::Create();
obs->minSensorDistance=minSensorDistance;
obs->maxSensorDistance=maxSensorDistance;
obs->fieldOfView_yaw = fieldOfView;
obs->fieldOfView_pitch = fieldOfView;
obs->sensorLocationOnRobot = sensorPoseOnRobot;
landmarkMap.simulateRangeBearingReadings(
realPose,
sensorPoseOnRobot,
*obs,
sensorDetectsIDs, // wheter to identy landmarks
stdRange,
stdYaw,
stdPitch );
// Keep the GT of the robot pose:
GT_path.setSize(i+1,6);
for (size_t k=0; k<6; k++)
GT_path(i,k)=realPose[k];
cout << obs->sensedData.size() << " landmarks in sight";
if (obs->sensedData.empty()) nWarningsNoSight++;
SF.push_back( obs );
// Simulate odometry, from "incPose3D" with noise:
if (!incPose_is_3D)
{ // 2D odometry:
CActionRobotMovement2D act;
CPose2D incOdo( incPose3D );
if (incPose3D.x()!=0 || incPose3D.y()!=0 || incPose3D.yaw()!=0)
{
incOdo.x_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
incOdo.y_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
incOdo.phi_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoisePhi_std );
}
act.computeFromOdometry(incOdo, opts);
acts.insert( act );
}
else
{ // 3D odometry:
CActionRobotMovement3D act;
act.estimationMethod = CActionRobotMovement3D::emOdometry;
CPose3D noisyIncPose = incPose3D;
if (incPose3D.x()!=0 || incPose3D.y()!=0 || incPose3D.yaw()!=0)
{
noisyIncPose.x_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
noisyIncPose.y_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
noisyIncPose.z_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
noisyIncPose.setYawPitchRoll(
noisyIncPose.yaw()+ randomGenerator.drawGaussian1D_normalized() * odometryNoisePhi_std,
noisyIncPose.pitch()+ randomGenerator.drawGaussian1D_normalized() * odometryNoisePitch_std,
noisyIncPose.roll()+ randomGenerator.drawGaussian1D_normalized() * odometryNoiseRoll_std );
}
act.poseChange.mean = noisyIncPose;
act.poseChange.cov.eye();
act.poseChange.cov(0,0) =
act.poseChange.cov(1,1) =
act.poseChange.cov(2,2) = square(odometryNoiseXY_std);
act.poseChange.cov(3,3) = square(odometryNoisePhi_std);
act.poseChange.cov(4,4) = square(odometryNoisePitch_std);
act.poseChange.cov(5,5) = square(odometryNoiseRoll_std);
acts.insert( act );
}
// Save:
fil << SF << acts;
// Next pose:
realPose = realPose + incPose3D;
cout << endl;
}
// Save the ground truth for the robot poses as well:
GT_path.saveToTextFile(
format("%s/%s_ground_truth_robot_path.txt",outDir.c_str(),outFile.c_str()),
MATRIX_FORMAT_FIXED,
true,
"% Columns are: x(m) y(m) z(m) yaw(rad) pitch(rad) roll(rad)\n");
cout << "Data saved to directory: " << outDir << endl;
if (nWarningsNoSight)
cout << "WARNING: " << nWarningsNoSight << " observations contained zero landmarks in the sensor range." << endl;
// Optionally, display in 3D:
if (show_in_3d && size(GT_path,1)>1)
{
#if MRPT_HAS_OPENGL_GLUT && MRPT_HAS_WXWIDGETS
mrpt::gui::CDisplayWindow3D win("Final simulation",400,300);
mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock();
scene->insert( mrpt::opengl::CGridPlaneXY::Create( min_x-10,max_x+10,min_y-10,max_y+10,0 ));
scene->insert( mrpt::opengl::stock_objects::CornerXYZ() );
// Insert all landmarks:
for (CLandmarksMap::TCustomSequenceLandmarks::const_iterator it=landmarkMap.landmarks.begin(); it!=landmarkMap.landmarks.end(); ++it)
{
mrpt::opengl::CSpherePtr lm = mrpt::opengl::CSphere::Create();
lm->setColor(1,0,0);
lm->setRadius(0.1);
lm->setLocation( it->pose_mean );
lm->setName( format("LM#%u",(unsigned) it->ID ) );
//lm->enableShowName(true);
scene->insert(lm);
}
// Insert all robot poses:
const size_t N = size(GT_path,1);
mrpt::opengl::CSetOfLinesPtr pathLines = mrpt::opengl::CSetOfLines::Create();
pathLines->setColor(0,0,1,0.5);
pathLines->setLineWidth(3.0);
pathLines->resize(N-1);
for (size_t i=0; i<N-1; i++)
pathLines->setLineByIndex(i, GT_path(i,0),GT_path(i,1),GT_path(i,2), GT_path(i+1,0),GT_path(i+1,1),GT_path(i+1,2) );
scene->insert(pathLines);
for (size_t i=0; i<N; i++)
{
mrpt::opengl::CSetOfObjectsPtr corner = mrpt::opengl::stock_objects::CornerXYZ();
corner->setScale(0.2);
corner->setPose(TPose3D(GT_path(i,0),GT_path(i,1),GT_path(i,2),GT_path(i,3),GT_path(i,4),GT_path(i,5)));
scene->insert(corner);
}
win.unlockAccess3DScene();
win.forceRepaint();
cout << "Press any key or close the 3D window to exit." << endl;
win.waitForKey();
#endif // MRPT_HAS_OPENGL_GLUT && MRPT_HAS_WXWIDGETS
}
}
catch (std::exception &e)
{
std::cout << e.what();
}
catch (...)
{
std::cout << "Untyped exception!";
}
}
/*---------------------------------------------------------------
getCovarianceAndMean
---------------------------------------------------------------*/
void CPose3DPDFParticles::getCovarianceAndMean(
CMatrixDouble66& cov, CPose3D& mean) const
{
MRPT_START
getMean(mean); // First! the mean value:
// Now the covariance:
cov.zeros();
CVectorDouble vars;
vars.assign(6, 0.0); // The diagonal of the final covariance matrix
// Elements off the diagonal of the covariance matrix:
double std_xy = 0, std_xz = 0, std_xya = 0, std_xp = 0, std_xr = 0;
double std_yz = 0, std_yya = 0, std_yp = 0, std_yr = 0;
double std_zya = 0, std_zp = 0, std_zr = 0;
double std_yap = 0, std_yar = 0;
double std_pr = 0;
// Mean values in [0, 2pi] range:
double mean_yaw = mean.yaw();
double mean_pitch = mean.pitch();
double mean_roll = mean.roll();
if (mean_yaw < 0) mean_yaw += M_2PI;
if (mean_pitch < 0) mean_pitch += M_2PI;
if (mean_roll < 0) mean_roll += M_2PI;
// Enought information to estimate the covariance?
if (m_particles.size() < 2) return;
// Sum all weight values:
double W = 0;
for (CPose3DPDFParticles::CParticleList::const_iterator it =
m_particles.begin();
it != m_particles.end(); ++it)
W += exp(it->log_w);
ASSERT_(W > 0);
// Compute covariance:
for (CPose3DPDFParticles::CParticleList::const_iterator it =
m_particles.begin();
it != m_particles.end(); ++it)
{
double w = exp(it->log_w) / W;
// Manage 1 PI range:
double err_yaw = wrapToPi(fabs(it->d->yaw() - mean_yaw));
double err_pitch = wrapToPi(fabs(it->d->pitch() - mean_pitch));
double err_roll = wrapToPi(fabs(it->d->roll() - mean_roll));
double err_x = it->d->x() - mean.x();
double err_y = it->d->y() - mean.y();
double err_z = it->d->z() - mean.z();
vars[0] += square(err_x) * w;
vars[1] += square(err_y) * w;
vars[2] += square(err_z) * w;
vars[3] += square(err_yaw) * w;
vars[4] += square(err_pitch) * w;
vars[5] += square(err_roll) * w;
std_xy += err_x * err_y * w;
std_xz += err_x * err_z * w;
std_xya += err_x * err_yaw * w;
std_xp += err_x * err_pitch * w;
std_xr += err_x * err_roll * w;
std_yz += err_y * err_z * w;
std_yya += err_y * err_yaw * w;
std_yp += err_y * err_pitch * w;
std_yr += err_y * err_roll * w;
std_zya += err_z * err_yaw * w;
std_zp += err_z * err_pitch * w;
std_zr += err_z * err_roll * w;
std_yap += err_yaw * err_pitch * w;
std_yar += err_yaw * err_roll * w;
std_pr += err_pitch * err_roll * w;
} // end for it
// Unbiased estimation of variance:
cov(0, 0) = vars[0];
cov(1, 1) = vars[1];
cov(2, 2) = vars[2];
cov(3, 3) = vars[3];
cov(4, 4) = vars[4];
cov(5, 5) = vars[5];
cov(1, 0) = cov(0, 1) = std_xy;
cov(2, 0) = cov(0, 2) = std_xz;
cov(3, 0) = cov(0, 3) = std_xya;
cov(4, 0) = cov(0, 4) = std_xp;
cov(5, 0) = cov(0, 5) = std_xr;
cov(2, 1) = cov(1, 2) = std_yz;
cov(3, 1) = cov(1, 3) = std_yya;
cov(4, 1) = cov(1, 4) = std_yp;
cov(5, 1) = cov(1, 5) = std_yr;
cov(3, 2) = cov(2, 3) = std_zya;
cov(4, 2) = cov(2, 4) = std_zp;
cov(5, 2) = cov(2, 5) = std_zr;
cov(4, 3) = cov(3, 4) = std_yap;
cov(5, 3) = cov(3, 5) = std_yar;
cov(5, 4) = cov(4, 5) = std_pr;
MRPT_END
}
/*---------------------------------------------------------------
pose3D = point3D + pose3D
---------------------------------------------------------------*/
CPose3D CPoint3D::operator+(const CPose3D& b) const
{
return CPose3D(
m_coords[0] + b.x(), m_coords[1] + b.y(), m_coords[2] + b.z(), b.yaw(),
b.pitch(), b.roll());
}
// ------------------------------------------------------
// Test_Kinect
// ------------------------------------------------------
void Test_Kinect()
{
// Launch grabbing thread:
// --------------------------------------------------------
TThreadParam thrPar;
std::thread thHandle = std::thread(thread_grabbing, std::ref(thrPar));
// Wait until data stream starts so we can say for sure the sensor has been
// initialized OK:
cout << "Waiting for sensor initialization...\n";
do
{
CObservation3DRangeScan::Ptr possiblyNewObs =
std::atomic_load(&thrPar.new_obs);
if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP)
break;
else
std::this_thread::sleep_for(10ms);
} while (!thrPar.quit);
// Check error condition:
if (thrPar.quit) return;
// Feature tracking variables:
CFeatureList trackedFeats;
unsigned int step_num = 0;
bool SHOW_FEAT_IDS = true;
bool SHOW_RESPONSES = true;
CGenericFeatureTrackerAutoPtr tracker;
// "CFeatureTracker_KL" is by far the most robust implementation for now:
tracker = CGenericFeatureTrackerAutoPtr(new CFeatureTracker_KL);
tracker->enableTimeLogger(true); // Do time profiling.
// Set of parameters common to any tracker implementation:
// To see all the existing params and documentation, see
// mrpt::vision::CGenericFeatureTracker
// http://reference.mrpt.org/devel/structmrpt_1_1vision_1_1_c_generic_feature_tracker.html
tracker->extra_params["add_new_features"] =
1; // track, AND ALSO, add new features
tracker->extra_params["add_new_feat_min_separation"] = 25;
tracker->extra_params["add_new_feat_max_features"] = 150;
tracker->extra_params["add_new_feat_patch_size"] = 21;
tracker->extra_params["minimum_KLT_response_to_add"] = 40;
tracker->extra_params["check_KLT_response_every"] =
5; // Re-check the KLT-response to assure features are in good points.
tracker->extra_params["minimum_KLT_response"] =
25; // Re-check the KLT-response to assure features are in good points.
tracker->extra_params["update_patches_every"] = 0; // Update patches
// Specific params for "CFeatureTracker_KL"
tracker->extra_params["window_width"] = 25;
tracker->extra_params["window_height"] = 25;
// Global points map:
CColouredPointsMap globalPtsMap;
globalPtsMap.colorScheme.scheme =
CColouredPointsMap::cmFromIntensityImage; // Take points color from
// RGB+D observations
// globalPtsMap.colorScheme.scheme =
// CColouredPointsMap::cmFromHeightRelativeToSensorGray;
// Create window and prepare OpenGL object in the scene:
// --------------------------------------------------------
mrpt::gui::CDisplayWindow3D win3D("kinect-3d-slam 3D view", 800, 600);
win3D.setCameraAzimuthDeg(140);
win3D.setCameraElevationDeg(20);
win3D.setCameraZoom(8.0);
win3D.setFOV(90);
win3D.setCameraPointingToPoint(2.5, 0, 0);
mrpt::opengl::CPointCloudColoured::Ptr gl_points =
mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
gl_points->setPointSize(2.5);
mrpt::opengl::CSetOfObjects::Ptr gl_curFeats =
mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
mrpt::opengl::CSetOfObjects::Ptr gl_keyframes =
mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
mrpt::opengl::CPointCloudColoured::Ptr gl_points_map =
mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
gl_points_map->setPointSize(2.0);
const double aspect_ratio =
480.0 / 640.0; // kinect.rows() / double( kinect.cols() );
mrpt::opengl::CSetOfObjects::Ptr gl_cur_cam_corner =
mrpt::opengl::stock_objects::CornerXYZSimple(0.4f, 4);
opengl::COpenGLViewport::Ptr viewInt;
{
mrpt::opengl::COpenGLScene::Ptr& scene = win3D.get3DSceneAndLock();
// Create the Opengl object for the point cloud:
scene->insert(gl_points_map);
scene->insert(gl_points);
scene->insert(gl_curFeats);
scene->insert(gl_keyframes);
scene->insert(mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>());
scene->insert(gl_cur_cam_corner);
const int VW_WIDTH =
350; // Size of the viewport into the window, in pixel units.
const int VW_HEIGHT = aspect_ratio * VW_WIDTH;
// Create the Opengl objects for the planar images each in a separate
// viewport:
viewInt = scene->createViewport("view2d_int");
viewInt->setViewportPosition(2, 2, VW_WIDTH, VW_HEIGHT);
viewInt->setTransparent(true);
win3D.unlockAccess3DScene();
win3D.repaint();
}
CImage previous_image;
map<TFeatureID, TPoint3D> lastVisibleFeats;
std::vector<TPose3D>
camera_key_frames_path; // The 6D path of the Kinect camera.
CPose3D
currentCamPose_wrt_last; // wrt last pose in "camera_key_frames_path"
bool gl_keyframes_must_refresh =
true; // Need to update gl_keyframes from camera_key_frames_path??
CObservation3DRangeScan::Ptr last_obs;
string str_status, str_status2;
while (win3D.isOpen() && !thrPar.quit)
{
CObservation3DRangeScan::Ptr possiblyNewObs =
std::atomic_load(&thrPar.new_obs);
if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP &&
(!last_obs || possiblyNewObs->timestamp != last_obs->timestamp))
{
// It IS a new observation:
last_obs = possiblyNewObs;
// Feature tracking -------------------------------------------
ASSERT_(last_obs->hasIntensityImage);
CImage theImg; // The grabbed image:
theImg = last_obs->intensityImage;
// Do tracking:
if (step_num > 1) // we need "previous_image" to be valid.
{
tracker->trackFeatures(previous_image, theImg, trackedFeats);
// Remove those now out of the image plane:
CFeatureList::iterator itFeat = trackedFeats.begin();
while (itFeat != trackedFeats.end())
{
const TFeatureTrackStatus status = (*itFeat)->track_status;
bool eras =
(status_TRACKED != status && status_IDLE != status);
if (!eras)
{
// Also, check if it's too close to the image border:
const float x = (*itFeat)->x;
const float y = (*itFeat)->y;
static const float MIN_DIST_MARGIN_TO_STOP_TRACKING =
10;
if (x < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
y < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
x > (last_obs->cameraParamsIntensity.ncols -
MIN_DIST_MARGIN_TO_STOP_TRACKING) ||
y > (last_obs->cameraParamsIntensity.nrows -
MIN_DIST_MARGIN_TO_STOP_TRACKING))
{
eras = true;
}
}
if (eras) // Erase or keep?
itFeat = trackedFeats.erase(itFeat);
else
++itFeat;
}
}
// Create list of 3D features in space, wrt current camera pose:
// --------------------------------------------------------------------
map<TFeatureID, TPoint3D> curVisibleFeats;
for (CFeatureList::iterator itFeat = trackedFeats.begin();
itFeat != trackedFeats.end(); ++itFeat)
{
// Pixel coordinates in the intensity image:
const int int_x = (*itFeat)->x;
const int int_y = (*itFeat)->y;
// Convert to pixel coords in the range image:
// APPROXIMATION: Assume coordinates are equal (that's not
// exact!!)
const int x = int_x;
const int y = int_y;
// Does this (x,y) have valid range data?
const float d = last_obs->rangeImage(y, x);
if (d > 0.05 && d < 10.0)
{
ASSERT_(
size_t(
last_obs->rangeImage.cols() *
last_obs->rangeImage.rows()) ==
last_obs->points3D_x.size());
const size_t nPt = last_obs->rangeImage.cols() * y + x;
curVisibleFeats[(*itFeat)->ID] = TPoint3D(
last_obs->points3D_x[nPt], last_obs->points3D_y[nPt],
last_obs->points3D_z[nPt]);
}
}
// Load local points map from 3D points + color:
CColouredPointsMap localPntsMap;
localPntsMap.colorScheme.scheme =
CColouredPointsMap::cmFromIntensityImage;
localPntsMap.loadFromRangeScan(*last_obs);
// Estimate our current camera pose from feature2feature matching:
// --------------------------------------------------------------------
if (!lastVisibleFeats.empty())
{
TMatchingPairList corrs; // pairs of correspondences
for (map<TFeatureID, TPoint3D>::const_iterator itCur =
curVisibleFeats.begin();
itCur != curVisibleFeats.end(); ++itCur)
{
map<TFeatureID, TPoint3D>::const_iterator itFound =
lastVisibleFeats.find(itCur->first);
if (itFound != lastVisibleFeats.end())
{
corrs.push_back(
TMatchingPair(
itFound->first, itCur->first, itFound->second.x,
itFound->second.y, itFound->second.z,
itCur->second.x, itCur->second.y,
itCur->second.z));
}
}
if (corrs.size() >= 3)
{
// Find matchings:
mrpt::tfest::TSE3RobustParams params;
params.ransac_minSetSize = 3;
params.ransac_maxSetSizePct = 6.0 / corrs.size();
mrpt::tfest::TSE3RobustResult results;
bool register_ok = false;
try
{
mrpt::tfest::se3_l2_robust(corrs, params, results);
register_ok = true;
}
catch (std::exception&)
{
/* Cannot find a minimum number of matches, inconsistent
* parameters due to very reduced numberof matches,etc.
*/
}
const CPose3D relativePose =
CPose3D(results.transformation);
str_status = mrpt::format(
"%d corrs | inliers: %d | rel.pose: %s ",
int(corrs.size()), int(results.inliers_idx.size()),
relativePose.asString().c_str());
str_status2 = string(
results.inliers_idx.size() == 0
? "LOST! Please, press 'r' to restart"
: "");
if (register_ok && std::abs(results.scale - 1.0) < 0.1)
{
// Seems a good match:
if ((relativePose.norm() > KEYFRAMES_MIN_DISTANCE ||
std::abs(relativePose.yaw()) > KEYFRAMES_MIN_ANG ||
std::abs(relativePose.pitch()) >
KEYFRAMES_MIN_ANG ||
std::abs(relativePose.roll()) > KEYFRAMES_MIN_ANG))
{
// Accept this as a new key-frame pose ------------
// Append new global pose of this key-frame:
const CPose3D new_keyframe_global =
CPose3D(*camera_key_frames_path.rbegin()) +
relativePose;
camera_key_frames_path.push_back(
new_keyframe_global.asTPose());
gl_keyframes_must_refresh = true;
currentCamPose_wrt_last =
CPose3D(); // It's (0,0,0) since the last
// key-frame is the current pose!
lastVisibleFeats = curVisibleFeats;
cout << "Adding new key-frame: pose="
<< new_keyframe_global << endl;
// Update global map: append another map at a given
// position:
globalPtsMap.insertObservation(
last_obs.get(), &new_keyframe_global);
win3D.get3DSceneAndLock();
gl_points_map->loadFromPointsMap(&globalPtsMap);
win3D.unlockAccess3DScene();
}
else
{
currentCamPose_wrt_last = relativePose;
// cout << "cur pose: " << currentCamPose_wrt_last
// << endl;
}
}
}
}
if (camera_key_frames_path.empty() || lastVisibleFeats.empty())
{
// First iteration:
camera_key_frames_path.clear();
camera_key_frames_path.push_back(TPose3D(0, 0, 0, 0, 0, 0));
gl_keyframes_must_refresh = true;
lastVisibleFeats = curVisibleFeats;
// Update global map:
globalPtsMap.clear();
globalPtsMap.insertObservation(last_obs.get());
win3D.get3DSceneAndLock();
gl_points_map->loadFromPointsMap(&globalPtsMap);
win3D.unlockAccess3DScene();
}
// Save the image for the next step:
previous_image = theImg;
// Draw marks on the RGB image:
theImg.selectTextFont("10x20");
{ // Tracked feats:
theImg.drawFeatures(
trackedFeats, TColor(0, 0, 255), SHOW_FEAT_IDS,
SHOW_RESPONSES);
theImg.textOut(
3, 22,
format("# feats: %u", (unsigned int)trackedFeats.size()),
TColor(200, 20, 20));
}
// Update visualization ---------------------------------------
// Show intensity image
win3D.get3DSceneAndLock();
viewInt->setImageView(theImg);
win3D.unlockAccess3DScene();
// Show 3D points & current visible feats, at the current camera 3D
// pose "currentCamPose_wrt_last"
// ---------------------------------------------------------------------
if (last_obs->hasPoints3D)
{
const CPose3D curGlobalPose =
CPose3D(*camera_key_frames_path.rbegin()) +
currentCamPose_wrt_last;
win3D.get3DSceneAndLock();
// All 3D points:
gl_points->loadFromPointsMap(&localPntsMap);
gl_points->setPose(curGlobalPose);
gl_cur_cam_corner->setPose(curGlobalPose);
// Current visual landmarks:
gl_curFeats->clear();
for (map<TFeatureID, TPoint3D>::const_iterator it =
curVisibleFeats.begin();
it != curVisibleFeats.end(); ++it)
{
static double D = 0.02;
mrpt::opengl::CBox::Ptr box =
mrpt::make_aligned_shared<mrpt::opengl::CBox>(
TPoint3D(-D, -D, -D), TPoint3D(D, D, D));
box->setWireframe(true);
box->setName(format("%d", int(it->first)));
box->enableShowName(true);
box->setLocation(it->second);
gl_curFeats->insert(box);
}
gl_curFeats->setPose(curGlobalPose);
win3D.unlockAccess3DScene();
win3D.repaint();
}
win3D.get3DSceneAndLock();
win3D.addTextMessage(
-100, -20, format("%.02f Hz", thrPar.Hz), TColorf(0, 1, 1), 100,
MRPT_GLUT_BITMAP_HELVETICA_18);
win3D.unlockAccess3DScene();
win3D.repaint();
step_num++;
} // end update visualization:
if (gl_keyframes_must_refresh)
{
gl_keyframes_must_refresh = false;
// cout << "Updating gl_keyframes with " <<
// camera_key_frames_path.size() << " frames.\n";
win3D.get3DSceneAndLock();
gl_keyframes->clear();
for (size_t i = 0; i < camera_key_frames_path.size(); i++)
{
CSetOfObjects::Ptr obj =
mrpt::opengl::stock_objects::CornerXYZSimple(0.3f, 3);
obj->setPose(camera_key_frames_path[i]);
gl_keyframes->insert(obj);
}
win3D.unlockAccess3DScene();
}
// Process possible keyboard commands:
// --------------------------------------
if (win3D.keyHit() && thrPar.pushed_key == 0)
{
const int key = tolower(win3D.getPushedKey());
switch (key)
{
// Some of the keys are processed in this thread:
case 'r':
lastVisibleFeats.clear();
camera_key_frames_path.clear();
gl_keyframes_must_refresh = true;
globalPtsMap.clear();
win3D.get3DSceneAndLock();
gl_points_map->loadFromPointsMap(&globalPtsMap);
win3D.unlockAccess3DScene();
break;
case 's':
{
const std::string s = "point_cloud.txt";
cout << "Dumping 3D point-cloud to: " << s << endl;
globalPtsMap.save3D_to_text_file(s);
break;
}
case 'o':
win3D.setCameraZoom(win3D.getCameraZoom() * 1.2);
win3D.repaint();
break;
case 'i':
win3D.setCameraZoom(win3D.getCameraZoom() / 1.2);
win3D.repaint();
break;
// ...and the rest in the kinect thread:
default:
thrPar.pushed_key = key;
break;
};
}
win3D.get3DSceneAndLock();
win3D.addTextMessage(
2, -30, format(
"'s':save point cloud, 'r': reset, 'o'/'i': zoom "
"out/in, mouse: orbit 3D, ESC: quit"),
TColorf(1, 1, 1), 110, MRPT_GLUT_BITMAP_HELVETICA_12);
win3D.addTextMessage(
2, -50, str_status, TColorf(1, 1, 1), 111,
MRPT_GLUT_BITMAP_HELVETICA_12);
win3D.addTextMessage(
2, -70, str_status2, TColorf(1, 1, 1), 112,
MRPT_GLUT_BITMAP_HELVETICA_18);
win3D.unlockAccess3DScene();
std::this_thread::sleep_for(1ms);
}
cout << "Waiting for grabbing thread to exit...\n";
thrPar.quit = true;
thHandle.join();
cout << "Bye!\n";
}
/*---------------------------------------------------------------
path_from_rtk_gps
---------------------------------------------------------------*/
void mrpt::topography::path_from_rtk_gps(
mrpt::poses::CPose3DInterpolator &robot_path,
const mrpt::slam::CRawlog &rawlog,
size_t first,
size_t last,
bool isGUI,
bool disableGPSInterp,
int PATH_SMOOTH_FILTER ,
TPathFromRTKInfo *outInfo
)
{
MRPT_START
#if MRPT_HAS_WXWIDGETS
// Use a smart pointer so we are safe against exceptions:
stlplus::smart_ptr<wxBusyCursor> waitCursorPtr;
if (isGUI)
waitCursorPtr = stlplus::smart_ptr<wxBusyCursor>( new wxBusyCursor() );
#endif
// Go: generate the map:
size_t i;
ASSERT_(first<=last);
ASSERT_(last<=rawlog.size()-1);
set<string> lstGPSLabels;
size_t count = 0;
robot_path.clear();
robot_path.setMaxTimeInterpolation(3.0); // Max. seconds of GPS blackout not to interpolate.
robot_path.setInterpolationMethod( CPose3DInterpolator::imSSLSLL );
TPathFromRTKInfo outInfoTemp;
if (outInfo) *outInfo = outInfoTemp;
map<string, map<TTimeStamp,TPoint3D> > gps_paths; // label -> (time -> 3D local coords)
bool abort = false;
bool ref_valid = false;
// Load configuration block:
CConfigFileMemory memFil;
rawlog.getCommentTextAsConfigFile(memFil);
TGeodeticCoords ref(
memFil.read_double("GPS_ORIGIN","lat_deg",0),
memFil.read_double("GPS_ORIGIN","lon_deg",0),
memFil.read_double("GPS_ORIGIN","height",0) );
ref_valid = !ref.isClear();
// Do we have info for the consistency test?
const double std_0 = memFil.read_double("CONSISTENCY_TEST","std_0",0);
bool doConsistencyCheck = std_0>0;
// Do we have the "reference uncertainty" matrix W^\star ??
memFil.read_matrix("UNCERTAINTY","W_star",outInfoTemp.W_star);
const bool doUncertaintyCovs = size(outInfoTemp.W_star,1)!=0;
if (doUncertaintyCovs && (size(outInfoTemp.W_star,1)!=6 || size(outInfoTemp.W_star,2)!=6))
THROW_EXCEPTION("ERROR: W_star matrix for uncertainty estimation is provided but it's not a 6x6 matrix.");
// ------------------------------------------
// Look for the 2 observations:
// ------------------------------------------
#if MRPT_HAS_WXWIDGETS
wxProgressDialog *progDia=NULL;
if (isGUI)
{
progDia = new wxProgressDialog(
wxT("Building map"),
wxT("Getting GPS observations..."),
(int)(last-first+1), // range
NULL, // parent
wxPD_CAN_ABORT |
wxPD_APP_MODAL |
wxPD_SMOOTH |
wxPD_AUTO_HIDE |
wxPD_ELAPSED_TIME |
wxPD_ESTIMATED_TIME |
wxPD_REMAINING_TIME);
}
#endif
// The list with all time ordered gps's in valid RTK mode
typedef std::map< mrpt::system::TTimeStamp, std::map<std::string,CObservationGPSPtr> > TListGPSs;
TListGPSs list_gps_obs;
map<string,size_t> GPS_RTK_reads; // label-># of RTK readings
map<string,TPoint3D> GPS_local_coords_on_vehicle; // label -> local pose on the vehicle
for (i=first;!abort && i<=last;i++)
{
switch ( rawlog.getType(i) )
{
default:
break;
case CRawlog::etObservation:
{
CObservationPtr o = rawlog.getAsObservation(i);
if (o->GetRuntimeClass()==CLASS_ID(CObservationGPS))
{
CObservationGPSPtr obs = CObservationGPSPtr(o);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
{
// Add to the list:
list_gps_obs[obs->timestamp][obs->sensorLabel] = obs;
lstGPSLabels.insert(obs->sensorLabel);
}
// Save to GPS paths:
if (obs->has_GGA_datum && (obs->GGA_datum.fix_quality==4 || obs->GGA_datum.fix_quality==5))
{
GPS_RTK_reads[obs->sensorLabel]++;
// map<string,TPoint3D> GPS_local_coords_on_vehicle; // label -> local pose on the vehicle
if (GPS_local_coords_on_vehicle.find(obs->sensorLabel)==GPS_local_coords_on_vehicle.end())
GPS_local_coords_on_vehicle[obs->sensorLabel] = TPoint3D( obs->sensorPose );
//map<string, map<TTimeStamp,TPoint3D> > gps_paths; // label -> (time -> 3D local coords)
gps_paths[obs->sensorLabel][obs->timestamp] = TPoint3D(
obs->GGA_datum.longitude_degrees,
obs->GGA_datum.latitude_degrees,
obs->GGA_datum.altitude_meters );
}
}
}
break;
} // end switch type
// Show progress:
if ((count++ % 100)==0)
{
#if MRPT_HAS_WXWIDGETS
if (progDia)
{
if (!progDia->Update((int)(i-first)))
abort = true;
wxTheApp->Yield();
}
#endif
}
} // end for i
#if MRPT_HAS_WXWIDGETS
if (progDia)
{
delete progDia;
progDia=NULL;
}
#endif
// -----------------------------------------------------------
// At this point we already have the sensor positions, thus
// we can estimate the covariance matrix D:
//
// TODO: Generalize equations for # of GPS > 3
// -----------------------------------------------------------
map< set<string>, double > Ad_ij; // InterGPS distances in 2D
map< set<string>, double > phi_ij; // Directions on XY of the lines between i-j
map< string, size_t> D_cov_indexes; // Sensor label-> index in the matrix (first=0, ...)
map< size_t, string> D_cov_rev_indexes; // Reverse of D_cov_indexes
CMatrixDouble D_cov; // square distances cov
CMatrixDouble D_cov_1; // square distances cov (inverse)
vector_double D_mean; // square distances mean
if (doConsistencyCheck && GPS_local_coords_on_vehicle.size()==3)
{
unsigned int cnt = 0;
for(map<string,TPoint3D>::iterator i=GPS_local_coords_on_vehicle.begin();i!=GPS_local_coords_on_vehicle.end();++i)
{
// Index tables:
D_cov_indexes[i->first] = cnt;
D_cov_rev_indexes[cnt] = i->first;
cnt++;
for(map<string,TPoint3D>::iterator j=i;j!=GPS_local_coords_on_vehicle.end();++j)
{
if (i!=j)
{
const TPoint3D &pi = i->second;
const TPoint3D &pj = j->second;
Ad_ij[ make_set(i->first,j->first) ] = pi.distanceTo( pj );
phi_ij[ make_set(i->first,j->first) ] = atan2( pj.y-pi.y, pj.x-pi.x );
}
}
}
ASSERT_( D_cov_indexes.size()==3 && D_cov_rev_indexes.size()==3 );
D_cov.setSize( D_cov_indexes.size(), D_cov_indexes.size() );
D_mean.resize( D_cov_indexes.size() );
// See paper for the formulas!
// TODO: generalize for N>3
D_cov(0,0) = 2*square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] );
D_cov(1,1) = 2*square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] );
D_cov(2,2) = 2*square( Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] );
D_cov(1,0) = Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] * Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])]
* cos( phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] - phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] );
D_cov(0,1) = D_cov(1,0);
D_cov(2,0) =-Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] * Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])]
* cos( phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] - phi_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] );
D_cov(0,2) = D_cov(2,0);
D_cov(2,1) = Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] * Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])]
* cos( phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] - phi_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] );
D_cov(1,2) = D_cov(2,1);
D_cov *= 4*square(std_0);
D_cov_1 = D_cov.inv();
//cout << D_cov.inMatlabFormat() << endl;
D_mean[0] = square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] );
D_mean[1] = square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] );
D_mean[2] = square( Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] );
}
else doConsistencyCheck =false;
// ------------------------------------------
// Look for the 2 observations:
// ------------------------------------------
int N_GPSs = list_gps_obs.size();
if (N_GPSs)
{
// loop interpolate 1-out-of-5: this solves the issue with JAVAD GPSs
// that skip some readings at some times .0 .2 .4 .6 .8
if (list_gps_obs.size()>4)
{
TListGPSs::iterator F = list_gps_obs.begin();
++F; ++F;
TListGPSs::iterator E = list_gps_obs.end();
--E; --E;
for (TListGPSs::iterator i=F;i!=E;++i)
{
// Now check if we have 3 gps with the same time stamp:
//const size_t N = i->second.size();
std::map<std::string, CObservationGPSPtr> & GPS = i->second;
// Check if any in "lstGPSLabels" is missing here:
for (set<string>::iterator l=lstGPSLabels.begin();l!=lstGPSLabels.end();++l)
{
// For each GPS in the current timestamp:
bool fnd = ( GPS.find(*l)!=GPS.end() );
if (fnd) continue; // this one is present.
// Ok, we have "*l" missing in the set "*i".
// Try to interpolate from neighbors:
TListGPSs::iterator i_b1 = i; --i_b1;
TListGPSs::iterator i_a1 = i; ++i_a1;
CObservationGPSPtr GPS_b1, GPS_a1;
if (i_b1->second.find( *l )!=i_b1->second.end())
GPS_b1 = i_b1->second.find( *l )->second;
if (i_a1->second.find( *l )!=i_a1->second.end())
GPS_a1 = i_a1->second.find( *l )->second;
if (!disableGPSInterp && GPS_a1 && GPS_b1)
{
if ( mrpt::system::timeDifference( GPS_b1->timestamp, GPS_a1->timestamp ) < 0.5 )
{
CObservationGPSPtr new_gps = CObservationGPSPtr( new CObservationGPS(*GPS_a1) );
new_gps->sensorLabel = *l;
//cout << mrpt::system::timeLocalToString(GPS_b1->timestamp) << " " << mrpt::system::timeLocalToString(GPS_a1->timestamp) << " " << *l;
//cout << endl;
new_gps->GGA_datum.longitude_degrees = 0.5 * ( GPS_a1->GGA_datum.longitude_degrees + GPS_b1->GGA_datum.longitude_degrees );
new_gps->GGA_datum.latitude_degrees = 0.5 * ( GPS_a1->GGA_datum.latitude_degrees + GPS_b1->GGA_datum.latitude_degrees );
new_gps->GGA_datum.altitude_meters = 0.5 * ( GPS_a1->GGA_datum.altitude_meters + GPS_b1->GGA_datum.altitude_meters );
new_gps->timestamp = (GPS_a1->timestamp + GPS_b1->timestamp) / 2;
i->second[new_gps->sensorLabel] = new_gps;
}
}
}
} // end loop interpolate 1-out-of-5
}
#if MRPT_HAS_WXWIDGETS
wxProgressDialog *progDia3=NULL;
if (isGUI)
{
progDia3 = new wxProgressDialog(
wxT("Building map"),
wxT("Estimating 6D path..."),
N_GPSs, // range
NULL, // parent
wxPD_CAN_ABORT |
wxPD_APP_MODAL |
wxPD_SMOOTH |
wxPD_AUTO_HIDE |
wxPD_ELAPSED_TIME |
wxPD_ESTIMATED_TIME |
wxPD_REMAINING_TIME);
}
#endif
int idx_in_GPSs = 0;
for (TListGPSs::iterator i=list_gps_obs.begin();i!=list_gps_obs.end();++i, idx_in_GPSs++)
{
// Now check if we have 3 gps with the same time stamp:
if (i->second.size()>=3)
{
const size_t N = i->second.size();
std::map<std::string, CObservationGPSPtr> & GPS = i->second;
vector_double X(N),Y(N),Z(N); // Global XYZ coordinates
std::map<string,size_t> XYZidxs; // Sensor label -> indices in X Y Z
if (!ref_valid) // get the reference lat/lon, if it's not set from rawlog configuration block.
{
ref_valid = true;
ref = GPS.begin()->second->GGA_datum.getAsStruct<TGeodeticCoords>();
}
// Compute the XYZ coordinates of all sensors:
TMatchingPairList corrs;
unsigned int k;
std::map<std::string, CObservationGPSPtr>::iterator g_it;
for (k=0,g_it=GPS.begin();g_it!=GPS.end();g_it++,k++)
{
TPoint3D P;
mrpt::topography::geodeticToENU_WGS84(
g_it->second->GGA_datum.getAsStruct<TGeodeticCoords>(),
P,
ref );
// Correction of offsets:
const string sect = string("OFFSET_")+g_it->second->sensorLabel;
P.x += memFil.read_double( sect, "x", 0 );
P.y += memFil.read_double( sect, "y", 0 );
P.z += memFil.read_double( sect, "z", 0 );
XYZidxs[g_it->second->sensorLabel] = k; // Save index correspondence
// Create the correspondence:
corrs.push_back( TMatchingPair(
k,k, // Indices
P.x,P.y,P.z, // "This"/Global coords
g_it->second->sensorPose.x(),g_it->second->sensorPose.y(),g_it->second->sensorPose.z() // "other"/local coordinates
));
X[k] = P.x;
Y[k] = P.y;
Z[k] = P.z;
}
if (doConsistencyCheck && GPS.size()==3)
{
// XYZ[k] have the k'd final coordinates of each GPS
// GPS[k] are the CObservations:
// Compute the inter-GPS square distances:
vector_double iGPSdist2(3);
// [0]: sq dist between: D_cov_rev_indexes[0],D_cov_rev_indexes[1]
TPoint3D P0( X[XYZidxs[D_cov_rev_indexes[0]]], Y[XYZidxs[D_cov_rev_indexes[0]]], Z[XYZidxs[D_cov_rev_indexes[0]]] );
TPoint3D P1( X[XYZidxs[D_cov_rev_indexes[1]]], Y[XYZidxs[D_cov_rev_indexes[1]]], Z[XYZidxs[D_cov_rev_indexes[1]]] );
TPoint3D P2( X[XYZidxs[D_cov_rev_indexes[2]]], Y[XYZidxs[D_cov_rev_indexes[2]]], Z[XYZidxs[D_cov_rev_indexes[2]]] );
iGPSdist2[0] = P0.sqrDistanceTo(P1);
iGPSdist2[1] = P0.sqrDistanceTo(P2);
iGPSdist2[2] = P1.sqrDistanceTo(P2);
double mahaD = mrpt::math::mahalanobisDistance( iGPSdist2, D_mean, D_cov_1 );
outInfoTemp.mahalabis_quality_measure[i->first] = mahaD;
//cout << "x: " << iGPSdist2 << " MU: " << D_mean << " -> " << mahaD << endl;
} // end consistency
// Use a 6D matching method to estimate the location of the vehicle:
CPose3D optimal_pose;
double optimal_scale;
// "this" (reference map) -> GPS global coordinates
// "other" -> GPS local coordinates on the vehicle
mrpt::scanmatching::leastSquareErrorRigidTransformation6D( corrs,optimal_pose,optimal_scale, true ); // Force scale=1
//cout << "optimal pose: " << optimal_pose << " " << optimal_scale << endl;
// Final vehicle pose:
CPose3D &veh_pose= optimal_pose;
// Add to the interpolator:
MRPT_CHECK_NORMAL_NUMBER( veh_pose.x() );
MRPT_CHECK_NORMAL_NUMBER( veh_pose.y() );
MRPT_CHECK_NORMAL_NUMBER( veh_pose.z() );
MRPT_CHECK_NORMAL_NUMBER( veh_pose.yaw() );
MRPT_CHECK_NORMAL_NUMBER( veh_pose.pitch() );
MRPT_CHECK_NORMAL_NUMBER( veh_pose.roll() );
robot_path.insert( i->first, veh_pose );
// If we have W_star, compute the pose uncertainty:
if (doUncertaintyCovs)
{
CPose3DPDFGaussian final_veh_uncert;
final_veh_uncert.mean.setFromValues(0,0,0,0,0,0);
final_veh_uncert.cov = outInfoTemp.W_star;
// Rotate the covariance according to the real vehicle pose:
final_veh_uncert.changeCoordinatesReference(veh_pose);
outInfoTemp.vehicle_uncertainty[ i->first ] = final_veh_uncert.cov;
}
}
// Show progress:
if ((count++ % 100)==0)
{
#if MRPT_HAS_WXWIDGETS
if (progDia3)
{
if (!progDia3->Update(idx_in_GPSs))
abort = true;
wxTheApp->Yield();
}
#endif
}
} // end for i
#if MRPT_HAS_WXWIDGETS
if (progDia3)
{
delete progDia3;
progDia3=NULL;
}
#endif
if (PATH_SMOOTH_FILTER>0 && robot_path.size()>1)
{
CPose3DInterpolator filtered_robot_path = robot_path;
// Do Angles smoother filter of the path:
// ---------------------------------------------
const double MAX_DIST_TO_FILTER = 4.0;
for (CPose3DInterpolator::iterator i=robot_path.begin();i!=robot_path.end();++i)
{
CPose3D p = i->second;
vector_double pitchs, rolls; // The elements to average
pitchs.push_back(p.pitch());
rolls.push_back(p.roll());
CPose3DInterpolator::iterator q=i;
for (int k=0;k<PATH_SMOOTH_FILTER && q!=robot_path.begin();k++)
{
--q;
if (abs( mrpt::system::timeDifference(q->first,i->first))<MAX_DIST_TO_FILTER )
{
pitchs.push_back( q->second.pitch() );
rolls.push_back( q->second.roll() );
}
}
q=i;
for (int k=0;k<PATH_SMOOTH_FILTER && q!=(--robot_path.end()) ;k++)
{
++q;
if (abs( mrpt::system::timeDifference(q->first,i->first))<MAX_DIST_TO_FILTER )
{
pitchs.push_back( q->second.pitch() );
rolls.push_back( q->second.roll() );
}
}
p.setYawPitchRoll(p.yaw(), mrpt::math::averageWrap2Pi(pitchs), mrpt::math::averageWrap2Pi(rolls) );
// save in filtered path:
filtered_robot_path.insert( i->first, p );
}
// Replace:
robot_path = filtered_robot_path;
} // end PATH_SMOOTH_FILTER
} // end step generate 6D path
// Here we can set best_gps_path (that with the max. number of RTK fixed/foat readings):
if (outInfo)
{
string bestLabel;
size_t bestNum = 0;
for (map<string,size_t>::iterator i=GPS_RTK_reads.begin();i!=GPS_RTK_reads.end();++i)
{
if (i->second>bestNum)
{
bestNum = i->second;
bestLabel = i->first;
}
}
outInfoTemp.best_gps_path = gps_paths[bestLabel];
// and transform to XYZ:
// Correction of offsets:
const string sect = string("OFFSET_")+bestLabel;
const double off_X = memFil.read_double( sect, "x", 0 );
const double off_Y = memFil.read_double( sect, "y", 0 );
const double off_Z = memFil.read_double( sect, "z", 0 );
// map<TTimeStamp,TPoint3D> best_gps_path; // time -> 3D local coords
for (map<TTimeStamp,TPoint3D>::iterator i=outInfoTemp.best_gps_path.begin();i!=outInfoTemp.best_gps_path.end();++i)
{
TPoint3D P;
mrpt::topography::geodeticToENU_WGS84(
TGeodeticCoords(i->second.x,i->second.y,i->second.z), // i->second.x,i->second.y,i->second.z, // lat, lon, heigh
P, // X Y Z
ref );
i->second.x = P.x + off_X;
i->second.y = P.y + off_Y;
i->second.z = P.z + off_Z;
}
} // end best_gps_path
if (outInfo) *outInfo = outInfoTemp;
MRPT_END
}
