// 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();
}
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());
}
}