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;
}
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;
}