rtabmap barely finds features in simulated stereo image
I am using rtabmap_ros
with a simulated stereo camera in gazebo, but I am struggeling to get the stereo_odometry
node computing any transformation.
The problem is that too few (mostly not even one) features are found in my camera images. Which is confusing me, since my own implementation of GFTT finds plenty of features in the same images.
Am I getting anything wrong? Are there more requirements on the visual features for rtabmap for visual odometry?
Considering the following setup, where I am using GFTT feature detector:
<node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry">
<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"/>
<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="odom_frame_id" type="string" value="odom"/>
<param name="publish_tf" value="true" />
<param name="Odom/InlierDistance" type="string" value="20"/>
<param name="Odom/MinInliers" type="string" value="10"/>
<param name="Odom/FeatureType" type="string" value="4"/>
<param name="GFTT/UseHarrisDetector" type="string" value="false"/>
<param name="GFTT/BlockSize" type="string" value="3"/>
<param name="GFTT/K" type="string" value="0.1"/>
<param name="GFTT/MinDistance" type="string" value="20"/>
<param name="GFTT/QualityLevel" type="string" value="0.005"/>
<param name="Odom/MaxFeatures" type="string" value="1000"/>
<param name="Odom/Strategy" type="string" value="1"/>
</node>
The output of rostopic echo /odom_info
says, I get a total amount of 2 recognized features in a particular scene (see attached image below):
---
header:
seq: 387
stamp:
secs: 1615
nsecs: 663000000
frame_id: odom
lost: True
matches: 2
inliers: 0
variance: 0.0
features: 0
localMapSize: -1
timeEstimation: 0.00351881980896
timeParticleFiltering: 4.57832234264e-41
stamp: 1615.66296387
interval: 0.10000000149
distanceTravelled: 0.0
type: 1
wordsKeys: []
wordsValues: []
wordMatches: []
wordInliers: []
refCorners:
-
x: 599.0
y: 419.0
-
x: 602.0
y: 421.0
newCorners:
-
x: 323.645477295
y: 419.167877197
-
x: 332.146850586
y: 421.871734619
cornerInliers: []
transform:
translation:
x: 0.0
y: 0.0
z: 0.0
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
transformFiltered:
translation:
x: 0.0
y: 0.0
z: 0.0
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
---
However, my own implementation of GFTT in OpenCV using the same parameters finds a plenty of features. The following image shows the features from my implementation. The same image was fed into the stereo_odometry
node. Is there any hint why stereo_odometry
finds only two features?
Edit: Example output of stereo_odometry
[ WARN] (2015-09-30 11:23:52.840) util3d_registration.cpp:173::transformFromXYZCorrespondences() RANSAC refineModel: Refinement failed: got an empty set of inliers!
[ WARN] (2015-09-30 11:23:52.840) OdometryOpticalFlow.cpp:403::computeTransform() Transform not valid (inliers = 0/14)
[ INFO] [1443605032.840375078, 364.936000000]: Odom: quality=0, std dev=0.000000m, update time=0.005786s
Edit2: Here is an example disparity map of my scene: https://goo.gl/photos/6SPKukujRP6fBQpLA
Edit3: example bag file: https://drive.google.com/file/d/0B_p8EkSkbFYSelpDOHZwYWJJbEU/view?usp=sharing
Asked by Frank Engelhardt on 2015-09-29 02:50:44 UTC
Answers
Hi,
You can add output="screen"
to <node>
tag to see more information. The "Odom/InlierDistance"
seems high to me (default is 0.02 m). What is your baseline from your right camera_info (baseline = -Tx/fx = -P(0,3)/P(0,0))? This defines the scale. If the scale is large or features are far from the camera, all features may be filtered by the parameter "Odom/MaxDepth"
(default 4 m). You can try to set this parameter to 0
(infinty) to see if there are less features filtered by depth. A smaller "GFTT/MinDistance"
will extract more features too (though less uniformly distributed).
EDIT
I tried the rosbag and your launch file with <param name="Odom/MaxDepth" type="string" value="0"/>
, <param name="frame_id" type="string" value="base_footprint"/>
, output="screen"
and removing the /world
transform from the bag using this. The odometry can be computed with around 100 inliers at the beginning and up to 500 inliers when the quadcopter is closer to the building.
[ INFO] [1443630468.064779036]: Setting odometry parameter "GFTT/BlockSize"="3"
[ INFO] [1443630468.066062573]: Setting odometry parameter "GFTT/K"="0.1"
[ INFO] [1443630468.067373906]: Setting odometry parameter "GFTT/MinDistance"="20"
[ INFO] [1443630468.068633356]: Setting odometry parameter "GFTT/QualityLevel"="0.005"
[ INFO] [1443630468.069919582]: Setting odometry parameter "GFTT/UseHarrisDetector"="false"
[ INFO] [1443630468.310488405]: Setting odometry parameter "Odom/FeatureType"="4"
[ INFO] [1443630468.323157768]: Setting odometry parameter "Odom/InlierDistance"="20"
[ INFO] [1443630468.326861842]: Setting odometry parameter "Odom/MaxDepth"="0"
[ INFO] [1443630468.327708857]: Setting odometry parameter "Odom/MaxFeatures"="1000"
[ INFO] [1443630468.328520449]: Setting odometry parameter "Odom/MinInliers"="10"
[ INFO] [1443630468.361485400]: Setting odometry parameter "Odom/Strategy"="1"
[ INFO] [1443630468.586403682]: Using OdometryOpticalFlow
[ INFO] [1443630468.594537522]: Approximate time sync = false
[ INFO] [1443630469.034229148]:
/stereo_odometry subscribed to:
/stereo_camera/left/image_rect/compressed,
/stereo_camera/right/image_rect/compressed,
/stereo_camera/left/camera_info,
/stereo_camera/right/camera_info
[ WARN] [1443630473.033588144]: odometry: Could not get transform from base_footprint to stereo_camera_optical_frame_l after 0.100000 seconds!
[ WARN] [1443630473.336413051]: odometry: Could not get transform from base_footprint to stereo_camera_optical_frame_l after 0.100000 seconds!
[ WARN] [1443630473.447967756]: odometry: Could not get transform from base_footprint to stereo_camera_optical_frame_l after 0.100000 seconds!
[ INFO] [1443630473.543517934]: Odom: quality=0, std dev=0.000000m, update time=0.004190s
[ INFO] [1443630473.854162675]: Odom: quality=114, std dev=0.224909m, update time=0.014923s
[ INFO] [1443630473.949129252]: Odom: quality=119, std dev=0.055143m, update time=0.008903s
[ INFO] [1443630474.548487135]: Odom: quality=116, std dev=0.119724m, update time=0.012002s
[ INFO] [1443630474.677255094]: Odom: quality=124, std dev=0.048421m, update time=0.016780s
...
[ INFO] [1443630496.576028191]: Odom: quality=538, std dev=0.089340m, update time=0.032480s
[ INFO] [1443630496.678199498]: Odom: quality=509, std dev=0.082091m, update time=0.022229s
[ INFO] [1443630496.788770333]: Odom: quality=520, std dev=0.093364m, update time=0.027266s
[ INFO] [1443630496.984756330]: Odom: quality=451, std dev=0.450412m, update time=0.031743s
[ INFO] [1443630497.088344741]: Odom: quality=539, std dev=0.089363m, update time=0.029948s
[ INFO] [1443630497.291880964]: Odom: quality=554, std dev=0.094866m, update time=0.030979s
Combining with rtabmap node to construct the map and rtabmapviz for visualization (change Preferences->3D Rendering->Map->Max depth to 20 m), this is the result I can get (left: odometry tracked features + loop closure detection, center: 3D map with trajectory, right: top-view of the trajectory in 2D):
cheers
Asked by matlabbe on 2015-09-29 08:16:36 UTC
Comments
My baseline according to right/camera_info is 0.4m, which matches my robot's configuration. The building is about 30m long.
Settling Odom/MaxDepth
to 0 does not do the trick.
Asked by Frank Engelhardt on 2015-09-30 05:15:39 UTC
When I understand you right, the features can be filtered out by their depth value. I wonder if my features disappear simply because their depth may be hard to determine. The disparity map contains many undefined pixels...
Asked by Frank Engelhardt on 2015-09-30 05:21:36 UTC
Comments
Can you record a small rosbag? It can be just 10 seconds...
rosbag record /stereo_camera/left/image_rect/compressed /stereo_camera/right/image_rect/compressed /stereo_camera/left/camera_info /stereo_camera/right/camera_info /tf
Asked by matlabbe on 2015-09-30 08:25:44 UTC