i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

rtabmap.launch: added odom_topic argument to odometry nodes. OdometryROS: using odom info local map as output local map.

Browse Source
This commit is contained in:
matlabbe
2017-12-18 15:44:31 -05:00
parent ea5707e267
commit acc607a8e6
4 changed files with 17 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+9 -5
View File
@@ -531,13 +531,17 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
}
// local map / reference frame
if(odomLocalMap_.getNumSubscribers() && odometry_->getType() == Odometry::kTypeF2M)
if(odomLocalMap_.getNumSubscribers() && !info.localMap.empty())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3();
for(std::multimap<int, cv::Point3f>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
pcl::PointCloud<pcl::PointXYZRGB> cloud;
for(std::map<int, cv::Point3f>::const_iterator iter=info.localMap.begin(); iter!=info.localMap.end(); ++iter)
{
cloud.push_back(pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z));
bool inlier = info.words.find(iter->first) != info.words.end();
pcl::PointXYZRGB pt(inlier?0:255, 255, 0);
pt.x = iter->second.x;
pt.y = iter->second.y;
pt.z = iter->second.z;
cloud.push_back(pt);
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);