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

Stereo: added subpixel kpts processing, added test_stereo_odometry.launch

Browse Source 
CoreWrapper: Fixed corrupted image when reextracting features

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1847 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-08 00:43:05 +00:00
parent 07f675b3b8
commit fcf92d90e5
9 changed files with 234 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/demo/demo_stereo.launch
+27 -53
View File
@@ -27,7 +27,7 @@
<node pkg="nodelet" type="nodelet" name="disparity2depth" args="load rtabmap/disparity_to_depth standalone_nodelet"/> <node pkg="nodelet" type="nodelet" name="disparity2depth" args="load rtabmap/disparity_to_depth standalone_nodelet"/>
</group> </group>
<!-- Odometry: Choose between viso2_ros, fovis_ros and homemade packages --> <!-- Odometry: Choose between viso2_ros, fovis_ros and homemade approach (default) -->
<!-- <!--
<node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen"> <node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen">
<remap from="stereo" to="/stereo_camera"/> <remap from="stereo" to="/stereo_camera"/>
@@ -37,6 +37,7 @@
<param name="ref_frame_change_method" value="1"/> <param name="ref_frame_change_method" value="1"/>
</node> </node>
--> -->
<!--
<node pkg="fovis_ros" type="fovis_stereo_odometer" name="stereo_odometer" > <node pkg="fovis_ros" type="fovis_stereo_odometer" name="stereo_odometer" >
<remap from="/stereo/left/image" to="/stereo_camera/left/image_rect" /> <remap from="/stereo/left/image" to="/stereo_camera/left/image_rect" />
<remap from="/stereo/right/image" to="/stereo_camera/right/image_rect" /> <remap from="/stereo/right/image" to="/stereo_camera/right/image_rect" />
@@ -44,60 +45,26 @@
<remap from="/stereo/right/camera_info" to="/stereo_camera/right/camera_info" /> <remap from="/stereo/right/camera_info" to="/stereo_camera/right/camera_info" />
<remap from="odometry" to="stereo_odometer/odometry" /> <remap from="odometry" to="stereo_odometer/odometry" />
</node> </node>
<!-- -->
<node pkg="rtabmap" type="visual_odometry" name="visual_odometry" output="screen"> <node pkg="rtabmap" type="stereo_odometry" name="stereo_odometer" output="screen">
<remap from="rgb/image" to="stereo_camera/left/image_rect_color"/> <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
<remap from="depth/image" to="stereo_camera/depth"/> <remap from="right/image_rect" to="/stereo_camera/right/image_rect_color"/>
<remap from="rgb/camera_info" to="stereo_camera/left/camera_info"/> <remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
<remap from="odom" to="/stereo_odometer/odometry"/> <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
<param name="frame_id" type="string" value="/base_link"/> <param name="frame_id" type="string" value="base_link"/>
<param name="Odom/Type" type="string" value="6"/> <param name="Odom/Type" type="string" value="6"/>
<param name="Odom/NearestNeighbor" type="string" value="3"/> <param name="Odom/NearestNeighbor" type="string" value="3"/>
<param name="Odom/MinInliers" type="string" value="10"/> <param name="Odom/LocalHistory" type="string" value="500"/>
<param name="Odom/MaxDepth" type="string" value="3"/> <param name="Odom/MaxDepth" type="string" value="3"/>
<param name="Odom/NNDR" type="string" value="0.8"/> <param name="Odom/NNDR" type="string" value="0.6"/>
<param name="Odom/WordsRatio" type="string" value="0.5"/> <param name="GFTT/MaxCorners" type="string" value="1000"/>
<param name="Odom/LocalHistory" type="string" value="1000"/>
<param name="Odom/InlierDistance" type="string" value="0.01"/>
<param name="GFTT/MaxCorners" type="string" value="400"/>
<param name="BRIEF/Bytes" type="string" value="16"/> <param name="BRIEF/Bytes" type="string" value="16"/>
</node> </node>
-->
<!--
<node pkg="rtabmap" type="stereo_odometry" name="stereo_odometry" output="screen">
<remap from="left/image_rect" to="stereo_camera/left/image_rect_color"/>
<remap from="right/image_rect" to="stereo_camera/right/image_rect_color"/>
<remap from="left/camera_info" to="stereo_camera/left/camera_info"/>
<remap from="right/camera_info" to="stereo_camera/right/camera_info"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
<param name="frame_id" type="string" value="/base_link"/>
<param name="min_disparity" type="int" value="0"/>
<param name="max_disparity" type="int" value="128"/>
<param name="k" type="int" value="10"/>
<param name="Odom/Type" type="string" value="6"/>
<param name="Odom/NearestNeighbor" type="string" value="3"/>
<param name="Odom/MinInliers" type="string" value="10"/>
<param name="Odom/Iterations" type="string" value="200"/>
<param name="Odom/MaxDepth" type="string" value="4"/>
<param name="Odom/NNDR" type="string" value="0.8"/>
<param name="Odom/WordsRatio" type="string" value="0.5"/>
<param name="Odom/LocalHistory" type="string" value="200"/>
<param name="Odom/InlierDistance" type="string" value="0.01"/>
<param name="GFTT/MaxCorners" type="string" value="800"/>
<param name="BRIEF/Bytes" type="string" value="16"/>
<param name="BRISK/Octaves" type="string" value="0"/>
<param name="BRISK/Thresh" type="string" value="10"/>
</node>
-->
<group ns="rtabmap"> <group ns="rtabmap">
<!-- Visual SLAM (robot side) --> <!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" --> <!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start"> <node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
@@ -117,11 +84,18 @@
<param name="Rtabmap/TimeThr" type="string" value="700"/> <param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/> <param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="SURF/HessianThreshold" type="string" value="600"/> <param name="SURF/HessianThreshold" type="string" value="600"/>
<param name="LccBow/MaxDepth" type="string" value="0"/>
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/> <param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/>
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/> <param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>
<param name="LccBow/MinInliers" type="string" value="10"/> <param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.05"/> <param name="LccBow/InlierDistance" type="string" value="0.02"/>
<param name="LccBow/MaxDepth" type="string" value="3"/>
<param name="LccReextract/FeatureType" type="string" value="4"/>
<param name="LccReextract/LoopClosureFeatures" type="string" value="true"/>
<param name="LccReextract/MaxDepth" type="string" value="3"/>
<param name="LccReextract/NNDR" type="string" value="0.8"/>
<param name="LccReextract/NNType" type="string" value="3"/>
</node> </node>
<!-- Visualisation (client side) --> <!-- Visualisation (client side) -->
@@ -138,9 +112,9 @@
</node> </node>
</group> </group>
<!-- RVIZ -->
<!--
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/rgbd.rviz"/> <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/rgbd.rviz"/>
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/> <node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap/data_odom_sync standalone_nodelet"> <node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="camera/rgb/image_rect_color"/> <remap from="rgb/image_in" to="camera/rgb/image_rect_color"/>
@@ -164,5 +138,5 @@
<param name="queue_size" type="int" value="10"/> <param name="queue_size" type="int" value="10"/>
<param name="voxel_size" type="double" value="0.01"/> <param name="voxel_size" type="double" value="0.01"/>
</node> </node>
-->
</launch> </launch>
launch/rgbd_localization.launch
+15
View File
@@ -3,6 +3,17 @@
<!-- RGB-D LOCALIZATION VERSION --> <!-- RGB-D LOCALIZATION VERSION -->
<!-- ODOMETRY ARGUMENTS: "type", "nn" and "local_map":
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-Local map size: number of unique features to keep track
-->
<arg name="type" default="0" />
<arg name="nn" default="1" />
<arg name="local_map" default="0" />
<!-- TF FRAMES --> <!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf" <node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera_link 100" /> args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera_link 100" />
@@ -16,6 +27,10 @@
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/> <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<param name="frame_id" type="string" value="base_link"/> <param name="frame_id" type="string" value="base_link"/>
<param name="Odom/Type" type="string" value="$(arg type)"/>
<param name="Odom/NearestNeighbor" type="string" value="$(arg nn)"/>
<param name="Odom/LocalHistory" type="string" value="$(arg local_map)"/>
</node> </node>
<!-- Visual SLAM (robot side) --> <!-- Visual SLAM (robot side) -->
launch/rgbd_mapping.launch
+18 -2
View File
@@ -5,6 +5,17 @@
<!-- WARNING : Database is automatically deleted on each startup --> <!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... --> <!-- See "delete_db_on_start" option below... -->
<!-- ODOMETRY ARGUMENTS: "type", "nn" and "local_map":
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-Local map size: number of unique features to keep track
-->
<arg name="type" default="6" />
<arg name="nn" default="3" />
<arg name="local_map" default="2000" />
<!-- TF FRAMES --> <!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf" <node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera_link 100" /> args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera_link 100" />
@@ -17,8 +28,9 @@
<remap from="depth/image" to="/camera/depth_registered/image_raw"/> <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/> <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<param name="Odom/MinInliers" type="string" value="10"/> <param name="Odom/Type" type="string" value="$(arg type)"/>
<param name="Odom/InlierDistance" type="string" value="0.01"/> <param name="Odom/NearestNeighbor" type="string" value="$(arg nn)"/>
<param name="Odom/LocalHistory" type="string" value="$(arg local_map)"/>
<param name="frame_id" type="string" value="base_link"/> <param name="frame_id" type="string" value="base_link"/>
</node> </node>
@@ -34,6 +46,10 @@
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/> <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<remap from="odom" to="odom"/> <remap from="odom" to="odom"/>
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.02"/>
<param name="queue_size" type="int" value="30"/> <param name="queue_size" type="int" value="30"/>
</node> </node>
launch/rgbd_mapping_rviz.launch
+19 -2
View File
@@ -5,6 +5,18 @@
<!-- WARNING : Database is automatically deleted on each startup --> <!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... --> <!-- See "delete_db_on_start" option below... -->
<!-- ODOMETRY ARGUMENTS: "type", "nn" and "local_map":
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-Local map size: number of unique features to keep track
-->
<arg name="type" default="6" />
<arg name="nn" default="3" />
<arg name="local_map" default="2000" />
<!-- TF FRAMES --> <!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf" <node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera_link 100" /> args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera_link 100" />
@@ -17,8 +29,9 @@
<remap from="depth/image" to="/camera/depth_registered/image_raw"/> <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/> <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<param name="Odom/MinInliers" type="string" value="10"/> <param name="Odom/Type" type="string" value="$(arg type)"/>
<param name="Odom/InlierDistance" type="string" value="0.01"/> <param name="Odom/NearestNeighbor" type="string" value="$(arg nn)"/>
<param name="Odom/LocalHistory" type="string" value="$(arg local_map)"/>
<param name="frame_id" type="string" value="base_link"/> <param name="frame_id" type="string" value="base_link"/>
</node> </node>
@@ -34,6 +47,10 @@
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/> <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<remap from="odom" to="odom"/> <remap from="odom" to="odom"/>
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.02"/>
<param name="queue_size" type="int" value="30"/> <param name="queue_size" type="int" value="30"/>
</node> </node>
launch/tests/test_odometry.launch
+10 -12
View File
@@ -1,18 +1,16 @@
<launch> <launch>
<!-- Arguments: "type", "nn" and "local_map" --> <!-- ODOMETRY ARGUMENTS: "type", "nn" and "local_map":
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
<!-- Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF --> -Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
<arg name="type" default="0" /> Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
<!-- Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH --> -Local map size: number of unique features to keep track
<!-- Should be 1 for float descriptor like SIFT/SURF --> -->
<!-- Should be 2 for binary descriptor like ORB/FREAK/BRIEF --> <arg name="type" default="6" />
<arg name="nn" default="1" /> <arg name="nn" default="3" />
<arg name="local_map" default="2000" />
<!-- local map size: number of unique features to keep track -->
<arg name="local_map" default="0" />
<!-- TF FRAMES --> <!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf" <node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
launch/tests/test_stereo_odometry.launch
+67
View File
@@ -0,0 +1,67 @@
<launch>
<!-- ODOMETRY ARGUMENTS: "type", "nn" and "local_map":
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-Local map size: number of unique features to keep track
-->
<arg name="type" default="6" />
<arg name="nn" default="3" />
<arg name="local_map" default="5000" />
<node pkg="camera1394stereo" type="camera1394stereo_node" name="camera1394stereo_node" output="screen" >
<param name="video_mode" value="format7_mode3" />
<param name="format7_color_coding" value="raw16" />
<param name="bayer_pattern" value="bggr" />
<param name="bayer_method" value="" />
<param name="stereo_method" value="Interlaced" />
<param name="camera_info_url_left" value="" />
<param name="camera_info_url_right" value="" />
</node>
<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
args="$(arg optical_rotate) base_link stereo_camera 100" />
<!-- Run the ROS package stereo_image_proc -->
<group ns="/stereo_camera" >
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>
<!-- Odometry -->
<node pkg="rtabmap" type="stereo_odometry" name="stereo_odometry" output="screen">
<remap from="left/image_rect" to="left/image_rect_color"/>
<remap from="right/image_rect" to="right/image_rect_color"/>
<remap from="left/camera_info" to="left/camera_info"/>
<remap from="right/camera_info" to="right/camera_info"/>
<remap from="odom" to="odom_stereo"/>
<remap from="odom_local_map" to="/odom_local_map"/>
<remap from="odom_last_frame" to="/odom_last_frame"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="min_disparity" type="int" value="0"/>
<param name="max_disparity" type="int" value="256"/>
<param name="k" type="int" value="10"/>
<param name="window_size" type="int" value="5"/>
<param name="Odom/Type" type="string" value="$(arg type)"/>
<param name="Odom/NearestNeighbor" type="string" value="$(arg nn)"/>
<param name="Odom/LocalHistory" type="string" value="$(arg local_map)"/>
<param name="Odom/MaxDepth" type="string" value="3"/>
<param name="Odom/NNDR" type="string" value="0.6"/>
<param name="GFTT/MaxCorners" type="string" value="1000"/>
<param name="BRIEF/Bytes" type="string" value="16"/>
</node>
</group>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/test_odometry.rviz"/>
</launch>
src/CoreWrapper.cpp
+2 -2
View File
@@ -537,10 +537,10 @@ void CoreWrapper::process(
} }
else else
{ {
depth16 = depth; depth16 = depth.clone();
} }
SensorData data(image, SensorData data(image.clone(),
depth16, depth16,
scan, scan,
depthFx, depthFx,
src/StereoOdometryNode.cpp
+68 -82
View File
@@ -59,6 +59,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UConversion.h" #include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UTimer.h"
using namespace rtabmap; using namespace rtabmap;
@@ -73,7 +74,8 @@ public:
publishTf_(true), publishTf_(true),
minDisparity_(0.0), minDisparity_(0.0),
maxDisparity_(128.0), maxDisparity_(128.0),
k_(100), k_(10),
winSize_(5),
sync_(0), sync_(0),
paused_(false) paused_(false)
{ {
@@ -82,8 +84,7 @@ public:
odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1); odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
odomLocalMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1); odomLocalMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1); odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
//fundMatMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("fund_mat_inliers", 1); odomDepth_ = nh.advertise<sensor_msgs::Image>("odom_depth", 1);
//stereoMatchesPub_ = nh.advertise<sensor_msgs::PointCloud2>("stereo_matches", 1);
ros::NodeHandle pnh("~"); ros::NodeHandle pnh("~");
@@ -95,6 +96,7 @@ public:
pnh.param("min_disparity", minDisparity_, minDisparity_); pnh.param("min_disparity", minDisparity_, minDisparity_);
pnh.param("max_disparity", maxDisparity_, maxDisparity_); pnh.param("max_disparity", maxDisparity_, maxDisparity_);
pnh.param("k", k_, k_); pnh.param("k", k_, k_);
pnh.param("window_size", winSize_, winSize_);
//parameters //parameters
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
@@ -272,6 +274,7 @@ public:
cv::Mat depth = cv::Mat::zeros(ptrImageLeft->image.rows, ptrImageLeft->image.cols, CV_32FC1); cv::Mat depth = cv::Mat::zeros(ptrImageLeft->image.rows, ptrImageLeft->image.cols, CV_32FC1);
std::vector<cv::KeyPoint> kptsLeft, kptsRight; std::vector<cv::KeyPoint> kptsLeft, kptsRight;
std::vector<cv::Point2f> cornersLeft, cornersRight;
cv::Mat descLeft, descRight; cv::Mat descLeft, descRight;
kptsLeft = feature2D_->generateKeypoints(ptrImageLeft->image); kptsLeft = feature2D_->generateKeypoints(ptrImageLeft->image);
if(kptsLeft.size()) if(kptsLeft.size())
@@ -282,6 +285,30 @@ public:
if(kptsRight.size()) if(kptsRight.size())
{ {
descRight = feature2D_->generateDescriptors(ptrImageRight->image, kptsRight); descRight = feature2D_->generateDescriptors(ptrImageRight->image, kptsRight);
if(kptsLeft.size() && kptsRight.size())
{
cornersLeft.resize(kptsLeft.size());
cornersRight.resize(kptsRight.size());
for(unsigned int i=0; i<kptsLeft.size() || i<kptsRight.size(); ++i)
{
if(i<kptsLeft.size())
{
cornersLeft[i] = kptsLeft[i].pt;
}
if(i<kptsRight.size())
{
cornersRight[i] = kptsRight[i].pt;
}
}
UTimer time;
cv::cornerSubPix( ptrImageLeft->image, cornersLeft, cv::Size( winSize_, winSize_ ), cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03 ) );
cv::cornerSubPix( ptrImageRight->image, cornersRight, cv::Size( winSize_, winSize_ ), cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03 ) );
UDEBUG("time subpix = %fs", time.ticks());
}
} }
} }
@@ -290,84 +317,32 @@ public:
{ {
std::vector<std::vector<cv::DMatch> > matches; std::vector<std::vector<cv::DMatch> > matches;
cv::BFMatcher matcher(descLeft.depth()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2); cv::BFMatcher matcher(descLeft.depth()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2);
matcher.knnMatch(descLeft, descRight, matches, k_); int k = std::min((int)kptsLeft.size(), k_);
k = std::min((int)kptsRight.size(), k);
matcher.knnMatch(descLeft, descRight, matches, k);
int added = 0;
if(matches.size()) if(matches.size())
{ {
image_geometry::StereoCameraModel model; image_geometry::StereoCameraModel model;
model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight); model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight);
// Remove outliers using fundamental matrix RANSAC
/*std::vector<uchar> status(matches.size(), 0);
//Convert Keypoints to a structure that OpenCV understands
//3 dimensions (Homogeneous vectors)
cv::Mat points1(1, (int)matches.size(), CV_32FC2);
cv::Mat points2(1, (int)matches.size(), CV_32FC2);
float * points1data = points1.ptr<float>(0);
float * points2data = points2.ptr<float>(0);
// Fill the points here ...
for(int i=0; i < matches.size(); ++i )
{
points1data[i*2] = kptsLeft[matches[i].queryIdx].pt.x;
points1data[i*2+1] = kptsLeft[matches[i].queryIdx].pt.y;
points2data[i*2] = kptsRight[matches[i].trainIdx].pt.x;
points2data[i*2+1] = kptsRight[matches[i].trainIdx].pt.y;
}
// Find the fundamental matrix
cv::Mat fundamentalMatrix = cv::findFundamentalMat(
points1,
points2,
status,
cv::FM_RANSAC,
3.0,
0.99);
int inliers = 0;
if(!fundamentalMatrix.empty())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
for(int i = 0; i<matches.size(); ++i)
{
if(status[i])
{
float disparity = kptsLeft[matches[i].queryIdx].pt.x - kptsRight[matches[i].trainIdx].pt.x;
cv::Point3d pt3d;
model.projectDisparityTo3d(cv::Point2d(kptsLeft[matches[i].queryIdx].pt.x, kptsLeft[matches[i].queryIdx].pt.y), disparity, pt3d);
cloud.push_back(pcl::PointXYZ(pt3d.x, pt3d.y, pt3d.z));
inliers++;
}
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
fundMatMapPub_.publish(cloudMsg);
}*/
int added = 0;
int addedFirst = 0; int addedFirst = 0;
// pcl::PointCloud<pcl::PointXYZ> cloud;
for(int i=0; i< matches.size(); ++i) for(int i=0; i< matches.size(); ++i)
{ {
// add only those on same Y // add only those on same Y
for(unsigned int j=0; j<k_; ++j) for(unsigned int j=0; j<matches[i].size(); ++j)
{ {
float disparity = kptsLeft[matches[i].at(j).queryIdx].pt.x - kptsRight[matches[i].at(j).trainIdx].pt.x; float disparity = cornersLeft[matches[i].at(j).queryIdx].x - cornersRight[matches[i].at(j).trainIdx].x;
if((int)disparity >= minDisparity_ && (int)disparity <= maxDisparity_) if((int)disparity >= minDisparity_ && (int)disparity <= maxDisparity_)
{ {
float d = model.getZ(disparity); float d = model.getZ(disparity);
if(kptsLeft[matches[i].at(j).queryIdx].pt.x >= kptsRight[matches[i].at(j).trainIdx].pt.x+0.5f && if( d>0 &&
int(kptsLeft[matches[i].at(j).queryIdx].pt.y+0.5f) >= int(kptsRight[matches[i].at(j).trainIdx].pt.y+0.5f) - 3 && cornersLeft[matches[i].at(j).queryIdx].y >= cornersRight[matches[i].at(j).trainIdx].y - 3.0f &&
int(kptsLeft[matches[i].at(j).queryIdx].pt.y+0.5f) <= int(kptsRight[matches[i].at(j).trainIdx].pt.y+0.5f) + 3) cornersLeft[matches[i].at(j).queryIdx].y <= cornersRight[matches[i].at(j).trainIdx].y + 3.0f)
{ {
kptsLeft[matches[i].at(j).queryIdx].pt = cornersLeft[matches[i].at(j).queryIdx];
depth.at<float>(int(kptsLeft[matches[i].at(j).queryIdx].pt.y+0.5f), int(kptsLeft[matches[i].at(j).queryIdx].pt.x+0.5f)) = d; depth.at<float>(int(kptsLeft[matches[i].at(j).queryIdx].pt.y+0.5f), int(kptsLeft[matches[i].at(j).queryIdx].pt.x+0.5f)) = d;
/*ROS_INFO("Add%d Left(%d, %d) Right(%d, %d) distance %d = %f disp=%f, depth=%f", /*ROS_INFO("Add%d Left(%d, %d) Right(%d, %d) distance %d = %f disp=%f, depth=%f",
j, j,
@@ -376,9 +351,6 @@ public:
int(kptsRight[matches[i].at(j).trainIdx].pt.x+0.5f), int(kptsRight[matches[i].at(j).trainIdx].pt.x+0.5f),
int(kptsRight[matches[i].at(j).trainIdx].pt.y+0.5f), int(kptsRight[matches[i].at(j).trainIdx].pt.y+0.5f),
i, matches[i].at(j).distance, disparity, d);*/ i, matches[i].at(j).distance, disparity, d);*/
//cv::Point3d pt3d;
//model.projectDisparityTo3d(cv::Point2d(kptsLeft[matches[i].queryIdx].pt.x, kptsLeft[matches[i].queryIdx].pt.y), disparity, pt3d);
//cloud.push_back(pcl::PointXYZ(pt3d.x, pt3d.y, pt3d.z));
if(j == 0) if(j == 0)
{ {
++addedFirst; ++addedFirst;
@@ -398,15 +370,10 @@ public:
} }
} }
} }
/*sensor_msgs::PointCloud2 cloudMsg; UDEBUG("addedFirst = %d/%d", addedFirst, added);
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
stereoMatchesPub_.publish(cloudMsg);*/
//ROS_INFO("added = %d / %d inlier=%d", added, matches.size(), inliers);
ROS_INFO("added = %d / %d (addedFirst=%d)", added, (int)matches.size(), addedFirst);
// //
UDEBUG("localTransform = %s", rtabmap::transformFromTF(localTransform).prettyPrint().c_str());
rtabmap::SensorData data(ptrImageLeft->image, rtabmap::SensorData data(ptrImageLeft->image,
depth, depth,
depthFx, depthFx,
@@ -451,7 +418,7 @@ public:
if(odomLocalMapPub_.getNumSubscribers()) if(odomLocalMapPub_.getNumSubscribers())
{ {
const std::multimap<int, pcl::PointXYZ> & map = odometry_->getLocalMeansMap(); const std::multimap<int, pcl::PointXYZ> & map = odometry_->getLocalMap();
pcl::PointCloud<pcl::PointXYZ> cloud; pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter) for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
{ {
@@ -484,9 +451,17 @@ public:
cloudMsg.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image cloudMsg.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_; cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg); odomLastFrame_.publish(cloudMsg);
ROS_INFO("cloud = %d", (int)cloud.size());
} }
} }
if(odomDepth_.getNumSubscribers())
{
cv_bridge::CvImage img;
img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
img.image = depth;
sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
rosMsg->header= imageRectLeft->header;
odomDepth_.publish(rosMsg);
}
} }
else else
{ {
@@ -502,10 +477,20 @@ public:
odomPub_.publish(odom); odomPub_.publish(odom);
} }
} }
ROS_INFO("Odom: quality=%d, update time=%fs, stereo matches: added %d/%d",
quality, (ros::WallTime::now()-time).toSec(),
added, (int)matches.size());
}
else
{
ROS_WARN("Odom: no keypoints extracted!");
} }
}
ROS_INFO("Odom: quality=%d, update time=%fs", quality, (ros::WallTime::now()-time).toSec()); }
else
{
ROS_WARN("Odom: input images empty?!?");
}
} }
} }
@@ -555,12 +540,12 @@ private:
int minDisparity_; int minDisparity_;
int maxDisparity_; int maxDisparity_;
int k_; int k_;
int winSize_;
ros::Publisher odomPub_; ros::Publisher odomPub_;
ros::Publisher odomLocalMapPub_; ros::Publisher odomLocalMapPub_;
ros::Publisher odomLastFrame_; ros::Publisher odomLastFrame_;
//ros::Publisher fundMatMapPub_; ros::Publisher odomDepth_;
//ros::Publisher stereoMatchesPub_;
ros::ServiceServer resetSrv_; ros::ServiceServer resetSrv_;
ros::ServiceServer pauseSrv_; ros::ServiceServer pauseSrv_;
ros::ServiceServer resumeSrv_; ros::ServiceServer resumeSrv_;
@@ -580,7 +565,8 @@ private:
int main(int argc, char *argv[]) int main(int argc, char *argv[])
{ {
ULogger::setType(ULogger::kTypeConsole); ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo); ULogger::setLevel(ULogger::kWarning);
//pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
ros::init(argc, argv, "visual_odometry"); ros::init(argc, argv, "visual_odometry");
for(int i=1;i<argc;++i) for(int i=1;i<argc;++i)
src/VisualOdometryNode.cpp
+1 -1
View File
@@ -268,7 +268,7 @@ public:
if(odomLocalMapPub_.getNumSubscribers()) if(odomLocalMapPub_.getNumSubscribers())
{ {
const std::multimap<int, pcl::PointXYZ> & map = odometry_->getLocalMeansMap(); const std::multimap<int, pcl::PointXYZ> & map = odometry_->getLocalMap();
pcl::PointCloud<pcl::PointXYZ> cloud; pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter) for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
{ {