void MapMaintener::saveMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
// save point cloud
if (mapPointCloud.features.cols())
{
mapPointCloud.save(vtkFinalMapName);
}
}
bool Mapper::saveMap(map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res)
{
if (!mapPointCloud)
return false;
try
{
mapPointCloud->save(req.filename.data);
}
catch (const std::runtime_error& e)
{
ROS_ERROR_STREAM("Unable to save: " << e.what());
return false;
}
ROS_INFO_STREAM("Map saved at " << req.filename.data << "with " << mapPointCloud->features.cols() << " points.");
return true;
}
bool Mapper::saveMap(map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res)
{
if (!mapPointCloud)
return false;
int lastindex = req.filename.data.find_last_of(".");
string rawname = req.filename.data.substr(0, lastindex);
try
{
savePathToCsv(rawname+"_path.csv");
mapPointCloud->save(req.filename.data);
}
catch (const std::runtime_error& e)
{
ROS_ERROR_STREAM("Unable to save: " << e.what());
return false;
}
ROS_INFO_STREAM("Map saved at " << req.filename.data << " with " << mapPointCloud->features.cols() << " points.");
return true;
}
Mapper::DP* Mapper::updateMap(DP* newMap, const PM::TransformationParameters Ticp, bool updateExisting)
{
timer t;
// Prepare empty field if not existing
// FIXME: this is only needed for the none overlaping part
if(newMap->descriptorExists("probabilityStatic") == false)
{
//newMap->addDescriptor("probabilityStatic", PM::Matrix::Zero(1, newMap->features.cols()));
newMap->addDescriptor("probabilityStatic", PM::Matrix::Constant(1, newMap->features.cols(), priorStatic));
}
if(newMap->descriptorExists("probabilityDynamic") == false)
{
//newMap->addDescriptor("probabilityDynamic", PM::Matrix::Zero(1, newMap->features.cols()));
newMap->addDescriptor("probabilityDynamic", PM::Matrix::Constant(1, newMap->features.cols(), priorDyn));
}
if(newMap->descriptorExists("debug") == false)
{
newMap->addDescriptor("debug", PM::Matrix::Zero(1, newMap->features.cols()));
}
if (!updateExisting)
{
// FIXME: correct that, ugly
cout << "Jumping map creation" << endl;
*newMap = transformation->compute(*newMap, Ticp);
mapPostFilters.apply(*newMap);
return newMap;
}
// FIXME: only for debug
mapPointCloud->getDescriptorViewByName("debug") = PM::Matrix::Zero(1, mapPointCloud->features.cols());
const int newMapPts(newMap->features.cols());
const int mapPtsCount(mapPointCloud->features.cols());
// Build a range image of the reading point cloud (local coordinates)
PM::Matrix radius_newMap = newMap->features.topRows(3).colwise().norm();
PM::Matrix angles_newMap(2, newMapPts); // 0=inclination, 1=azimuth
// No atan in Eigen, so we are for to loop through it...
for(int i=0; i<newMapPts; i++)
{
const float ratio = newMap->features(2,i)/radius_newMap(0,i);
angles_newMap(0,i) = acos(ratio);
angles_newMap(1,i) = atan2(newMap->features(1,i), newMap->features(0,i));
}
std::shared_ptr<NNS> kdtree;
kdtree.reset( NNS::create(angles_newMap));
//-------------- Global map ------------------------------
// Transform the global map in local coordinates
DP mapLocal = transformation->compute(*mapPointCloud, Ticp.inverse());
// ROI: Region of Interest
// We reduce the global map to the minimum for the processing
//const float sensorMaxRange = 80.0; // ICRA
PM::Matrix globalId(1, mapPtsCount);
int ROIpts = 0;
int notROIpts = 0;
// Split mapLocal
DP mapLocalROI(mapLocal.createSimilarEmpty());
for (int i = 0; i < mapPtsCount; i++)
{
// Copy the points of the ROI in a new map
if (mapLocal.features.col(i).head(3).norm() < sensorMaxRange)
{
mapLocalROI.setColFrom(ROIpts, mapLocal, i);
globalId(0,ROIpts) = i;
ROIpts++;
}
else // Remove the points of the ROI from the global map
{
mapLocal.setColFrom(notROIpts, mapLocal, i);
notROIpts++;
}
}
mapLocalROI.conservativeResize(ROIpts);
mapLocal.conservativeResize(notROIpts);
// Convert the map in spherical coordinates
PM::Matrix radius_map = mapLocalROI.features.topRows(3).colwise().norm();
PM::Matrix angles_map(2, ROIpts); // 0=inclination, 1=azimuth
// No atan in Eigen, so we are looping through it...
// TODO: check for: A.binaryExpr(B, std::ptr_fun(atan2))
for(int i=0; i < ROIpts; i++)
{
const float ratio = mapLocalROI.features(2,i)/radius_map(0,i);
//if(ratio < -1 || ratio > 1)
//cout << "Error angle!" << endl;
angles_map(0,i) = acos(ratio);
angles_map(1,i) = atan2(mapLocalROI.features(1,i), mapLocalROI.features(0,i));
}
// Prepare access to descriptors
DP::View viewOn_Msec_overlap = newMap->getDescriptorViewByName("stamps_Msec");
DP::View viewOn_sec_overlap = newMap->getDescriptorViewByName("stamps_sec");
DP::View viewOn_nsec_overlap = newMap->getDescriptorViewByName("stamps_nsec");
DP::View viewOnProbabilityStatic = mapLocalROI.getDescriptorViewByName("probabilityStatic");
DP::View viewOnProbabilityDynamic = mapLocalROI.getDescriptorViewByName("probabilityDynamic");
DP::View viewDebug = mapLocalROI.getDescriptorViewByName("debug");
DP::View viewOn_normals_map = mapLocalROI.getDescriptorViewByName("normals");
DP::View viewOn_Msec_map = mapLocalROI.getDescriptorViewByName("stamps_Msec");
DP::View viewOn_sec_map = mapLocalROI.getDescriptorViewByName("stamps_sec");
DP::View viewOn_nsec_map = mapLocalROI.getDescriptorViewByName("stamps_nsec");
// Search for the nearest point in newMap
Matches::Dists dists(1, ROIpts);
Matches::Ids ids(1, ROIpts);
kdtree->knn(angles_map, ids, dists, 1, 0, NNS::ALLOW_SELF_MATCH, maxAngle);
// update probability of being dynamic for all points in ROI
for(int i=0; i < ROIpts; i++)
{
const int mapId = i;
viewDebug(0,mapId) = 1; //FIXME: debug
if(dists(i) != numeric_limits<float>::infinity())
{
const int readId = ids(0,i);
//const int mapId = globalId(0,i);
// in local coordinates
const Eigen::Vector3f readPt = newMap->features.col(readId).head(3);
const Eigen::Vector3f mapPt = mapLocalROI.features.col(i).head(3);
const Eigen::Vector3f mapPt_n = mapPt.normalized();
const float delta = (readPt - mapPt).norm();
const float d_max = eps_a * readPt.norm();
const Eigen::Vector3f normal_map = viewOn_normals_map.col(mapId);
// Weight for dynamic elements
const float w_v = eps + (1 - eps)*fabs(normal_map.dot(mapPt_n));
const float w_d1 = eps + (1 - eps)*(1 - sqrt(dists(i))/maxAngle);
const float offset = delta - eps_d;
float w_d2 = 1;
if(delta < eps_d || mapPt.norm() > readPt.norm())
{
w_d2 = eps;
}
else
{
if (offset < d_max)
{
w_d2 = eps + (1 - eps )*offset/d_max;
}
}
float w_p2 = eps;
if(delta < eps_d)
{
w_p2 = 1;
}
else
{
if(offset < d_max)
{
w_p2 = eps + (1 - eps)*(1 - offset/d_max);
}
}
// We don't update point behind the reading
if((readPt.norm() + eps_d + d_max) >= mapPt.norm())
{
const float lastDyn = viewOnProbabilityDynamic(0,mapId);
const float lastStatic = viewOnProbabilityStatic(0, mapId);
const float c1 = (1 - (w_v*(1 - w_d1)));
const float c2 = w_v*(1 - w_d1);
//Lock dynamic point to stay dynamic under a threshold
if(lastDyn < maxDyn)
{
viewOnProbabilityDynamic(0,mapId) = c1*lastDyn + c2*w_d2*((1 - alpha)*lastStatic + beta*lastDyn);
viewOnProbabilityStatic(0, mapId) = c1*lastStatic + c2*w_p2*(alpha*lastStatic + (1 - beta)*lastDyn);
}
else
{
viewOnProbabilityStatic(0,mapId) = eps;
viewOnProbabilityDynamic(0,mapId) = 1-eps;
}
// normalization
const float sumZ = viewOnProbabilityDynamic(0,mapId) + viewOnProbabilityStatic(0, mapId);
assert(sumZ >= eps);
viewOnProbabilityDynamic(0,mapId) /= sumZ;
viewOnProbabilityStatic(0,mapId) /= sumZ;
//viewDebug(0,mapId) =viewOnProbabilityDynamic(0, mapId);
//viewDebug(0,mapId) = w_d2;
// Refresh time
viewOn_Msec_map(0,mapId) = viewOn_Msec_overlap(0,readId);
viewOn_sec_map(0,mapId) = viewOn_sec_overlap(0,readId);
viewOn_nsec_map(0,mapId) = viewOn_nsec_overlap(0,readId);
}
}
}
// Compute density
const int mapLocalROIPts = mapLocalROI.features.cols();
cout << "build first kdtree with " << mapLocalROIPts << endl;
// build and populate NNS
std::shared_ptr<NNS> kdtree2;
kdtree2.reset( NNS::create(mapLocalROI.features, mapLocalROI.features.rows() - 1, NNS::KDTREE_LINEAR_HEAP, NNS::TOUCH_STATISTICS));
PM::Matches matches_overlap(
Matches::Dists(1, newMapPts),
Matches::Ids(1, newMapPts)
);
kdtree2->knn(newMap->features, matches_overlap.ids, matches_overlap.dists, 1, 0, NNS::ALLOW_SELF_MATCH, maxDistNewPoint);
//-------------- New point cloud ------------------------------
DP newMapOverlap(newMap->createSimilarEmpty());// Not used for now
PM::Matrix minDist = PM::Matrix::Constant(1,mapLocalROIPts, std::numeric_limits<float>::max());
// Reduce newMap to its none overlapping part
int ptsOut = 0;
int ptsIn = 0;
bool hasSensorNoise = mapLocalROI.descriptorExists("simpleSensorNoise") && newMap->descriptorExists("simpleSensorNoise");
if(hasSensorNoise) // Split and update point with lower noise
{
DP::View viewOn_noise_mapLocal = mapLocalROI.getDescriptorViewByName("simpleSensorNoise");
DP::View viewOn_noise_newMap = newMap->getDescriptorViewByName("simpleSensorNoise");
for (int i = 0; i < newMapPts; ++i)
{
const int localMapId = matches_overlap.ids(i);
if (matches_overlap.dists(i) == numeric_limits<float>::infinity())
{
newMap->setColFrom(ptsOut, *newMap, i);
ptsOut++;
}
else // Overlapping points
{
// Update point with lower sensor noise
if(viewOn_noise_newMap(0,i) < viewOn_noise_mapLocal(0,localMapId))
{
if(matches_overlap.dists(i) < minDist(localMapId))
{
minDist(localMapId) = matches_overlap.dists(i);
const float debug = viewDebug(0, localMapId);
const float probDyn = viewOnProbabilityDynamic(0, localMapId);
const float probStatic = viewOnProbabilityStatic(0, localMapId);
mapLocalROI.setColFrom(localMapId, *newMap, i);
viewDebug(0, localMapId) = 2;
viewOnProbabilityDynamic(0, localMapId) = probDyn;
viewOnProbabilityStatic(0, localMapId) = probStatic;
}
}
newMapOverlap.setColFrom(ptsIn, *newMap, i);
ptsIn++;
}
}
}
else // Only split newMap
{
for (int i = 0; i < newMapPts; ++i)
{
if (matches_overlap.dists(i) == numeric_limits<float>::infinity())
{
newMap->setColFrom(ptsOut, *newMap, i);
ptsOut++;
}
else // Overlapping points
{
newMapOverlap.setColFrom(ptsIn, *newMap, i);
ptsIn++;
}
}
}
// shrink the newMap to the new information
newMap->conservativeResize(ptsOut);
newMapOverlap.conservativeResize(ptsIn);
cout << "ptsOut=" << ptsOut << ", ptsIn=" << ptsIn << endl;
// Publish debug
//if (debugPub.getNumSubscribers())
//{
// debugPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*newMap, mapFrame, mapCreationTime));
//}
//no_overlap.addDescriptor("debug", PM::Matrix::Zero(1, no_overlap.features.cols()));
// Add the ROI to the none overlapping part
newMap->concatenate(mapLocalROI);
// Apply the filters only on the ROI
mapPostFilters.apply(*newMap);
// Add the rest of the map
newMap->concatenate(mapLocal);
// Transform the map back to global coordinates
*newMap = transformation->compute(*newMap, Ticp);
cout << "... end map creation" << endl;
ROS_INFO_STREAM("[TIME] New map available (" << newMap->features.cols() << " pts), update took " << t.elapsed() << " [s]");
return newMap;
}
// Point cloud processing
void MapMaintener::processCloud(DP newPointCloud, const TP TScannerToMap)
{
string reason;
timer t;
// Convert point cloud
const size_t goodCount(newPointCloud.features.cols());
if (goodCount == 0)
{
ROS_ERROR("I found no good points in the cloud");
return;
}
ROS_INFO("Processing new point cloud");
{
timer t; // Print how long take the algo
// Apply filters to incoming cloud, in scanner coordinates
inputFilters.apply(newPointCloud);
ROS_INFO_STREAM("Input filters took " << t.elapsed() << " [s]");
}
// Correct new points using ICP result and move them in their own frame
//cout << "TObjectToMap: " << endl << TObjectToMap << endl;
//cout << "TScannerToMap: " << endl << TScannerToMap << endl;
//cout << "concat: " << endl << TObjectToMap.inverse() * TScannerToMap << endl;
newPointCloud = transformation->compute(newPointCloud, transformation->correctParameters(TObjectToMap.inverse() * TScannerToMap));
// Preparation of cloud for inclusion in map
mapPreFilters.apply(newPointCloud);
// FIXME put that as parameter
if(newPointCloud.features.cols() < 400)
return;
// Generate first map
if(!mapPointCloud.features.rows())
{
mapPointCloud = newPointCloud;
return;
}
// Check dimension
if (newPointCloud.features.rows() != mapPointCloud.features.rows())
{
ROS_ERROR_STREAM("Dimensionality missmatch: incoming cloud is " << newPointCloud.features.rows()-1 << " while map is " << mapPointCloud.features.rows()-1);
return;
}
PM::TransformationParameters Tcorr;
try
{
Tcorr = icp(newPointCloud, mapPointCloud);
TObjectToMap = TObjectToMap * Tcorr.inverse();
}
catch (PM::ConvergenceError error)
{
ROS_WARN_STREAM("ICP failed to converge: " << error.what());
return;
}
const double estimatedOverlap = icp.errorMinimizer->getOverlap();
if(estimatedOverlap < 0.40)
{
ROS_WARN_STREAM("Estimated overlap too small: " << estimatedOverlap);
return;
}
ROS_DEBUG_STREAM("Tcorr:\n" << Tcorr);
cout << "Tcorr: " << endl << Tcorr << endl;
newPointCloud = transformation->compute(newPointCloud, Tcorr);
// Merge point clouds to map
mapPointCloud.concatenate(newPointCloud);
// Map maintenance
mapPostFilters.apply(mapPointCloud);
ROS_INFO_STREAM("New map available (" << mapPointCloud.features.cols() << " pts), update took " << t.elapsed() << " [s]");
// Publish map point cloud
// FIXME this crash when used without descriptor
if (mapPub.getNumSubscribers())
mapPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(mapPointCloud, objectFrame, ros::Time::now()));
ROS_INFO_STREAM("Total map merging: " << t.elapsed() << " [s]");
//ros::Rate r(2);
//r.sleep();
}
bool CloudMatcher::match(pointmatcher_ros::MatchClouds::Request& req, pointmatcher_ros::MatchClouds::Response& res)
{
// get and check reference
const DP referenceCloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(req.reference));
const unsigned referenceGoodCount(referenceCloud.features.cols());
const unsigned referencePointCount(req.reference.width * req.reference.height);
const double referenceGoodRatio(double(referenceGoodCount) / double(referencePointCount));
if (referenceGoodCount == 0)
{
ROS_ERROR("I found no good points in the reference cloud");
return false;
}
if (referenceGoodRatio < 0.5)
{
ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - referenceGoodRatio*100.0 <<
"% of the cloud (received " << referenceGoodCount << ")");
}
// get and check reading
const DP readingCloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(req.readings));
const unsigned readingGoodCount(referenceCloud.features.cols());
const unsigned readingPointCount(req.readings.width * req.readings.height);
const double readingGoodRatio(double(readingGoodCount) / double(readingPointCount));
if (readingGoodCount == 0)
{
ROS_ERROR("I found no good points in the reading cloud");
return false;
}
if (readingGoodRatio < 0.5)
{
ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - readingGoodRatio*100.0 << "% of the cloud (received " << readingGoodCount << ")");
}
// check dimensions
if (referenceCloud.features.rows() != readingCloud.features.rows())
{
ROS_ERROR_STREAM("Dimensionality missmatch: reference cloud is " <<
referenceCloud.features.rows() - 1 << " while reading cloud is " <<
readingCloud.features.rows() - 1);
return false;
}
// call ICP
PM::TransformationParameters transform;
try
{
transform = icp(readingCloud, referenceCloud);
tf::transformTFToMsg(PointMatcher_ros::eigenMatrixToTransform<float>(transform), res.transform);
ROS_INFO_STREAM("match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl);
}
catch (PM::ConvergenceError error)
{
ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
return false;
}
if(traceMatching)
{
std::stringstream ss;
ss << "reading_" << matchedCloudsCount << ".vtk";
readingCloud.save(ss.str());
ss.str(std::string());
ss << "reference_" << matchedCloudsCount << ".vtk";
referenceCloud.save(ss.str());
PM::Transformation* rigidTrans;
rigidTrans = PM::get().REG(Transformation).create("RigidTransformation");
PM::DataPoints matchedCloud = rigidTrans->compute(readingCloud, transform);
ss.str(std::string());
ss << "matched_" << matchedCloudsCount << ".vtk";
matchedCloud.save(ss.str());
}
matchedCloudsCount++;
return true;
}
void Mapper::gotCloud(const sensor_msgs::PointCloud2& cloudMsgIn)
{
PM::Parameters params;
PM::TransformationParameters T = PM::TransformationParameters::Identity(4,4);
sensor_msgs::PointCloud2 cloudMsg;
pcl_ros::transformPointCloud(mapFrame, cloudMsgIn, cloudMsg, tf_listener);
size_t goodCount(0);
DP newPointCloud(rosMsgToPointMatcherCloud(cloudMsg, goodCount));
if (goodCount == 0)
{
ROS_ERROR("I found no good points in the cloud");
return;
}
else
{
ROS_INFO("Processing new point cloud");
}
bool icpWasSuccess = true;
newPointCloud = filterScannerPtsCloud(newPointCloud);
// Ensure a minimum amount of point after filtering
const int ptsCount = newPointCloud.features.cols();
if(ptsCount < minReadingPointCount)
{
ROS_ERROR_STREAM("Not enough points in newPointCloud: " << ptsCount << "pts.");
return;
}
// Keep the map in memory
if(mapPointCloud.features.rows() == 0)
{
// Initialize the map
mapPointCloud = newPointCloud;
return;
}
// Apply ICP
try
{
T = icp(newPointCloud, mapPointCloud);
mapPointCloud.features = T.inverse() * mapPointCloud.features;
// Ensure minimum overlap between scans
const double estimatedOverlap = icp.errorMinimizer->getWeightedPointUsedRatio();
if (estimatedOverlap < minOverlap)
{
ROS_ERROR("Estimated overlap too small, move back");
return;
}
ROS_INFO_STREAM("**** Adding new points to the map with " << estimatedOverlap << " overlap");
// Create point cloud filters
PM::DataPointsFilter* uniformSubsample;
params = PM::Parameters({{"aggressivity", toParam(0.5)}});
uniformSubsample = pm.DataPointsFilterRegistrar.create("UniformizeDensityDataPointsFilter", params);
// Merge point clouds to map
mapPointCloud.concatenate(newPointCloud);
mapPointCloud = uniformSubsample->filter(mapPointCloud);
// Controle the size of the point cloud
const double probToKeep = maxMapPointCount/(double)mapPointCloud.features.cols();
if(probToKeep < 1.0)
{
PM::DataPointsFilter* randSubsample;
params = PM::Parameters({{"prob", toParam(probToKeep)}});
randSubsample = pm.DataPointsFilterRegistrar.create("RandomSamplingDataPointsFilter", params);
mapPointCloud = randSubsample->filter(mapPointCloud);
}
//Publish map point cloud
stringstream nameStream;
nameStream << "nifti_map_" << cloudMsg.header.seq;
PM::saveVTK(mapPointCloud, nameStream.str());
mapPub.publish(pointMatcherCloudToRosMsg(mapPointCloud, mapFrame));
}
catch (PM::ConvergenceError error)
{
icpWasSuccess = false;
ROS_WARN_STREAM("ICP failed to converge: " << error.what());
}
}