// return false on any error.
bool processOneObservation(CObservationPtr &o)
{
if (!IS_CLASS(o, CObservationOdometry ) )
return true;
CObservationOdometry* obs = CObservationOdometryPtr(o).pointer();
if (!obs->hasEncodersInfo)
throw std::runtime_error("CObservationOdometry entry does not contain encoder info, cannot recalculate odometry!");
CActionRobotMovement2D auxOdoIncr;
auxOdoIncr.hasEncodersInfo = true;
auxOdoIncr.encoderLeftTicks = obs->encoderLeftTicks;
auxOdoIncr.encoderRightTicks = obs->encoderRightTicks;
auxOdoIncr.computeFromEncoders( KL,KR, D );
if (!m_odo_accum_valid)
{
m_odo_accum_valid=true;
m_odo_accum = obs->odometry;
// and don't modify this odo val.
}
else
{
obs->odometry = m_odo_accum + auxOdoIncr.rawOdometryIncrementReading;
m_odo_accum = obs->odometry;
}
m_entriesSaved++;
return true; // All ok
}
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!";
}
}
/** The PF algorithm implementation for "optimal sampling for non-parametric observation models"
*/
void CLSLAM_RBPF_2DLASER::prediction_and_update_pfAuxiliaryPFOptimal(
CLocalMetricHypothesis *LMH,
const mrpt::slam::CActionCollection * actions,
const mrpt::slam::CSensoryFrame * sf,
const bayes::CParticleFilter::TParticleFilterOptions &PF_options )
{
MRPT_START
// Get the current robot pose estimation:
TPoseID currentPoseID = LMH->m_currentRobotPose;
size_t i,k,N,M = LMH->m_particles.size();
// ----------------------------------------------------------------------
// We can execute optimal PF only when we have both, an action, and
// a valid observation from which to compute the likelihood:
// Accumulate odometry/actions until we have a valid observation, then
// process them simultaneously.
// ----------------------------------------------------------------------
// static CActionRobotMovement2D accumRobotMovement;
// static bool accumRobotMovementIsValid = false;
bool SFhasValidObservations = false;
// A valid action?
if (actions!=NULL)
{
CActionRobotMovement2DPtr act = actions->getBestMovementEstimation(); // Find a robot movement estimation:
if (!act) THROW_EXCEPTION("Action list does not contain any CActionRobotMovement2D derived object!");
if (!LMH->m_accumRobotMovementIsValid) // Reset accum.
{
act->poseChange->getMean( LMH->m_accumRobotMovement.rawOdometryIncrementReading );
LMH->m_accumRobotMovement.motionModelConfiguration = act->motionModelConfiguration;
}
else
LMH->m_accumRobotMovement.rawOdometryIncrementReading =
LMH->m_accumRobotMovement.rawOdometryIncrementReading +
act->poseChange->getMeanVal();
LMH->m_accumRobotMovementIsValid = true;
}
if (sf!=NULL)
{
ASSERT_(LMH->m_particles.size()>0);
SFhasValidObservations = (*LMH->m_particles.begin()).d->metricMaps.canComputeObservationsLikelihood( *sf );
}
// All the needed things?
if (!LMH->m_accumRobotMovementIsValid || !SFhasValidObservations)
return; // Nothing we can do here...
// OK, we have all we need, let's start!
// Take the accum. actions as input:
CActionRobotMovement2D theResultingRobotMov;
// Over
keep_max(
LMH->m_accumRobotMovement.motionModelConfiguration.gausianModel.minStdXY,
LMH->m_parent->m_options.MIN_ODOMETRY_STD_XY);
keep_max(
LMH->m_accumRobotMovement.motionModelConfiguration.gausianModel.minStdPHI,
LMH->m_parent->m_options.MIN_ODOMETRY_STD_PHI);
theResultingRobotMov.computeFromOdometry( LMH->m_accumRobotMovement.rawOdometryIncrementReading, LMH->m_accumRobotMovement.motionModelConfiguration );
const CActionRobotMovement2D *robotMovement = &theResultingRobotMov;
LMH->m_accumRobotMovementIsValid = false; // To reset odometry at next iteration!
// ----------------------------------------------------------------------
// 0) Common part: Prepare m_particles "draw" and compute
// ----------------------------------------------------------------------
// Precompute a list of "random" samples from the movement model:
LMH->m_movementDraws.clear();
// Fast pseudorandom generator of poses...
//m_movementDraws.resize( max(2000,(int)(PF_options.pfAuxFilterOptimal_MaximumSearchSamples * 5.6574) ) );
LMH->m_movementDraws.resize( PF_options.pfAuxFilterOptimal_MaximumSearchSamples * M );
size_t size_movementDraws = LMH->m_movementDraws.size();
LMH->m_movementDrawsIdx = (unsigned int)floor(randomGenerator.drawUniform(0.0f,((float)size_movementDraws)-0.01f));
robotMovement->prepareFastDrawSingleSamples();
for (size_t i=0;i<LMH->m_movementDraws.size();i++)
robotMovement->fastDrawSingleSample( LMH->m_movementDraws[i] );
LMH->m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
LMH->m_maxLikelihood.clear();
LMH->m_maxLikelihood.resize(M,0);
LMH->m_movementDrawMaximumLikelihood.resize(M);
// Prepare data for executing "fastDrawSample"
CTicTac tictac;
tictac.Tic();
LMH->prepareFastDrawSample(
PF_options,
particlesEvaluator_AuxPFOptimal,
robotMovement,
sf );
printf("[prepareFastDrawSample] Done in %.06f ms\n",tictac.Tac()*1e3f);
#if 0
printf("[prepareFastDrawSample] max (log) = %10.06f\n", math::maximum(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) );
printf("[prepareFastDrawSample] max-mean (log) = %10.06f\n", -math::mean(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) + math::maximum(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) );
printf("[prepareFastDrawSample] max-min (log) = %10.06f\n", -math::minimum(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) + math::maximum(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) );
#endif
// Now we have the vector "m_fastDrawProbability" filled out with:
// w[i]·p(zt|z^{t-1},x^{[i],t-1},X)
// where X is the robot pose prior (as implemented in
// the aux. function "particlesEvaluator_AuxPFOptimal"),
// and also the "m_maxLikelihood" filled with the maximum lik. values.
StdVector_CPose2D newParticles;
vector<double> newParticlesWeight;
vector<size_t> newParticlesDerivedFromIdx;
// We need the (aproximate) maximum likelihood value for each
// previous particle [i]:
//
// max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }
//
//CVectorDouble maxLikelihood(M, -1 );
float MIN_ACCEPT_UNIF_DISTRIB = 0.00f;
CPose2D movementDraw,newPose,oldPose;
double acceptanceProb,newPoseLikelihood,ratioLikLik;
unsigned int statsTrialsCount = 0, statsTrialsSuccess = 0;
std::vector<bool> maxLikMovementDrawHasBeenUsed(M, false);
unsigned int statsWarningsAccProbAboveOne = 0;
//double maxMeanLik = math::maximum( LMH->m_pfAuxiliaryPFOptimal_estimatedProb ); // For normalization purposes only
ASSERT_(!PF_options.adaptiveSampleSize);
// ----------------------------------------------------------------------
// 1) FIXED SAMPLE SIZE VERSION
// ----------------------------------------------------------------------
newParticles.resize(M);
newParticlesWeight.resize(M);
newParticlesDerivedFromIdx.resize(M);
bool doResample = LMH->ESS() < 0.5;
for (i=0;i<M;i++)
{
// Generate a new particle:
// (a) Draw a "t-1" m_particles' index:
// ----------------------------------------------------------------
if (doResample)
k = LMH->fastDrawSample(PF_options); // Based on weights of last step only!
else k = i;
oldPose = CPose2D( *LMH->getCurrentPose(k) );
// (b) Rejection-sampling: Draw a new robot pose from x[k],
// and accept it with probability p(zk|x) / maxLikelihood:
// ----------------------------------------------------------------
if ( LMH->m_SFs.empty() )
{
// The first robot pose in the SLAM execution: All m_particles start
// at the same point (this is the lowest bound of subsequent uncertainty):
movementDraw = CPose2D(0,0,0);
newPose = oldPose; // + movementDraw;
}
else
{
// Rejection-sampling:
do
{
// Draw new robot pose:
if (!maxLikMovementDrawHasBeenUsed[k])
{
// No! first take advantage of a good drawn value, but only once!!
maxLikMovementDrawHasBeenUsed[k] = true;
movementDraw = LMH->m_movementDrawMaximumLikelihood[k];
#if 0
cout << "Drawn pose (max. lik): " << movementDraw << endl;
#endif
}
else
{
// Draw new robot pose:
//robotMovement->drawSingleSample( movementDraw );
//robotMovement->fastDrawSingleSample( movementDraw );
movementDraw = LMH->m_movementDraws[ LMH->m_movementDrawsIdx++ % size_movementDraws];
}
newPose.composeFrom( oldPose, movementDraw ); // newPose = oldPose + movementDraw;
// Compute acceptance probability:
newPoseLikelihood = auxiliarComputeObservationLikelihood(PF_options, LMH,k,sf,&newPose);
ratioLikLik = exp( newPoseLikelihood - LMH->m_maxLikelihood[k] );
acceptanceProb = min( 1.0, ratioLikLik );
if ( ratioLikLik > 1)
{
if (ratioLikLik>1.5)
{
statsWarningsAccProbAboveOne++;
// DEBUG
//printf("[pfAuxiliaryPFOptimal] Warning!! p(z|x)/p(z|x*)=%f\n",ratioLikLik);
}
LMH->m_maxLikelihood[k] = newPoseLikelihood; // :'-( !!!
acceptanceProb = 0; // Keep searching!!
}
statsTrialsCount++; // Stats
} while ( acceptanceProb < randomGenerator.drawUniform(MIN_ACCEPT_UNIF_DISTRIB,0.999f) );
statsTrialsSuccess++; // Stats:
}
// Insert the new particle!:
newParticles[i] = newPose;
// And its weight:
double weightFact = LMH->m_pfAuxiliaryPFOptimal_estimatedProb[k] * PF_options.powFactor;
// Add to historic record of log_w weights:
LMH->m_log_w_metric_history.resize(M);
LMH->m_log_w_metric_history[i][ currentPoseID] += weightFact;
if (doResample)
newParticlesWeight[i] = 0;
else newParticlesWeight[i] = LMH->m_particles[k].log_w + weightFact;
// and its heritance:
newParticlesDerivedFromIdx[i] = (unsigned int)k;
} // for i
// ---------------------------------------------------------------------------------
// Substitute old by new particle set:
// Old are in "m_particles"
// New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"
// ---------------------------------------------------------------------------------
N = newParticles.size();
CLocalMetricHypothesis::CParticleList newParticlesArray(N);
CLocalMetricHypothesis::CParticleList::iterator newPartIt,trgPartIt;
// For efficiency, just copy the "CRBPFParticleData" from the old particle into the
// new one, but this can be done only once:
std::vector<bool> oldParticleAlreadyCopied(LMH->m_particles.size(),false);
CLSLAMParticleData *newPartData;
for (newPartIt=newParticlesArray.begin(),i=0;newPartIt!=newParticlesArray.end();newPartIt++,i++)
{
// The weight:
newPartIt->log_w = newParticlesWeight[i];
// The data (CRBPFParticleData):
if (!oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]])
{
// The first copy of this old particle:
newPartData = LMH->m_particles[ newParticlesDerivedFromIdx[i] ].d;
oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]] = true;
}
else
{
// Make a copy:
newPartData = new CLSLAMParticleData( *LMH->m_particles[ newParticlesDerivedFromIdx[i] ].d );
}
newPartIt->d = newPartData;
} // end for "newPartIt"
// Now add the new robot pose to the paths: (this MUST be done after the above loop, separately):
for (newPartIt=newParticlesArray.begin(),i=0;newPartIt!=newParticlesArray.end();newPartIt++,i++)
newPartIt->d->robotPoses[ LMH->m_currentRobotPose ] = CPose3D( newParticles[i] );
// Free those old m_particles not being copied into the new ones:
for (i=0;i<LMH->m_particles.size();i++)
{
if (!oldParticleAlreadyCopied[i])
delete LMH->m_particles[ i ].d;
// And set all to NULL, so don't try to delete them below:
LMH->m_particles[ i ].d = NULL;
}
// Copy into "m_particles":
LMH->m_particles.resize( newParticlesArray.size() );
for (newPartIt=newParticlesArray.begin(),trgPartIt=LMH->m_particles.begin(); newPartIt!=newParticlesArray.end(); newPartIt++, trgPartIt++ )
{
trgPartIt->log_w = newPartIt->log_w;
trgPartIt->d = newPartIt->d;
newPartIt->d = NULL;
}
// Free buffers:
newParticles.clear();
newParticlesArray.clear();
newParticlesWeight.clear();
newParticlesDerivedFromIdx.clear();
double out_max_log_w;
LMH->normalizeWeights( &out_max_log_w ); // Normalize weights:
LMH->m_log_w += out_max_log_w;
#if 0
printf("[REJ-SAMP.RATIO: \t%.03f%% \t %u out of %u with P(acep)>1]\n\n",
100.0f*statsTrialsSuccess / (float)(max(1u,statsTrialsCount)),
statsWarningsAccProbAboveOne,
statsTrialsCount
);
#endif
MRPT_END
}
/*---------------------------------------------------------------
processActionObservation
---------------------------------------------------------------*/
void CMetricMapBuilderRBPF::processActionObservation(
CActionCollection &action,
CSensoryFrame &observations )
{
MRPT_START
// Enter critical section (updating map)
enterCriticalSection();
// Update the traveled distance estimations:
{
CActionRobotMovement3DPtr act3D = action.getActionByClass<CActionRobotMovement3D>();
CActionRobotMovement2DPtr act2D = action.getActionByClass<CActionRobotMovement2D>();
if (act3D)
{
odoIncrementSinceLastMapUpdate += act3D->poseChange.getMeanVal();
odoIncrementSinceLastLocalization += act3D->poseChange;
}
else if (act2D)
{
odoIncrementSinceLastMapUpdate += act2D->poseChange->getMeanVal();
odoIncrementSinceLastLocalization.mean += act2D->poseChange->getMeanVal();
}
else
{
std::cerr << "[CMetricMapBuilderRBPF] WARNING: action contains no odometry." << std::endl;
}
}
// Execute particle filter:
// But only if the traveled distance since the last update is long enough,
// or this is the first observation, etc...
// ----------------------------------------------------------------------------
bool do_localization = (
!mapPDF.SFs.size() || // This is the first observation!
options.debugForceInsertion ||
odoIncrementSinceLastLocalization.mean.norm()>localizeLinDistance ||
std::abs(odoIncrementSinceLastLocalization.mean.yaw())>localizeAngDistance);
bool do_map_update = (
!mapPDF.SFs.size() || // This is the first observation!
options.debugForceInsertion ||
odoIncrementSinceLastMapUpdate.norm()>insertionLinDistance ||
std::abs(odoIncrementSinceLastMapUpdate.yaw())>insertionAngDistance);
// Used any "options.alwaysInsertByClass" ??
for (CListOfClasses::const_iterator itCl=options.alwaysInsertByClass.begin();!do_map_update && itCl!=options.alwaysInsertByClass.end();++itCl)
for ( CSensoryFrame::iterator it=observations.begin();it!=observations.end();++it)
if ((*it)->GetRuntimeClass()==*itCl)
{
do_map_update = true;
do_localization = true;
break;
}
if (do_map_update)
do_localization = true;
if (do_localization)
{
// Create an artificial action object, since
// we've been collecting them until a threshold:
// ------------------------------------------------
CActionCollection fakeActs;
{
CActionRobotMovement3DPtr act3D = action.getActionByClass<CActionRobotMovement3D>();
if (act3D)
{
CActionRobotMovement3D newAct;
newAct.estimationMethod = act3D->estimationMethod;
newAct.poseChange = odoIncrementSinceLastLocalization;
newAct.timestamp = act3D->timestamp;
fakeActs.insert(newAct);
}
else
{
// It must be 2D odometry:
CActionRobotMovement2DPtr act2D = action.getActionByClass<CActionRobotMovement2D>();
ASSERT_(act2D)
CActionRobotMovement2D newAct;
newAct.computeFromOdometry( CPose2D(odoIncrementSinceLastLocalization.mean), act2D->motionModelConfiguration );
newAct.timestamp = act2D->timestamp;
fakeActs.insert(newAct);
}
}
// Reset distance counters:
odoIncrementSinceLastLocalization.mean.setFromValues(0,0,0,0,0,0);
odoIncrementSinceLastLocalization.cov.zeros();
CParticleFilter pf;
pf.m_options = m_PF_options;
pf.executeOn( mapPDF, &fakeActs, &observations );
if (options.verbose)
{
// Get current pose estimation:
CPose3DPDFParticles poseEstimation;
CPose3D meanPose;
mapPDF.getEstimatedPosePDF(poseEstimation);
poseEstimation.getMean(meanPose);
CPose3D estPos;
CMatrixDouble66 cov;
poseEstimation.getCovarianceAndMean(cov,estPos);
std::cout << " New pose=" << estPos << std::endl << "New ESS:"<< mapPDF.ESS() << std::endl;
std::cout << format(" STDs: x=%2.3f y=%2.3f z=%.03f yaw=%2.3fdeg\n", sqrt(cov(0,0)),sqrt(cov(1,1)),sqrt(cov(2,2)),RAD2DEG(sqrt(cov(3,3))));
}
}
if (do_map_update)
{
odoIncrementSinceLastMapUpdate.setFromValues(0,0,0,0,0,0);
// Update the particles' maps:
// -------------------------------------------------
if (options.verbose)
printf(" 3) New observation inserted into the map!\n");
// Add current observation to the map:
mapPDF.insertObservation(observations);
m_statsLastIteration.observationsInserted = true;
}
else
{
m_statsLastIteration.observationsInserted = false;
}
// Added 29/JUN/2007 JLBC: Tell all maps that they can now free aux. variables
// (if any) since one PF cycle is over:
for (CMultiMetricMapPDF::CParticleList::iterator it = mapPDF.m_particles.begin(); it!=mapPDF.m_particles.end();++it)
it->d->mapTillNow.auxParticleFilterCleanUp();
leaveCriticalSection(); /* Leaving critical section (updating map) */
MRPT_END_WITH_CLEAN_UP( leaveCriticalSection(); /* Leaving critical section (updating map) */ );
// Convert from observations only to actions-SF format:
void xRawLogViewerFrame::OnMenuConvertSF(wxCommandEvent& event)
{
WX_START_TRY
bool onlyOnePerLabel =
(wxYES == wxMessageBox(
_("Keep only one observation of each label within each "
"sensoryframe?"),
_("Convert to sensoryframe's"), wxYES_NO, this));
wxString strMaxL = wxGetTextFromUser(
_("Maximum length of each sensoryframe (seconds):"),
_("Convert to sensoryframe's"), _("1.0"));
double maxLengthSF;
strMaxL.ToDouble(&maxLengthSF);
// Process:
CRawlog new_rawlog;
new_rawlog.setCommentText(rawlog.getCommentText());
wxBusyCursor waitCursor;
unsigned int nEntries = (unsigned int)rawlog.size();
wxProgressDialog progDia(
wxT("Progress"), wxT("Parsing rawlog..."),
nEntries, // range
this, // parent
wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE |
wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME);
wxTheApp->Yield(); // Let the app. process messages
CSensoryFrame SF_new;
set<string> SF_new_labels;
TTimeStamp SF_new_first_t = INVALID_TIMESTAMP;
CObservationOdometry::Ptr last_sf_odo, cur_sf_odo;
for (unsigned int countLoop = 0; countLoop < nEntries; countLoop++)
{
if (countLoop % 20 == 0)
{
if (!progDia.Update(
countLoop,
wxString::Format(
wxT("Parsing rawlog... %u objects"), countLoop)))
{
return;
}
wxTheApp->Yield(); // Let the app. process messages
}
switch (rawlog.getType(countLoop))
{
case CRawlog::etSensoryFrame:
case CRawlog::etActionCollection:
{
THROW_EXCEPTION(
"The rawlog already has sensory frames and/or actions!");
}
break;
case CRawlog::etObservation:
{
CObservation::Ptr o = rawlog.getAsObservation(countLoop);
// Update stats:
bool label_existed =
SF_new_labels.find(o->sensorLabel) != SF_new_labels.end();
double SF_len =
SF_new_first_t == INVALID_TIMESTAMP
? 0
: timeDifference(SF_new_first_t, o->timestamp);
// Decide:
// End SF and start a new one?
if (SF_len > maxLengthSF && SF_new.size() != 0)
{
new_rawlog.addObservations(SF_new);
// Odometry increments:
CActionCollection acts;
if (last_sf_odo && cur_sf_odo)
{
CActionRobotMovement2D act;
act.timestamp = cur_sf_odo->timestamp;
CActionRobotMovement2D::TMotionModelOptions opts;
act.computeFromOdometry(
cur_sf_odo->odometry - last_sf_odo->odometry, opts);
acts.insert(act);
}
new_rawlog.addActions(acts);
last_sf_odo = cur_sf_odo;
cur_sf_odo.reset();
SF_new.clear();
SF_new_labels.clear();
SF_new_first_t = INVALID_TIMESTAMP;
}
// Insert into SF:
if (!onlyOnePerLabel || !label_existed)
{
SF_new.insert(o);
SF_new_labels.insert(o->sensorLabel);
}
if (SF_new_first_t == INVALID_TIMESTAMP)
SF_new_first_t = o->timestamp;
if (o->GetRuntimeClass() == CLASS_ID(CObservationOdometry))
{
cur_sf_odo =
std::dynamic_pointer_cast<CObservationOdometry>(o);
}
}
break;
default:
break;
} // end for each entry
} // end while keep loading
// Remaining obs:
if (SF_new.size())
{
new_rawlog.addObservations(SF_new);
SF_new.clear();
}
progDia.Update(nEntries);
// Update:
rawlog.moveFrom(new_rawlog);
rebuildTreeView();
WX_END_TRY
}
void xRawLogViewerFrame::OnMenuLossLessDecFILE(wxCommandEvent& event)
{
WX_START_TRY
string filToOpen;
if (!AskForOpenRawlog(filToOpen)) return;
wxString strDecimation = wxGetTextFromUser(
_("The number of observations will be decimated (only 1 out of M will "
"be kept). Enter the decimation ratio M:"),
_("Decimation"), _("1"));
long DECIMATE_RATIO;
strDecimation.ToLong(&DECIMATE_RATIO);
ASSERT_(DECIMATE_RATIO >= 1)
string filToSave;
AskForSaveRawlog(filToSave);
CFileGZInputStream fil(filToOpen);
CFileGZOutputStream f_out(filToSave);
wxBusyCursor waitCursor;
unsigned int filSize = (unsigned int)fil.getTotalBytesCount();
wxString auxStr;
wxProgressDialog progDia(
wxT("Progress"), wxT("Parsing file..."),
filSize, // range
this, // parent
wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE |
wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME);
wxTheApp->Yield(); // Let the app. process messages
unsigned int countLoop = 0;
int entryIndex = 0;
bool keepLoading = true;
string errorMsg;
// ------------------------------------------------------------------------------
// METHOD TO BE MEMORY EFFICIENT:
// To free the memory of the current rawlog entries as we create the new
// one,
// then call "clearWithoutDelete" at the end.
// ------------------------------------------------------------------------------
CSensoryFrame::Ptr accum_sf;
CActionRobotMovement2D::TMotionModelOptions odometryOptions;
bool cummMovementInit = false;
long SF_counter = 0;
// Reset cummulative pose change:
CPose2D accumMovement(0, 0, 0);
TTimeStamp timestamp_lastAction = INVALID_TIMESTAMP;
while (keepLoading)
{
if (countLoop++ % 100 == 0)
{
auxStr.sprintf(wxT("Parsing file... %u objects"), entryIndex);
if (!progDia.Update((int)fil.getPosition(), auxStr))
keepLoading = false;
wxTheApp->Yield(); // Let the app. process messages
}
CSerializable::Ptr newObj;
try
{
fil >> newObj;
entryIndex++;
// Check type:
if (newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
{
// Decimate Observations
// ---------------------------
// Add observations to the accum. SF:
if (!accum_sf)
accum_sf = mrpt::make_aligned_shared<CSensoryFrame>();
// Copy pointers to observations only (fast):
accum_sf->moveFrom(
*std::dynamic_pointer_cast<CSensoryFrame>(newObj));
if (++SF_counter >= DECIMATE_RATIO)
{
SF_counter = 0;
// INSERT OBSERVATIONS:
f_out << *accum_sf;
accum_sf.reset();
// INSERT ACTIONS:
CActionCollection::Ptr actsCol =
mrpt::make_aligned_shared<CActionCollection>();
if (cummMovementInit)
{
CActionRobotMovement2D cummMovement;
cummMovement.computeFromOdometry(
accumMovement, odometryOptions);
cummMovement.timestamp = timestamp_lastAction;
actsCol->insert(cummMovement);
// Reset odometry accumulation:
accumMovement = CPose2D(0, 0, 0);
}
f_out << *actsCol;
actsCol.reset();
}
}
else if (newObj->GetRuntimeClass() == CLASS_ID(CActionCollection))
{
// Accumulate Actions
// ----------------------
CActionCollection::Ptr curActs =
std::dynamic_pointer_cast<CActionCollection>(newObj);
CActionRobotMovement2D::Ptr mov =
curActs->getBestMovementEstimation();
if (mov)
{
timestamp_lastAction = mov->timestamp;
CPose2D incrPose = mov->poseChange->getMeanVal();
// If we have previous observations, shift them according to
// this new increment:
if (accum_sf)
{
CPose3D inv_incrPose3D(
CPose3D(0, 0, 0) - CPose3D(incrPose));
for (CSensoryFrame::iterator it = accum_sf->begin();
it != accum_sf->end(); ++it)
{
CPose3D tmpPose;
(*it)->getSensorPose(tmpPose);
tmpPose = inv_incrPose3D + tmpPose;
(*it)->setSensorPose(tmpPose);
}
}
// Accumulate from odometry:
accumMovement = accumMovement + incrPose;
// Copy the probabilistic options from the first entry we
// find:
if (!cummMovementInit)
{
odometryOptions = mov->motionModelConfiguration;
cummMovementInit = true;
}
}
}
else
{
// Unknown class:
THROW_EXCEPTION("Unknown class found in the file!");
}
}
catch (exception& e)
{
errorMsg = e.what();
keepLoading = false;
}
catch (...)
{
keepLoading = false;
}
} // end while keep loading
progDia.Update(filSize);
wxTheApp->Yield(); // Let the app. process messages
WX_END_TRY
}
void xRawLogViewerFrame::OnMenuLossLessDecimate(wxCommandEvent& event)
{
WX_START_TRY
CRawlog newRawLog;
newRawLog.setCommentText(rawlog.getCommentText());
wxString strDecimation = wxGetTextFromUser(
_("The number of observations will be decimated (only 1 out of M will "
"be kept). Enter the decimation ratio M:"),
_("Decimation"), _("1"));
long DECIMATE_RATIO;
strDecimation.ToLong(&DECIMATE_RATIO);
ASSERT_(DECIMATE_RATIO >= 1)
wxBusyCursor busyCursor;
wxTheApp->Yield(); // Let the app. process messages
size_t i, N = rawlog.size();
// ------------------------------------------------------------------------------
// METHOD TO BE MEMORY EFFICIENT:
// To free the memory of the current rawlog entries as we create the new
// one,
// then call "clearWithoutDelete" at the end.
// ------------------------------------------------------------------------------
CSensoryFrame::Ptr accum_sf;
CActionRobotMovement2D::TMotionModelOptions odometryOptions;
bool cummMovementInit = false;
long SF_counter = 0;
// Reset cummulative pose change:
CPose2D accumMovement(0, 0, 0);
// For each entry:
for (i = 0; i < N; i++)
{
CSerializable::Ptr obj = rawlog.getAsGeneric(i);
if (rawlog.getType(i) == CRawlog::etActionCollection)
{
// Accumulate Actions
// ----------------------
CActionCollection::Ptr curActs =
std::dynamic_pointer_cast<CActionCollection>(obj);
CActionRobotMovement2D::Ptr mov =
curActs->getBestMovementEstimation();
if (mov)
{
CPose2D incrPose = mov->poseChange->getMeanVal();
// If we have previous observations, shift them according to
// this new increment:
if (accum_sf)
{
CPose3D inv_incrPose3D(
CPose3D(0, 0, 0) - CPose3D(incrPose));
for (CSensoryFrame::iterator it = accum_sf->begin();
it != accum_sf->end(); ++it)
{
CPose3D tmpPose;
(*it)->getSensorPose(tmpPose);
tmpPose = inv_incrPose3D + tmpPose;
(*it)->setSensorPose(tmpPose);
}
}
// Accumulate from odometry:
accumMovement = accumMovement + incrPose;
// Copy the probabilistic options from the first entry we find:
if (!cummMovementInit)
{
odometryOptions = mov->motionModelConfiguration;
cummMovementInit = true;
}
}
}
else if (rawlog.getType(i) == CRawlog::etSensoryFrame)
{
// Decimate Observations
// ---------------------------
// Add observations to the accum. SF:
if (!accum_sf)
accum_sf = mrpt::make_aligned_shared<CSensoryFrame>();
// Copy pointers to observations only (fast):
accum_sf->moveFrom(*std::dynamic_pointer_cast<CSensoryFrame>(obj));
if (++SF_counter >= DECIMATE_RATIO)
{
SF_counter = 0;
// INSERT OBSERVATIONS:
newRawLog.addObservationsMemoryReference(accum_sf);
accum_sf.reset();
// INSERT ACTIONS:
CActionCollection actsCol;
if (cummMovementInit)
{
CActionRobotMovement2D cummMovement;
cummMovement.computeFromOdometry(
accumMovement, odometryOptions);
actsCol.insert(cummMovement);
// Reset odometry accumulation:
accumMovement = CPose2D(0, 0, 0);
}
newRawLog.addActions(actsCol);
}
}
else
THROW_EXCEPTION(
"This operation implemented for SF-based rawlogs only.");
// Delete object
obj.reset();
} // end for i each entry
// Clear the list only (objects already deleted)
rawlog.clear();
// Copy as new log:
rawlog = newRawLog;
rebuildTreeView();
WX_END_TRY
}
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-beacons - 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 );
randomGenerator.randomize();
int i;
char auxStr[2000];
// Set default values:
int nBeacons = 3;
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 odometryBias_Y = 0;
float minSensorDistance = 0;
float maxSensorDistance = 10;
float stdError = 0.04f;
bool circularPath = true;
int squarePathLength=40;
float ratio_outliers = 0;
float ratio_outliers_first_step = 0;
float outlier_uniform_min=0;
float outlier_uniform_max=5.0;
// Load params from INI:
MRPT_LOAD_CONFIG_VAR(nBeacons,int, ini,"Params");
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(squarePathLength,int,ini,"Params");
MRPT_LOAD_CONFIG_VAR(ratio_outliers,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(ratio_outliers_first_step,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(outlier_uniform_min,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(outlier_uniform_max,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(min_x,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(max_x,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(min_y,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(max_y,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(min_z,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryNoiseXY_std,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryBias_Y,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(minSensorDistance,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(maxSensorDistance,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(stdError,float, ini,"Params");
// Create out dir:
mrpt::system::createDirectory(outDir);
// ---------------------------------------------
// Create the point-beacons:
// ---------------------------------------------
printf("Creating beacon map...");
mrpt::slam::CBeaconMap beaconMap;
for (i=0;i<nBeacons;i++)
{
CBeacon b;
CPoint3D pt3D;
// Random coordinates:
pt3D.x( randomGenerator.drawUniform(min_x,max_x) );
pt3D.y( randomGenerator.drawUniform(min_y,max_y) );
pt3D.z( randomGenerator.drawUniform(min_z,max_z) );
// Add:
b.m_typePDF=CBeacon::pdfMonteCarlo;
b.m_locationMC.setSize(1,pt3D);
b.m_ID = i;
beaconMap.push_back(b);
}
os::sprintf(auxStr,sizeof(auxStr),"%s/outSimMap.txt",outDir.c_str());
beaconMap.saveToTextFile(auxStr);
printf("Done!\n");
//beaconMap.simulateBeaconReadings( );
// ---------------------------------------------
// Simulate:
// ---------------------------------------------
CActionRobotMovement2D::TMotionModelOptions opts;
CPoint3D null3D(0,0,0);
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 = DEG2RAD( 0.002f );
os::sprintf(auxStr,sizeof(auxStr),"%s/%s",outDir.c_str(),outFile.c_str());
CFileOutputStream fil(auxStr);
CPose2D realPose;
CPose2D incPose;
int stopSteps = 4;
for (i=0;i<nSteps;i++)
{
printf("Generating step %i...",i);
CSensoryFrame SF;
CActionCollection acts;
CActionRobotMovement2D act;
CPose3D pose3D( realPose );
if (i>=stopSteps)
{
if (circularPath)
{
// Circular path:
float Ar = DEG2RAD(5);
incPose = CPose2D(0.20f*cos(Ar),0.20f*sin(Ar),Ar);
}
else
{
// Square path:
if ( (i % squarePathLength) > (squarePathLength-5) )
incPose = CPose2D(0,0,DEG2RAD(90.0f/4));
else incPose = CPose2D(0.20f,0,0);
}
}
else incPose = CPose2D(0,0,0);
// Simulate observations:
CObservationBeaconRangesPtr obs=CObservationBeaconRanges::Create();
obs->minSensorDistance=minSensorDistance;
obs->maxSensorDistance=maxSensorDistance;
obs->stdError=stdError;
beaconMap.simulateBeaconReadings( pose3D,null3D,*obs );
// Corrupt with ourliers:
float probability_corrupt = i==0 ? ratio_outliers_first_step : ratio_outliers;
for (size_t q=0;q<obs->sensedData.size();q++)
{
if ( randomGenerator.drawUniform(0.0f,1.0f) < probability_corrupt )
{
obs->sensedData[q].sensedDistance += randomGenerator.drawUniform( outlier_uniform_min,outlier_uniform_max );
if (obs->sensedData[q].sensedDistance<0)
obs->sensedData[q].sensedDistance = 0;
}
}
std::cout << obs->sensedData.size() << " beacons at range";
SF.push_back( obs );
// Simulate actions:
CPose2D incOdo( incPose );
if (incPose.x()!=0 || incPose.y()!=0 || incPose.phi()!=0)
{
incOdo.x_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
incOdo.y_incr( odometryBias_Y + randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
}
act.computeFromOdometry( incOdo, opts);
acts.insert( act );
// Save:
fil << SF << acts;
// Next pose:
realPose = realPose + incPose;
printf("\n");
}
cout << "Data saved to directory: " << outDir << endl;
}
catch(std::exception &e)
{
std::cout << e.what();
}
catch(...)
{
std::cout << "Untyped exception!";
}
}
/*---------------------------------------------------------------
processActionObservation
---------------------------------------------------------------*/
void CMetricMapBuilderRBPF::processActionObservation(
CActionCollection& action, CSensoryFrame& observations)
{
MRPT_START
std::lock_guard<std::mutex> csl(
critZoneChangingMap); // Enter critical section (updating map)
// Update the traveled distance estimations:
{
CActionRobotMovement3D::Ptr act3D =
action.getActionByClass<CActionRobotMovement3D>();
CActionRobotMovement2D::Ptr act2D =
action.getActionByClass<CActionRobotMovement2D>();
if (act3D)
{
MRPT_LOG_DEBUG_STREAM(
"processActionObservation(): Input action is "
"CActionRobotMovement3D="
<< act3D->poseChange.getMeanVal().asString());
odoIncrementSinceLastMapUpdate += act3D->poseChange.getMeanVal();
odoIncrementSinceLastLocalization += act3D->poseChange;
}
else if (act2D)
{
MRPT_LOG_DEBUG_STREAM(
"processActionObservation(): Input action is "
"CActionRobotMovement2D="
<< act2D->poseChange->getMeanVal().asString());
odoIncrementSinceLastMapUpdate +=
mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
odoIncrementSinceLastLocalization.mean +=
mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
}
else
{
MRPT_LOG_WARN("Action contains no odometry.\n");
}
}
// Execute particle filter:
// But only if the traveled distance since the last update is long enough,
// or this is the first observation, etc...
// ----------------------------------------------------------------------------
bool do_localization =
(!mapPDF.SFs.size() || // This is the first observation!
options.debugForceInsertion ||
odoIncrementSinceLastLocalization.mean.norm() > localizeLinDistance ||
std::abs(odoIncrementSinceLastLocalization.mean.yaw()) >
localizeAngDistance);
bool do_map_update =
(!mapPDF.SFs.size() || // This is the first observation!
options.debugForceInsertion ||
odoIncrementSinceLastMapUpdate.norm() > insertionLinDistance ||
std::abs(odoIncrementSinceLastMapUpdate.yaw()) > insertionAngDistance);
// Used any "options.alwaysInsertByClass" ??
for (auto itCl = options.alwaysInsertByClass.data.begin();
!do_map_update && itCl != options.alwaysInsertByClass.data.end();
++itCl)
for (auto& o : observations)
if (o->GetRuntimeClass() == *itCl)
{
do_map_update = true;
do_localization = true;
break;
}
if (do_map_update) do_localization = true;
MRPT_LOG_DEBUG(mrpt::format(
"do_map_update=%s do_localization=%s", do_map_update ? "YES" : "NO",
do_localization ? "YES" : "NO"));
if (do_localization)
{
// Create an artificial action object, since
// we've been collecting them until a threshold:
// ------------------------------------------------
CActionCollection fakeActs;
{
CActionRobotMovement3D::Ptr act3D =
action.getActionByClass<CActionRobotMovement3D>();
if (act3D)
{
CActionRobotMovement3D newAct;
newAct.estimationMethod = act3D->estimationMethod;
newAct.poseChange = odoIncrementSinceLastLocalization;
newAct.timestamp = act3D->timestamp;
fakeActs.insert(newAct);
}
else
{
// It must be 2D odometry:
CActionRobotMovement2D::Ptr act2D =
action.getActionByClass<CActionRobotMovement2D>();
ASSERT_(act2D);
CActionRobotMovement2D newAct;
newAct.computeFromOdometry(
CPose2D(odoIncrementSinceLastLocalization.mean),
act2D->motionModelConfiguration);
newAct.timestamp = act2D->timestamp;
fakeActs.insert(newAct);
}
}
MRPT_LOG_DEBUG_STREAM(
"odoIncrementSinceLastLocalization before resetting = "
<< odoIncrementSinceLastLocalization.mean);
// Reset distance counters:
odoIncrementSinceLastLocalization.mean.setFromValues(0, 0, 0, 0, 0, 0);
odoIncrementSinceLastLocalization.cov.zeros();
CParticleFilter pf;
pf.m_options = m_PF_options;
pf.setVerbosityLevel(this->getMinLoggingLevel());
pf.executeOn(mapPDF, &fakeActs, &observations);
if (isLoggingLevelVisible(mrpt::system::LVL_INFO))
{
// Get current pose estimation:
CPose3DPDFParticles poseEstimation;
CPose3D meanPose;
mapPDF.getEstimatedPosePDF(poseEstimation);
poseEstimation.getMean(meanPose);
CPose3D estPos;
CMatrixDouble66 cov;
poseEstimation.getCovarianceAndMean(cov, estPos);
MRPT_LOG_INFO_STREAM(
"New pose=" << estPos << std::endl
<< "New ESS:" << mapPDF.ESS() << std::endl);
MRPT_LOG_INFO(format(
" STDs: x=%2.3f y=%2.3f z=%.03f yaw=%2.3fdeg\n",
sqrt(cov(0, 0)), sqrt(cov(1, 1)), sqrt(cov(2, 2)),
RAD2DEG(sqrt(cov(3, 3)))));
}
}
if (do_map_update)
{
odoIncrementSinceLastMapUpdate.setFromValues(0, 0, 0, 0, 0, 0);
// Update the particles' maps:
// -------------------------------------------------
MRPT_LOG_INFO("New observation inserted into the map.");
// Add current observation to the map:
const bool anymap_update = mapPDF.insertObservation(observations);
if (!anymap_update)
MRPT_LOG_WARN_STREAM(
"**No map was updated** after inserting a CSensoryFrame with "
<< observations.size());
m_statsLastIteration.observationsInserted = true;
}
else
{
m_statsLastIteration.observationsInserted = false;
}
// Added 29/JUN/2007 JLBC: Tell all maps that they can now free aux.
// variables
// (if any) since one PF cycle is over:
for (auto& m_particle : mapPDF.m_particles)
m_particle.d->mapTillNow.auxParticleFilterCleanUp();
MRPT_END;
}
