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

OdometryIcp: fixed local scan map transform. Other minor changes.

Browse Source
This commit is contained in:
matlabbe
2018-03-17 18:27:27 -04:00
parent 88b7991e22
commit cb61f33ccf
3 changed files with 3 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/rtabmap.launch
+1 -1
View File
@@ -55,7 +55,7 @@
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" /> <arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="depth_topic" default="/camera/depth_registered/image_raw" /> <arg name="depth_topic" default="/camera/depth_registered/image_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" /> <arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<arg name="depth_camera_info_topic" default="/camera/depth_registered/camera_info" /> <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
<!-- stereo related topics --> <!-- stereo related topics -->
<arg name="stereo_namespace" default="/stereo_camera"/> <arg name="stereo_namespace" default="/stereo_camera"/>
src/OdometryROS.cpp
+2 -2
View File
@@ -600,12 +600,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
sensor_msgs::PointCloud2 cloudMsg; sensor_msgs::PointCloud2 cloudMsg;
if(info.localScanMap.hasNormals()) if(info.localScanMap.hasNormals())
{ {
pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap); pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap, info.localScanMap.localTransform());
pcl::toROSMsg(*cloud, cloudMsg); pcl::toROSMsg(*cloud, cloudMsg);
} }
else else
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(info.localScanMap); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(info.localScanMap, info.localScanMap.localTransform());
pcl::toROSMsg(*cloud, cloudMsg); pcl::toROSMsg(*cloud, cloudMsg);
} }
src/nodelets/icp_odometry.cpp
-4
View File
@@ -275,10 +275,6 @@ private:
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
maxLaserScans /= scanDownsamplingStep_; maxLaserScans /= scanDownsamplingStep_;
} }
if(!pclScan->is_dense)
{
pclScan = util3d::removeNaNNormalsFromPointCloud(pclScan);
}
scan = util3d::laserScanFromPointCloud(*pclScan); scan = util3d::laserScanFromPointCloud(*pclScan);
} }
else else