bool Mapper::reset(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
{
waitForMapBuildingCompleted();
// note: no need for locking as we do ros::spin(), to update if we go for multi-threading
publishLock.lock();
TOdomToMap = PM::TransformationParameters::Identity(4,4);
publishLock.unlock();
icp.clearMap();
return true;
}
void Mapper::setMap(DP* newPointCloud)
{
// delete old map
if (mapPointCloud)
delete mapPointCloud;
// set new map
mapPointCloud = newPointCloud;
icp.setMap(*mapPointCloud);
// Publish map point cloud
// FIXME this crash when used without descriptor
if (mapPub.getNumSubscribers())
mapPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*mapPointCloud, mapFrame, mapCreationTime));
}
void Mapper::setMap(DP* newPointCloud)
{
// delete old map
if (mapPointCloud)
delete mapPointCloud;
// set new map
mapPointCloud = newPointCloud;
cerr << "copying map to ICP" << endl;
//FIXME: this is taking the all map instead of the small part we need
icp.setMap(*mapPointCloud); // This do a full copy...
cerr << "publishing map" << endl;
// Publish map point cloud
// FIXME this crash when used without descriptor
if (mapPub.getNumSubscribers())
mapPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*mapPointCloud, mapFrame, mapCreationTime));
}
// IMPORTANT: We need to receive the point clouds in local coordinates (scanner or robot)
void Mapper::processCloud(unique_ptr<DP> newPointCloud, const std::string& scannerFrame, const ros::Time& stamp, uint32_t seq)
{
// if the future has completed, use the new map
processNewMapIfAvailable(); // This call lock the tf publication
cerr << "received new map" << endl;
timer t;
processingNewCloud = true;
BoolSetter stopProcessingSetter(processingNewCloud, false);
// Convert point cloud
const size_t goodCount(newPointCloud->features.cols());
if (goodCount == 0)
{
ROS_ERROR("I found no good points in the cloud");
return;
}
// Dimension of the point cloud, important since we handle 2D and 3D
const int dimp1(newPointCloud->features.rows());
if(!(newPointCloud->descriptorExists("stamps_Msec") && newPointCloud->descriptorExists("stamps_sec") && newPointCloud->descriptorExists("stamps_nsec")))
{
const float Msec = round(stamp.sec/1e6);
const float sec = round(stamp.sec - Msec*1e6);
const float nsec = round(stamp.nsec);
const PM::Matrix desc_Msec = PM::Matrix::Constant(1, goodCount, Msec);
const PM::Matrix desc_sec = PM::Matrix::Constant(1, goodCount, sec);
const PM::Matrix desc_nsec = PM::Matrix::Constant(1, goodCount, nsec);
newPointCloud->addDescriptor("stamps_Msec", desc_Msec);
newPointCloud->addDescriptor("stamps_sec", desc_sec);
newPointCloud->addDescriptor("stamps_nsec", desc_nsec);
cout << "Adding time" << endl;
}
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]");
}
string reason;
// Initialize the transformation to identity if empty
if(!icp.hasMap())
{
// we need to know the dimensionality of the point cloud to initialize properly
publishLock.lock();
TOdomToMap = PM::TransformationParameters::Identity(dimp1, dimp1);
TOdomToMap(2,3) = mapElevation;
publishLock.unlock();
}
// Fetch transformation from scanner to odom
// Note: we don't need to wait for transform. It is already called in transformListenerToEigenMatrix()
PM::TransformationParameters TOdomToScanner;
try
{
TOdomToScanner = PointMatcher_ros::eigenMatrixToDim<float>(
PointMatcher_ros::transformListenerToEigenMatrix<float>(
tfListener,
scannerFrame,
odomFrame,
stamp
), dimp1);
}
catch(tf::ExtrapolationException e)
{
ROS_ERROR_STREAM("Extrapolation Exception. stamp = "<< stamp << " now = " << ros::Time::now() << " delta = " << ros::Time::now() - stamp << endl << e.what() );
return;
}
ROS_DEBUG_STREAM("TOdomToScanner(" << odomFrame<< " to " << scannerFrame << "):\n" << TOdomToScanner);
ROS_DEBUG_STREAM("TOdomToMap(" << odomFrame<< " to " << mapFrame << "):\n" << TOdomToMap);
const PM::TransformationParameters TscannerToMap = TOdomToMap * TOdomToScanner.inverse();
ROS_DEBUG_STREAM("TscannerToMap (" << scannerFrame << " to " << mapFrame << "):\n" << TscannerToMap);
// 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: only " << ptsCount << " pts.");
return;
}
// Initialize the map if empty
if(!icp.hasMap())
{
ROS_INFO_STREAM("Creating an initial map");
mapCreationTime = stamp;
setMap(updateMap(newPointCloud.release(), TscannerToMap, false));
// we must not delete newPointCloud because we just stored it in the mapPointCloud
return;
}
// Check dimension
if (newPointCloud->features.rows() != icp.getInternalMap().features.rows())
{
ROS_ERROR_STREAM("Dimensionality missmatch: incoming cloud is " << newPointCloud->features.rows()-1 << " while map is " << icp.getInternalMap().features.rows()-1);
return;
}
try
{
// Apply ICP
PM::TransformationParameters Ticp;
Ticp = icp(*newPointCloud, TscannerToMap);
Ticp = transformation->correctParameters(Ticp);
// extract corrections
PM::TransformationParameters Tdelta = Ticp * TscannerToMap.inverse();
// ISER
//{
// // remove roll and pitch
// Tdelta(2,0) = 0;
// Tdelta(2,1) = 0;
// Tdelta(2,2) = 1;
// Tdelta(0,2) = 0;
// Tdelta(1,2) = 0;
// Tdelta(2,3) = 0; //z
// Tdelta.block(0,0,3,3) = transformation->correctParameters(Tdelta.block(0,0,3,3));
// Ticp = Tdelta*TscannerToMap;
// ROS_DEBUG_STREAM("Ticp:\n" << Ticp);
//}
// Ensure minimum overlap between scans
const double estimatedOverlap = icp.errorMinimizer->getOverlap();
ROS_INFO_STREAM("Overlap: " << estimatedOverlap);
if (estimatedOverlap < minOverlap)
{
ROS_ERROR_STREAM("Estimated overlap too small, ignoring ICP correction!");
return;
}
// Compute tf
publishStamp = stamp;
publishLock.lock();
TOdomToMap = Ticp * TOdomToScanner;
PM::TransformationParameters Terror = TscannerToMap.inverse() * Ticp;
cerr << "Correcting translation error of " << Terror.block(0,3, 3,1).norm() << " m" << endl;
// Add transformation to path
path.push_back(Ticp);
errors.push_back(Terror);
// Publish tf
if(publishMapTf == true)
{
tfBroadcaster.sendTransform(PointMatcher_ros::eigenMatrixToStampedTransform<float>(TOdomToMap, mapFrame, odomFrame, stamp));
}
publishLock.unlock();
processingNewCloud = false;
ROS_DEBUG_STREAM("TOdomToMap:\n" << TOdomToMap);
// Publish odometry
if (odomPub.getNumSubscribers())
odomPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(Tdelta, mapFrame, stamp));
// TODO: check that, might be wrong....
// Publish error on odometry
if (odomErrorPub.getNumSubscribers())
odomErrorPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(TOdomToMap, mapFrame, stamp));
// ***Debug:
//debugPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(transformation->compute(*newPointCloud, Ticp), mapFrame, mapCreationTime));
// ***
// check if news points should be added to the map
if (
mapping &&
((estimatedOverlap < maxOverlapToMerge) || (icp.getInternalMap().features.cols() < minMapPointCount)) &&
(!mapBuildingInProgress)
)
{
cout << "map Creation..." << endl;
// make sure we process the last available map
mapCreationTime = stamp;
ROS_INFO("Adding new points to the map in background");
mapBuildingTask = MapBuildingTask(boost::bind(&Mapper::updateMap, this, newPointCloud.release(), Ticp, true));
mapBuildingFuture = mapBuildingTask.get_future();
mapBuildingThread = boost::thread(boost::move(boost::ref(mapBuildingTask)));
mapBuildingInProgress = true;
}
}
catch (PM::ConvergenceError error)
{
ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
return;
}
//Statistics about time and real-time capability
int realTimeRatio = 100*t.elapsed() / (stamp.toSec()-lastPoinCloudTime.toSec());
ROS_INFO_STREAM("[TIME] Total ICP took: " << t.elapsed() << " [s]");
if(realTimeRatio < 80)
ROS_INFO_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");
else
ROS_WARN_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");
lastPoinCloudTime = stamp;
}
TEST(icpTest, icpSequenceTest)
{
DP pts0 = DP::load(dataPath + "cloud.00000.vtk");
DP pts1 = DP::load(dataPath + "cloud.00001.vtk");
DP pts2 = DP::load(dataPath + "cloud.00002.vtk");
PM::TransformationParameters Ticp = PM::Matrix::Identity(4,4);
PM::ICPSequence icpSequence;
std::ifstream ifs((dataPath + "default.yaml").c_str());
icpSequence.loadFromYaml(ifs);
EXPECT_FALSE(icpSequence.hasMap());
DP map = icpSequence.getInternalMap();
EXPECT_EQ(map.getNbPoints(), 0u);
EXPECT_EQ(map.getHomogeneousDim(), 0u);
map = icpSequence.getMap();
EXPECT_EQ(map.getNbPoints(), 0u);
EXPECT_EQ(map.getHomogeneousDim(), 0u);
icpSequence.setMap(pts0);
map = icpSequence.getInternalMap();
EXPECT_EQ(map.getNbPoints(), pts0.getNbPoints());
EXPECT_EQ(map.getHomogeneousDim(), pts0.getHomogeneousDim());
Ticp = icpSequence(pts1);
map = icpSequence.getMap();
EXPECT_EQ(map.getNbPoints(), pts0.getNbPoints());
EXPECT_EQ(map.getHomogeneousDim(), pts0.getHomogeneousDim());
Ticp = icpSequence(pts2);
map = icpSequence.getMap();
EXPECT_EQ(map.getNbPoints(), pts0.getNbPoints());
EXPECT_EQ(map.getHomogeneousDim(), pts0.getHomogeneousDim());
icpSequence.clearMap();
map = icpSequence.getInternalMap();
EXPECT_EQ(map.getNbPoints(), 0u);
EXPECT_EQ(map.getHomogeneousDim(), 0u);
}
void Mapper::processCloud(unique_ptr<DP> newPointCloud, const std::string& scannerFrame, const ros::Time& stamp, uint32_t seq)
{
processingNewCloud = true;
BoolSetter stopProcessingSetter(processingNewCloud, false);
// if the future has completed, use the new map
processNewMapIfAvailable();
// IMPORTANT: We need to receive the point clouds in local coordinates (scanner or robot)
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;
}
// Dimension of the point cloud, important since we handle 2D and 3D
const int dimp1(newPointCloud->features.rows());
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]");
}
string reason;
// Initialize the transformation to identity if empty
if(!icp.hasMap())
{
// we need to know the dimensionality of the point cloud to initialize properly
publishLock.lock();
TOdomToMap = PM::TransformationParameters::Identity(dimp1, dimp1);
publishLock.unlock();
}
// Fetch transformation from scanner to odom
// Note: we don't need to wait for transform. It is already called in transformListenerToEigenMatrix()
PM::TransformationParameters TOdomToScanner;
try
{
TOdomToScanner = PointMatcher_ros::eigenMatrixToDim<float>(
PointMatcher_ros::transformListenerToEigenMatrix<float>(
tfListener,
scannerFrame,
odomFrame,
stamp
), dimp1);
}
catch(tf::ExtrapolationException e)
{
ROS_ERROR_STREAM("Extrapolation Exception. stamp = "<< stamp << " now = " << ros::Time::now() << " delta = " << ros::Time::now() - stamp);
return;
}
ROS_DEBUG_STREAM("TOdomToScanner(" << odomFrame<< " to " << scannerFrame << "):\n" << TOdomToScanner);
ROS_DEBUG_STREAM("TOdomToMap(" << odomFrame<< " to " << mapFrame << "):\n" << TOdomToMap);
const PM::TransformationParameters TscannerToMap = TOdomToMap * TOdomToScanner.inverse();
ROS_DEBUG_STREAM("TscannerToMap (" << scannerFrame << " to " << mapFrame << "):\n" << TscannerToMap);
// 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: only " << ptsCount << " pts.");
return;
}
// Initialize the map if empty
if(!icp.hasMap())
{
ROS_INFO_STREAM("Creating an initial map");
mapCreationTime = stamp;
setMap(updateMap(newPointCloud.release(), TscannerToMap, false));
// we must not delete newPointCloud because we just stored it in the mapPointCloud
return;
}
// Check dimension
if (newPointCloud->features.rows() != icp.getInternalMap().features.rows())
{
ROS_ERROR_STREAM("Dimensionality missmatch: incoming cloud is " << newPointCloud->features.rows()-1 << " while map is " << icp.getInternalMap().features.rows()-1);
return;
}
try
{
// Apply ICP
PM::TransformationParameters Ticp;
Ticp = icp(*newPointCloud, TscannerToMap);
ROS_DEBUG_STREAM("Ticp:\n" << Ticp);
// Ensure minimum overlap between scans
const double estimatedOverlap = icp.errorMinimizer->getOverlap();
ROS_INFO_STREAM("Overlap: " << estimatedOverlap);
if (estimatedOverlap < minOverlap)
{
ROS_ERROR_STREAM("Estimated overlap too small, ignoring ICP correction!");
return;
}
// Compute tf
publishStamp = stamp;
publishLock.lock();
TOdomToMap = Ticp * TOdomToScanner;
// Publish tf
tfBroadcaster.sendTransform(PointMatcher_ros::eigenMatrixToStampedTransform<float>(TOdomToMap, mapFrame, odomFrame, stamp));
publishLock.unlock();
processingNewCloud = false;
ROS_DEBUG_STREAM("TOdomToMap:\n" << TOdomToMap);
// Publish odometry
if (odomPub.getNumSubscribers())
odomPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(Ticp, mapFrame, stamp));
// Publish error on odometry
if (odomErrorPub.getNumSubscribers())
odomErrorPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(TOdomToMap, mapFrame, stamp));
// Publish outliers
if (outlierPub.getNumSubscribers())
{
//DP outliers = PM::extractOutliers(transformation->compute(*newPointCloud, Ticp), *mapPointCloud, 0.6);
//outlierPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(outliers, mapFrame, mapCreationTime));
}
// check if news points should be added to the map
if (
mapping &&
((estimatedOverlap < maxOverlapToMerge) || (icp.getInternalMap().features.cols() < minMapPointCount)) &&
#if BOOST_VERSION >= 104100
(!mapBuildingInProgress)
#else // BOOST_VERSION >= 104100
true
#endif // BOOST_VERSION >= 104100
)
{
// make sure we process the last available map
mapCreationTime = stamp;
#if BOOST_VERSION >= 104100
ROS_INFO("Adding new points to the map in background");
mapBuildingTask = MapBuildingTask(boost::bind(&Mapper::updateMap, this, newPointCloud.release(), Ticp, true));
mapBuildingFuture = mapBuildingTask.get_future();
mapBuildingThread = boost::thread(boost::move(boost::ref(mapBuildingTask)));
mapBuildingInProgress = true;
#else // BOOST_VERSION >= 104100
ROS_INFO("Adding new points to the map");
setMap(updateMap( newPointCloud.release(), Ticp, true));
#endif // BOOST_VERSION >= 104100
}
}
catch (PM::ConvergenceError error)
{
ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
return;
}
//Statistics about time and real-time capability
int realTimeRatio = 100*t.elapsed() / (stamp.toSec()-lastPoinCloudTime.toSec());
ROS_INFO_STREAM("[TIME] Total ICP took: " << t.elapsed() << " [s]");
if(realTimeRatio < 80)
ROS_INFO_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");
else
ROS_WARN_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");
lastPoinCloudTime = stamp;
}
void Mapper::debugFeaturesDescriptors(DP newPointCloud)
{
uint i = 0;
if(newPointCloud.features.rows() != icp.getInternalMap().features.rows())
{
std::cout << "[DEBUG] Feature Rows arent the same" << std::endl;
std::cout << "[DEBUG] Feature Rows of newPointCloud is " << newPointCloud.features.rows() << std::endl;
std::cout << "[DEBUG] Feature Rows of internalMap is " << icp.getInternalMap().features.rows() << std::endl;
}else if(newPointCloud.features.cols() != icp.getInternalMap().features.cols())
{
std::cout << "[DEBUG] Feature Cols arent the same" << std::endl;
}else if(newPointCloud.featureLabels.size() != icp.getInternalMap().featureLabels.size())
{
std::cout << "[DEBUG] Feature Labels arent the same" << std::endl;
for(i=0; i < newPointCloud.featureLabels.size(); i++){
std::cout << "[DEBUG] Field " << i << " of featureLabels of newpointcloud = " << newPointCloud.featureLabels[i].text <<
"[" << newPointCloud.featureLabels[i].span << "]" << std::endl;
}
for(i=0; i < icp.getInternalMap().featureLabels.size(); i++){
std::cout << "[DEBUG] Field " << i << " of featureLabels of internalMap = " << icp.getInternalMap().featureLabels[i].text <<
"[" << icp.getInternalMap().featureLabels[i].span << "]" << std::endl;
}
}else if(newPointCloud.descriptors.rows() != icp.getInternalMap().descriptors.rows())
{
std::cout << "[DEBUG] Descriptor Rows arent the same" << std::endl;
std::cout << "[DEBUG] Descriptor Rows of newPointCloud is " << newPointCloud.descriptors.rows() << std::endl;
std::cout << "[DEBUG] Descriptor Rows of internalMap is " << icp.getInternalMap().descriptors.rows() << std::endl;
for(i=0; i < newPointCloud.descriptorLabels.size(); i++){
std::cout << "[DEBUG] Field " << i << " of descriptorLabels of newpointcloud = " << newPointCloud.descriptorLabels[i].text <<
"[" << newPointCloud.descriptorLabels[i].span << "]" << std::endl;
}
for(i=0; i < icp.getInternalMap().descriptorLabels.size(); i++){
std::cout << "[DEBUG] Field " << i << " of descriptorLabels of internalMap = " << icp.getInternalMap().descriptorLabels[i].text <<
"[" << icp.getInternalMap().descriptorLabels[i].span << "]" << std::endl;
}
}else if(newPointCloud.descriptors.cols() != icp.getInternalMap().descriptors.cols())
{
std::cout << "[DEBUG] Descriptor Cols arent the same" << std::endl;
}else if(newPointCloud.descriptorLabels.size() != icp.getInternalMap().descriptorLabels.size())
{
std::cout << "[DEBUG] Descriptor Labels arent the same" << std::endl;
for(i=0; i < newPointCloud.descriptorLabels.size(); i++){
std::cout << "[DEBUG] Field " << i << " of descriptorLabels of newpointcloud = " << newPointCloud.descriptorLabels[i].text <<
"[" << newPointCloud.descriptorLabels[i].span << "]" << std::endl;
}
for(i=0; i < icp.getInternalMap().descriptorLabels.size(); i++){
std::cout << "[DEBUG] Field " << i << " of descriptorLabels of internalMap = " << icp.getInternalMap().descriptorLabels[i].text <<
"[" << icp.getInternalMap().descriptorLabels[i].span << "]" << std::endl;
}
}
}
