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