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

Fixed large number of stereo correspondences rejected (thus disabling loop closure) because the left/right images were not cloned by the ROS wrapper

Browse Source
This commit is contained in:
matlabbe
2016-03-13 17:01:48 -04:00
parent 7debcc4371
commit cfcc4b57a3
3 changed files with 29 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/demo/demo_stereo_outdoor.launch
+23 -23
View File
@@ -36,28 +36,28 @@
<param name="disparity_range" value="128"/> <param name="disparity_range" value="128"/>
</node> </node>
</group> </group>
<!-- Stereo Odometry -->
<node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
<remap from="left/image_rect" to="/stereo_camera/left/image_rect"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
<remap from="odom" to="/stereo_odometry"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="Odom/Strategy" type="string" value="0"/> <!-- 0=Frame-to-Map, 1=Frame=to=Frame -->
<param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D 1=3D->2D (PnP) -->
<param name="Vis/MaxDepth" type="string" value="10"/>
<param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
<param name="GFTT/MinDistance" type="string" value="10"/>
<param name="GFTT/QualityLevel" type="string" value="0.00001"/>
</node>
<group ns="rtabmap"> <group ns="rtabmap">
<!-- Stereo Odometry -->
<node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
<remap from="left/image_rect" to="/stereo_camera/left/image_rect"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
<remap from="odom" to="/stereo_odometry"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="Odom/Strategy" type="string" value="0"/> <!-- 0=Frame-to-Map, 1=Frame=to=Frame -->
<param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D 1=3D->2D (PnP) -->
<param name="Vis/MaxDepth" type="string" value="10"/>
<param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
<param name="GFTT/MinDistance" type="string" value="10"/>
<param name="GFTT/QualityLevel" type="string" value="0.00001"/>
</node>
<!-- Visual SLAM: args: "delete_db_on_start" and "udebug" --> <!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start"> <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_footprint"/> <param name="frame_id" type="string" value="base_footprint"/>
@@ -81,7 +81,7 @@
<param name="SURF/HessianThreshold" type="string" value="1000"/> <param name="SURF/HessianThreshold" type="string" value="1000"/>
<param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D, 1=3D->2D (PnP) --> <param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D, 1=3D->2D (PnP) -->
<param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/> <param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/>
<param name="Vis/MaxDepth" type="string" value="10"/> <param name="Vis/MaxDepth" type="string" value="10"/>
</node> </node>
<!-- Visualisation RTAB-Map --> <!-- Visualisation RTAB-Map -->
@@ -94,7 +94,7 @@
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/> <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/> <remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/> <remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
<remap from="odom_info" to="/odom_info"/> <remap from="odom_info" to="odom_info"/>
<remap from="odom" to="/stereo_odometry"/> <remap from="odom" to="/stereo_odometry"/>
<remap from="mapData" to="mapData"/> <remap from="mapData" to="mapData"/>
</node> </node>
src/CoreWrapper.cpp
+4 -4
View File
@@ -1080,17 +1080,17 @@ void CoreWrapper::commonStereoCallback(
scan = util3d::laserScanFromPointCloud(*pclScan); scan = util3d::laserScanFromPointCloud(*pclScan);
} }
cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage; cv_bridge::CvImagePtr ptrLeftImage, ptrRightImage;
if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{ {
ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "mono8"); ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "mono8");
} }
else else
{ {
ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8"); ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "bgr8");
} }
ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8"); ptrRightImage = cv_bridge::toCvCopy(rightImageMsg, "mono8");
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
src/StereoOdometryNode.cpp
+2 -2
View File
@@ -175,8 +175,8 @@ public:
} }
} }
cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8"); cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8");
cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8"); cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8");
UTimer stepTimer; UTimer stepTimer;
// //