/*---------------------------------------------------------------
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 nullptr).
\param sf The observations to process (or nullptr).
WE ALREADY HAVE CONTROL OVER THE CRITICAL SECTION OF THE LMHs!
--------------------------------------------------------------- */
void CLSLAM_RBPF_2DLASER::processOneLMH(
CLocalMetricHypothesis* LMH, const CActionCollection::Ptr& actions,
const CSensoryFrame::Ptr& 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 (auto& m_particle : LMH->m_particles)
m_particle.d->robotPoses[currentPoseID] = initPose;
ASSERT_(m_parent->m_map.nodeCount() == 1);
m_parent->m_map_cs.lock();
CHMHMapNode::Ptr 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.unlock();
}
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:
TMapPoseID2Pose3D lstRobotPoses;
LMH->getMeans(lstRobotPoses);
CPose3D* currentRobotPose = &lstRobotPoses[currentPoseID];
float minDistLin = 1e6f;
float minDistAng = 1e6f;
// printf("Poses in graph:\n");
for (auto& lstRobotPose : lstRobotPoses)
{
if (lstRobotPose.first != currentPoseID)
{
float linDist =
lstRobotPose.second.distanceTo(*currentRobotPose);
float angDist = fabs(math::wrapToPi(
lstRobotPose.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.get(), sf.get());
// 3) The appearance observation: update the log likelihood
// ...
// -----------------------------------------------------------
// 4) UPDATE THE MAP
// -----------------------------------------------------------
if (insertNewRobotPose)
{
m_parent->logStr(
mrpt::system::LVL_INFO,
"[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 (auto& m_particle : LMH->m_particles)
{
const CPose3D* curRobotPose =
&m_particle.d->robotPoses[currentPoseID];
m_particle.d->robotPoses[newCurrentPoseID] = *curRobotPose;
sf->insertObservationsInto(&m_particle.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->logFmt(
mrpt::system::LVL_INFO, "[CLSLAM_RBPF_2DLASER] Added pose %i.\n",
(int)newlyAddedPose);
// Notice LC detectors:
// ------------------------------
{
std::lock_guard<std::mutex> lock(m_parent->m_topLCdets_cs);
for (auto& m_topLCdet : m_parent->m_topLCdets)
m_topLCdet->OnNewPose(newlyAddedPose, sf.get());
}
} // end of insertNewRobotPose
MRPT_END
}
// ------------------------------------------------------
// TestGeometry3D
// ------------------------------------------------------
void TestLaser2Imgs()
{
// Set your rawlog file name
if (!mrpt::system::fileExists(RAWLOG_FILE))
THROW_EXCEPTION_FMT(
"Rawlog file does not exist: %s", RAWLOG_FILE.c_str())
CActionCollection::Ptr action;
CSensoryFrame::Ptr observations;
size_t rawlogEntry = 0;
// bool end = false;
CDisplayWindow wind;
// Set relative path for externally-stored images in rawlogs:
string rawlog_images_path = extractFileDirectory(RAWLOG_FILE);
rawlog_images_path += "/Images";
CImage::setImagesPathBase(rawlog_images_path); // Set it.
mrpt::io::CFileGZInputStream rawlogFile(RAWLOG_FILE);
for (;;)
{
if (os::kbhit())
{
char c = os::getch();
if (c == 27) break;
}
// Load action/observation pair from the rawlog:
// --------------------------------------------------
cout << "Reading act/oct pair from rawlog..." << endl;
auto arch = mrpt::serialization::archiveFrom(rawlogFile);
if (!CRawlog::readActionObservationPair(
arch, action, observations, rawlogEntry))
break; // file EOF
// CAMERA............
// Get CObservationStereoImages
CObservationStereoImages::Ptr sImgs;
CObservationImage::Ptr Img;
sImgs = observations->getObservationByClass<CObservationStereoImages>();
if (!sImgs)
{
Img = observations->getObservationByClass<CObservationImage>();
if (!Img) continue;
}
CPose3D cameraPose; // Get Camera Pose (B) (CPose3D)
CMatrixDouble33 K; // Get Calibration matrix (K)
sImgs ? sImgs->getSensorPose(cameraPose)
: Img->getSensorPose(cameraPose);
K = sImgs ? sImgs->leftCamera.intrinsicParams
: Img->cameraParams.intrinsicParams;
// LASER.............
// Get CObservationRange2D
CObservation2DRangeScan::Ptr laserScan =
observations->getObservationByClass<CObservation2DRangeScan>();
if (!laserScan) continue;
// Get Laser Pose (A) (CPose3D)
CPose3D laserPose;
laserScan->getSensorPose(laserPose);
if (abs(laserPose.yaw()) > DEG2RAD(90)) continue; // Only front lasers
// Get 3D Point relative to the Laser coordinate Frame (P1) (CPoint3D)
CPoint3D point;
CSimplePointsMap mapa;
mapa.insertionOptions.minDistBetweenLaserPoints = 0;
observations->insertObservationsInto(
&mapa); // <- The map contains the pose of the points (P1)
// Get the points into the map
vector<float> X, Y, Z;
vector<float>::iterator itX, itY, itZ;
mapa.getAllPoints(X, Y, Z);
unsigned int imgW =
sImgs ? sImgs->imageLeft.getWidth() : Img->image.getWidth();
unsigned int imgH =
sImgs ? sImgs->imageLeft.getHeight() : Img->image.getHeight();
// unsigned int idx = 0;
vector<float> imgX, imgY;
vector<float>::iterator itImgX, itImgY;
imgX.resize(X.size());
imgY.resize(Y.size());
CImage image;
image = sImgs ? sImgs->imageLeft : Img->image;
// Get pixels in the image:
// Pimg = (kx,ky,k)^T = K(I|0)*P2
// Main loop
for (itX = X.begin(), itY = Y.begin(), itZ = Z.begin(),
itImgX = imgX.begin(), itImgY = imgY.begin();
itX != X.end(); itX++, itY++, itZ++, itImgX++, itImgY++)
{
// Coordinates Transformation
CPoint3D pLaser(*itX, *itY, *itZ);
CPoint3D pCamera(pLaser - cameraPose);
if (pCamera.z() > 0)
{
*itImgX = -K(0, 0) * ((pCamera.x()) / (pCamera.z())) + K(0, 2);
*itImgY = -K(1, 1) * ((pCamera.y()) / (pCamera.z())) + K(1, 2);
if (*itImgX > 0 && *itImgX<imgW&& * itImgY> 0 && *itImgY < imgH)
image.filledRectangle(
*itImgX - 1, *itImgY - 1, *itImgX + 1, *itImgY + 1,
TColor(255, 0, 0));
} // end if
} // end for
action.reset();
observations.reset();
wind.showImage(image);
std::this_thread::sleep_for(50ms);
}; // end for
mrpt::system::pause();
}