void conect()
{
mEstimator.setModelCam(&mModelCam);
//mEstimator.setDataCam(mEstimator.pDataCam);
//mEstimator.setMap(pMap);
mEstimator.setModel((CModel*)(pVehicle));
//mUpdater.setDataCam(mEstimator.pDataCam);
//mUpdater.setMap(pMap);
mUpdater.setTracker((CTracker*)pTracker);
mUpdater.setModelCam(&mModelCam);
mUpdater.pEstimator=&mEstimator;
// pDataOut->setDataCam(mEstimator.pDataCam);
// pDataOut->setMap(pMap);
pDataOut->setTracker((CTracker*)pTracker);
pDataOut->setModelCam(&mModelCam);
pDataOut->pEstimator=&mEstimator;
#ifndef KALMAN
pDataOut->setParticle(&mEstimator);
#else
// pDataOut->setKalman(&mEstimator);//FIXME!!!!
#endif
mModelCam.pEstimator=&mEstimator;
pTracker->pEstimator=&mEstimator;
// pTracker->setDataCam(mEstimator.pDataCam);
}
// CParticleFilter
CParticleFilter::TParticleFilterStats CParticleFilter_executeOn(
CParticleFilter& self, CParticleFilterCapable& obj,
const mrpt::obs::CActionCollection::Ptr action,
const mrpt::obs::CSensoryFrame::Ptr observation)
{
CParticleFilter::TParticleFilterStats stats;
self.executeOn(obj, action.get(), observation.get(), &stats);
return stats;
}
}
void init_ptos()
{
if (video == true )
{
//obtencion del primer fram
frame = cvQueryFrame( capture );
}
else
{
char filein[400];
sprintf(filein,DATA,iter);
printf("%s\n",filein);
frame=cvLoadImage(filein,CV_LOAD_IMAGE_COLOR );
}
cvCopy( frame,framecopy);
mUpdater.Add(frame,frameold);
//inicializacion de la posición de la camara con funciones de opencv
cout <<"marcas visibles en init "<<mEstimator.pMap->visible<<endl;
CvMat *h;
h=cvCreateMat(3,1,CV_32FC1);
for (list<CElempunto*>::iterator It=mEstimator.pMap->bbdd.begin();It != mEstimator.pMap->bbdd.end();It++)
{
mModelCam.cvInverseParam(&h,(*It)->pto);
(*It)->wx=cvmGet(mEstimator.pDataCam->translation,0,0);
(*It)->wy=cvmGet(mEstimator.pDataCam->translation,1,0);
(*It)->wz=cvmGet(mEstimator.pDataCam->translation,2,0);
(*It)->theta=atan2(-cvmGet(h,1,0),sqrt(cvmGet(h,0,0)*cvmGet(h,0,0)+cvmGet(h,2,0)*cvmGet(h,2,0)));
(*It)->phi=atan2(cvmGet(h,0,0),cvmGet(h,2,0));
(*It)->rho=1./RHO_INIT_DIST;
(*It)->state=st_inited;
mEstimator.pMap->inited++;
}
cout<<"kalman ini"<<endl;
mEstimator.initState();
cout<<"kalman inited"<<endl;
/* splot 'post' u 2:4:6 w l,'post' u 14:15:16, 'post' u 17:18:19, 'post' u 20:21:22, 'post' u 23:24:25, 'post' u 26:27:28, 'post' u 30:31:32, 'post' u 33:34:35, 'post' u 36:37:38, 'post' u 39:40:41*/
}
int main (int argc, char **argv)
{
cout<<"start.."<<endl;
//cvSetErrMode( CV_ErrModeSilent );
xMainNode=XMLNode::openFileHelper("config.xml","slam");
const char * t;
/** Search for tracker in the config.xml
* Available trackers:
* - tracker_harris
* - tracker_surf
**/
t=xMainNode.getChildNode("tracker").getText();
if (!strcmp(t,"tracker_harris"))
{
pTracker= new CTracker_harris();
}
else if (!strcmp(t,"tracker_surf"))
{
pTracker= new CTracker_surf();
}
else
{
cout<< "error no tracker"<<endl;
exit(-1);
}
cout<<"TRACKER IS : "<<t<<endl;
/** Search for data dir in confif.xml **/
DATA=xMainNode.getChildNode("data").getText();
cout <<"DATA IS : " <<DATA<<endl;
#ifdef JAVA_GUI
{
t=xMainNode.getChildNode("gui").getText();
if (!strcmp(t,"true"))
{
env = create_vm();
invoke_class( );
}
}
#endif
t=xMainNode.getChildNode("frame_increment").getText();
int frame_increment=1;
sscanf(t,"%d",&frame_increment);
cout<<"FRAME INCREMENT IS : "<<frame_increment<<endl;
t=xMainNode.getChildNode("waitkey").getText();
int waitkey=0;
sscanf(t,"%d",&waitkey);
cout<<"WAITKEY IS : "<<waitkey<<endl;
bool mahalanobis = false;
if (xMainNode.getChildNode("tests").getChildNode("mahalanobis").isEmpty()==false)
mahalanobis = true;
cout<<"MAHALANOBIS: "<< mahalanobis<<endl;
bool ransac = false;
if( xMainNode.getChildNode("tests").getChildNode("ransac").isEmpty()==false)
ransac = true;
cout<<"RANSAC: "<< ransac <<endl;
t=xMainNode.getChildNode("resdir").getText();
string res;
if (t!=NULL)
res= t;
else
res="";
pDataOut = new CDataOut(res);
cout<<"RESDIR: "<<pDataOut->resdir<<endl;
t=xMainNode.getChildNode("vehicle").getAttribute("id");
string vehicle_mode = t;
if (vehicle_mode == "FreeCam"){
pVehicle = new CFreeCam;
}else if (vehicle_mode == "RobotXYTh"){
pVehicle = new CRobotXYTh;
((CRobotXYTh*)pVehicle)->set_filename((char*) xMainNode.getChildNode("vehicle").getChildNode("data").getText());
((CRobotXYTh*)pVehicle)->set_iter(iter);
}
cout<<"VEHICLE: "<<t<<endl;
//Map Manager initialization
mEstimator.newMap();
conect();
cout<<"param_init"<<endl;
param_init();
cout<<"init_video"<<endl;
init_video( argc, argv);
cout<<"init_ptos"<<endl;
init_ptos();
int n=0;
int i;
while(1)
{
iter+=frame_increment;
((CRobotXYTh*)pVehicle)->set_iter(iter);
cout <<"ITERACION: "<< iter<<endl;
// frame = cvQueryFrame( capture );
cvCopy(frame,frameold);
if (video == true )
{
//obtencion del primer fram
frame = cvQueryFrame( capture );
}
else
{
char filein[400];
sprintf(filein,DATA,iter);
printf("%s\n",filein);
printf("precalvoid\n");
#ifdef JAVAGUI
env->CallVoidMethod(startobj,setImage,env->NewStringUTF(filein));
if (env->ExceptionOccurred())
{
env->ExceptionDescribe();
}
exit(-1);
#endif
frame=cvLoadImage(filein,CV_LOAD_IMAGE_COLOR );
}
// compruebo que no se ha acabado el video
if(frame==NULL)
break;
// match frame
mModelCam.ProjectPoints();
pTracker->Match(frame);//entre los puntos de la imagen anterior y los de esta
mUpdater.Add(frame,frameold);
i=0;
n++;
mUpdater.remove();
mEstimator.UpdateMatrixSize();
mEstimator.Predict();
mModelCam.ProjectPoints();
if (mahalanobis) mEstimator.Test();
if (ransac) mUpdater.TestRANSAC();
mEstimator.UpdateMatrixSize();
mEstimator.Correct();
mModelCam.ProjectPoints();
#ifdef KALMAN
mEstimator.Print(iter);
#endif
cout<<"update"<<endl;
mUpdater.update();
cout<<"dataout"<<endl;
mEstimator.ManageMap();
data_out();
cout<<"waitkey correct"<<endl;
c = cvWaitKey(waitkey);//esto probablemente se pueda quitar
if( c == 27 )//si presiono escape salgo del programa limpiamente
break;
}
/// FIN
//////////////////////////////////
/*---------------------------------------------------------------
CLSLAM_RBPF_2DLASER
Implements a 2D local SLAM method based on a RBPF
over an occupancy grid map. A part of HMT-SLAM.
\param LMH The local metric hypothesis which must be updated by this SLAM algorithm.
\param act The action to process (or NULL).
\param sf The observations to process (or NULL).
WE ALREADY HAVE CONTROL OVER THE CRITICAL SECTION OF THE LMHs!
--------------------------------------------------------------- */
void CLSLAM_RBPF_2DLASER::processOneLMH(
CLocalMetricHypothesis *LMH,
const CActionCollectionPtr &actions,
const CSensoryFramePtr &sf )
{
MRPT_START
// Get the current robot pose estimation:
TPoseID currentPoseID = LMH->m_currentRobotPose;
// If this is the first iteration, just create a new robot pose at the origin:
if (currentPoseID == POSEID_INVALID )
{
currentPoseID = CHMTSLAM::generatePoseID();
LMH->m_currentRobotPose = currentPoseID; // Change it in the LMH
// Create a new robot pose:
CPose3D initPose(0,0,0);
ASSERT_( LMH->m_particles.size()>0 );
for (CLocalMetricHypothesis::CParticleList::iterator it=LMH->m_particles.begin();it!=LMH->m_particles.end();++it)
it->d->robotPoses[ currentPoseID ] = initPose;
ASSERT_( m_parent->m_map.nodeCount()==1 );
m_parent->m_map_cs.enter();
CHMHMapNodePtr firstArea = m_parent->m_map.getFirstNode();
ASSERT_(firstArea);
LMH->m_nodeIDmemberships[currentPoseID] = firstArea->getID();
// Set annotation for the reference pose:
firstArea->m_annotations.setElemental( NODE_ANNOTATION_REF_POSEID, currentPoseID , LMH->m_ID);
m_parent->m_map_cs.leave();
}
bool insertNewRobotPose = false;
if (sf)
{
if ( LMH->m_nodeIDmemberships.size()<2 ) // If there is just 1 node (the current robot pose), then there is no observation in the map yet!
{ // Update map if this is the first observation!
insertNewRobotPose = true;
}
else
{
// Check minimum distance from current robot pose to those in the neighborhood:
map< TPoseID, CPose3D > lstRobotPoses;
LMH->getMeans( lstRobotPoses );
CPose3D *currentRobotPose = & lstRobotPoses[currentPoseID];
float minDistLin = 1e6f;
float minDistAng = 1e6f;
//printf("Poses in graph:\n");
for (map< TPoseID, CPose3D >::iterator it = lstRobotPoses.begin();it!=lstRobotPoses.end();++it)
{
if (it->first != currentPoseID )
{
float linDist = it->second.distanceTo( *currentRobotPose );
float angDist = fabs(math::wrapToPi( it->second.yaw() - currentRobotPose->yaw() ));
minDistLin = min( minDistLin, linDist );
if ( linDist < m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS )
minDistAng = min(minDistAng, angDist);
}
}
// time to insert a new node??
insertNewRobotPose = (minDistLin>m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS) || ( minDistAng > m_parent->m_options.SLAM_MIN_HEADING_BETWEEN_OBS );
}
} // end if there are SF
// Save data in members so PF callback "prediction_and_update_pfXXXX" have access to them:
m_insertNewRobotPose = insertNewRobotPose;
// ------------------------------------------------
// Execute RBPF method:
// 1) PROCESS ACTION
// 2) PROCESS OBSERVATIONS
// ------------------------------------------------
CParticleFilter pf;
pf.m_options = m_parent->m_options.pf_options;
pf.executeOn( *LMH, actions.pointer(), sf.pointer() );
// 3) The appearance observation: update the log likelihood
// ...
// -----------------------------------------------------------
// 4) UPDATE THE MAP
// -----------------------------------------------------------
if (insertNewRobotPose)
{
m_parent->printf_debug("[CLSLAM_RBPF_2DLASER] Adding new pose...\n");
// Leave the up-to-now "current pose" in the map, insert the SF in it, and...
// ----------------------------------------------------------------------------
TPoseID newCurrentPoseID = CHMTSLAM::generatePoseID();
// ...and create a new "current pose" making a copy of the previous one:
// and insert the observations into the metric maps:
// ----------------------------------------------------------------------------
for (CLocalMetricHypothesis::CParticleList::iterator partIt = LMH->m_particles.begin();
partIt!=LMH->m_particles.end();
partIt++)
{
const CPose3D *curRobotPose = &partIt->d->robotPoses[currentPoseID];
partIt->d->robotPoses[newCurrentPoseID]= *curRobotPose;
sf->insertObservationsInto( &partIt->d->metricMaps, curRobotPose );
}
// Add node membership: for now, a copy of the current pose:
LMH->m_nodeIDmemberships[newCurrentPoseID] = LMH->m_nodeIDmemberships[currentPoseID];
// Now, insert the SF in the just added robot pose:
// -----------------------------------------------------
LMH->m_SFs[ currentPoseID ] = *sf;
// Sets the new poseID as current robot pose:
// ----------------------------------------------------
TPoseID newlyAddedPose = currentPoseID;
currentPoseID = LMH->m_currentRobotPose = newCurrentPoseID;
// Mark the "newlyAddedPose" as pending to reconsider in the graph-cut method
// (Done in the main loop @ LSLAM thread)
// --------------------------------------------------------------------------------
LMH->m_posesPendingAddPartitioner.push_back( newlyAddedPose );
m_parent->printf_debug("[CLSLAM_RBPF_2DLASER] Added pose %i.\n", (int)newlyAddedPose);
// Notice LC detectors:
// ------------------------------
{
synch::CCriticalSectionLocker lock( &m_parent->m_topLCdets_cs );
for (std::deque<CTopLCDetectorBase*>::iterator it=m_parent->m_topLCdets.begin();it!=m_parent->m_topLCdets.end();++it)
(*it)->OnNewPose( newlyAddedPose, sf.pointer() );
}
} // end of insertNewRobotPose
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) */ );
/*---------------------------------------------------------------
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;
}
// ------------------------------------------------------
// TestBayesianTracking
// ------------------------------------------------------
void TestBayesianTracking()
{
randomGenerator.randomize();
CDisplayWindowPlots winEKF("Tracking - Extended Kalman Filter",450,400);
CDisplayWindowPlots winPF("Tracking - Particle Filter",450,400);
winEKF.setPos(10,10);
winPF.setPos(480,10);
winEKF.axis(-2,20,-10,10); winEKF.axis_equal();
winPF.axis(-2,20,-10,10); winPF.axis_equal();
// Create EKF
// ----------------------
CRangeBearing EKF;
EKF.KF_options.method = kfEKFNaive;
EKF.KF_options.verbose = true;
EKF.KF_options.enable_profiler = true;
// Create PF
// ----------------------
CParticleFilter::TParticleFilterOptions PF_options;
PF_options.adaptiveSampleSize = false;
PF_options.PF_algorithm = CParticleFilter::pfStandardProposal;
PF_options.resamplingMethod = CParticleFilter::prSystematic;
CRangeBearingParticleFilter particles;
particles.initializeParticles(NUM_PARTICLES);
CParticleFilter PF;
PF.m_options = PF_options;
#ifdef SAVE_GT_LOGS
CFileOutputStream fo_log_ekf("log_GT_vs_EKF.txt");
fo_log_ekf.printf("%%%% GT_X GT_Y EKF_MEAN_X EKF_MEAN_Y EKF_STD_X EKF_STD_Y\n");
#endif
// Init. simulation:
// -------------------------
float x=VEHICLE_INITIAL_X,y=VEHICLE_INITIAL_Y,phi=DEG2RAD(-180),v=VEHICLE_INITIAL_V,w=VEHICLE_INITIAL_W;
float t=0;
while (winEKF.isOpen() && winPF.isOpen() && !mrpt::system::os::kbhit() )
{
// Update vehicle:
x+=v*DELTA_TIME*(cos(phi)-sin(phi));
y+=v*DELTA_TIME*(sin(phi)+cos(phi));
phi+=w*DELTA_TIME;
v+=1.0f*DELTA_TIME*cos(t);
w-=0.1f*DELTA_TIME*sin(t);
// Simulate noisy observation:
float realBearing = atan2( y,x );
float obsBearing = realBearing + BEARING_SENSOR_NOISE_STD * randomGenerator.drawGaussian1D_normalized();
printf("Real/Simulated bearing: %.03f / %.03f deg\n", RAD2DEG(realBearing), RAD2DEG(obsBearing) );
float realRange = sqrt(square(x)+square(y));
float obsRange = max(0.0, realRange + RANGE_SENSOR_NOISE_STD * randomGenerator.drawGaussian1D_normalized() );
printf("Real/Simulated range: %.03f / %.03f \n", realRange, obsRange );
// Process with EKF:
EKF.doProcess(DELTA_TIME,obsRange, obsBearing);
// Process with PF:
CSensoryFrame SF;
CObservationBearingRangePtr obsRangeBear = CObservationBearingRange::Create();
obsRangeBear->sensedData.resize(1);
obsRangeBear->sensedData[0].range = obsRange;
obsRangeBear->sensedData[0].yaw = obsBearing;
SF.insert( obsRangeBear ); // memory freed by SF.
EKF.getProfiler().enter("PF:complete_step");
PF.executeOn(particles, NULL,&SF); // Process in the PF
EKF.getProfiler().leave("PF:complete_step");
// Show EKF state:
CRangeBearing::KFVector EKF_xkk;
CRangeBearing::KFMatrix EKF_pkk;
EKF.getState( EKF_xkk, EKF_pkk );
printf("Real: x:%.03f y=%.03f heading=%.03f v=%.03f w=%.03f\n",x,y,phi,v,w);
cout << "EKF: " << EKF_xkk << endl;
// Show PF state:
cout << "Particle filter ESS: " << particles.ESS() << endl;
// Draw EKF state:
CRangeBearing::KFMatrix COVXY(2,2);
COVXY(0,0) = EKF_pkk(0,0);
COVXY(1,1) = EKF_pkk(1,1);
COVXY(0,1) = COVXY(1,0) = EKF_pkk(0,1);
winEKF.plotEllipse( EKF_xkk[0], EKF_xkk[1], COVXY, 3, "b-2", "ellipse_EKF" );
// Save GT vs EKF state:
#ifdef SAVE_GT_LOGS
// %% GT_X GT_Y EKF_MEAN_X EKF_MEAN_Y EKF_STD_X EKF_STD_Y:
fo_log_ekf.printf("%f %f %f %f %f %f\n",
x,y, // Real (GT)
EKF_xkk[0], EKF_xkk[1],
std::sqrt(EKF_pkk(0,0)), std::sqrt(EKF_pkk(1,1))
);
#endif
// Draw the velocity vector:
vector_float vx(2),vy(2);
vx[0] = EKF_xkk[0]; vx[1] = vx[0] + EKF_xkk[2] * 1;
vy[0] = EKF_xkk[1]; vy[1] = vy[0] + EKF_xkk[3] * 1;
winEKF.plot( vx,vy, "g-4", "velocityEKF" );
// Draw PF state:
{
size_t i,N = particles.m_particles.size();
vector_float parts_x(N),parts_y(N);
for (i=0;i<N;i++)
{
parts_x[i] = particles.m_particles[i].d->x;
parts_y[i] = particles.m_particles[i].d->y;
}
winPF.plot( parts_x, parts_y, "b.2", "particles" );
// Draw PF velocities:
float avrg_x, avrg_y, avrg_vx,avrg_vy;
particles.getMean(avrg_x, avrg_y, avrg_vx,avrg_vy);
vector_float vx(2),vy(2);
vx[0] = avrg_x; vx[1] = vx[0] + avrg_vx * 1;
vy[0] = avrg_y; vy[1] = vy[0] + avrg_vy * 1;
winPF.plot( vx,vy, "g-4", "velocityPF" );
}
// Draw GT:
winEKF.plot( vector_float(1,x), vector_float(1,y),"k.8","plot_GT");
winPF.plot( vector_float(1,x), vector_float(1,y),"k.8","plot_GT");
// Draw noisy observations:
vector_float obs_x(2),obs_y(2);
obs_x[0] = obs_y[0] = 0;
obs_x[1] = obsRange * cos( obsBearing );
obs_y[1] = obsRange * sin( obsBearing );
winEKF.plot(obs_x,obs_y,"r", "plot_obs_ray");
winPF.plot(obs_x,obs_y,"r", "plot_obs_ray");
// Delay:
mrpt::system::sleep((int)(DELTA_TIME*1000));
t+=DELTA_TIME;
}
}
