i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-12 14:20:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated README with examples of usage of the MIT stata center dataset based on jfr2018 paper. #1350

Browse Source
This commit is contained in:
matlabbe
2025-09-18 04:43:24 +00:00
parent f601fd43bf
commit f9ad71a7af
12 changed files with 324 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_legacy/launch/jfr2018/README.md
+186 -2
View File
@@ -1,7 +1,191 @@
**TODO: currently committed here for backup, though a comprehensive step-by-step procedure would be required so that anyone could reproduce the results**
This is the raw files used to generate results for MIT Stata Center dataset of this paper (for KITTI, EuRoC and TUM datasets, see this [page](https://github.com/introlab/rtabmap/tree/master/docker/jfr2018)):
* M. Labbé and F. Michaud, “RTAB-Map as an Open-Source Lidar and Visual SLAM Library for Large-Scale and Long-Term Online Operation,” in Journal of Field Robotics, accepted, 2018. ([pdf](https://introlab.3it.usherbrooke.ca/mediawiki-introlab/images/7/7a/Labbe18JFR_preprint.pdf)) ([Wiley](https://doi.org/10.1002/rob.21831))
Manual instructions are in [launch_lidar](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_lidar), [launch_stereo](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_stereo) and [launch_rgbd](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_rgbd) files depending on the sensor configuration. Refer also to explanations in the paper.
Manual instructions are in [launch_lidar](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_lidar), [launch_stereo](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_stereo) and [launch_rgbd](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_rgbd) files depending on the sensor configuration. Refer also to explanations in the paper. Below are some examples of usage per sensor type.
# Long-Range LiDAR with WheelIMU→S2M odometry
1. Launch rtabmap following config "Long-Range LiDAR with WheelIMU→S2M odometry"
```
roslaunch rtabmap_launch rtabmap.launch \
args:="\
-d \
--Rtabmap/PublishRAMUsage true \
--Rtabmap/StartNewMapOnLoopClosure true \
--Reg/Force3DoF false \
--RGBD/ProximityPathMaxNeighbors 0 \
--Mem/STMSize 15 \
--Mem/BinDataKept true \
--Kp/FlannRebalancingFactor 1.0 \
--RGBD/LinearUpdate 0 \
--RGBD/ProximityBySpace true \
--RGBD/OptimizeMaxError 3 \
--FAST/Threshold 7" \
odom_args:="\
--Odom/Strategy 0 \
--Vis/CorType 0 \
--Odom/KeyFrameThr 0.3 \
--OdomF2M/MaxSize 2000 \
--uwarn" \
rgbd_sync:=true \
depth_scale:=1.043 \
frame_id:=base_footprint \
ground_truth_frame_id:=world \
ground_truth_base_frame_id:=scan_gt \
use_sim_time:=true \
odom_topic:=odom \
rgb_topic:=/camera/rgb/image_raw \
depth_topic:=/camera/depth/image_raw \
camera_info_topic:=/camera/rgb/camera_info \
approx_sync:=false \
odom_guess_frame_id:=odom_combined \
odom_always_process_most_recent_frame:=false
```
2. Publish the ground truth in TF
```
python3 gt_tf_broadcaster.py \
_file:=2012-01-25-12-33-29_part1_floor2.gt.laser.poses \
_frame_id:=scan_gt \
_fixed_frame_id:=world \
_offset_time:=82.2 \
_offset_x:=-0.275
```
3. Launch the rosbag. Make sure you are running the rosbag on a SSD directly connected inside the computer (not USB SSD) to limit the lags. Note that in contrast to visual odometry approaches below, the lags seem affecting less lidar odometry, so we are still using the original rosbag here.
```
rosbag play --clock --pause 2012-01-25-12-33-29.bag
```
# RGB-D Camera with F2M odometry
1. To avoid lags when replaying the original rosbag, just extract the RGB-D data into another rosbag. With the script in this folder, do:
```
python3 extract_rgbd.py 2012-01-25-12-33-29
```
This will create a new rosbag called `2012-01-25-12-33-29_rgbd.bag`.
2. Launch rtabmap following config "RGB-D Camera with F2M odometry"
```
roslaunch rtabmap_launch rtabmap.launch \
args:="-d \
--Rtabmap/PublishRAMUsage true \
--Rtabmap/StartNewMapOnLoopClosure true \
--Reg/Force3DoF false \
--RGBD/ProximityPathMaxNeighbors 0 \
--Mem/STMSize 15 \
--Mem/BinDataKept true \
--Kp/FlannRebalancingFactor 1.0 \
--RGBD/LinearUpdate 0 \
--RGBD/ProximityBySpace true \
--RGBD/OptimizeMaxError 3 \
--GFTT/QualityLevel 0.01 \
--Vis/MinInliers 10" \
odom_args:="\
--Odom/Strategy 0 \
--Vis/CorType 0 \
--Odom/KeyFrameThr 0.3 \
--OdomF2M/MaxSize 2000 \
--uwarn" \
rgbd_sync:=true \
depth_scale:=1.043 \
frame_id:=base_footprint \
ground_truth_frame_id:=world \
ground_truth_base_frame_id:=scan_gt \
use_sim_time:=true \
odom_topic:=odom \
rgb_topic:=/camera/rgb/image_raw \
depth_topic:=/camera/depth/image_raw \
camera_info_topic:=/camera/rgb/camera_info \
approx_sync:=false \
approx_sync_max_interval:=0.015 \
odom_guess_frame_id:=odom_combined \
odom_always_process_most_recent_frame:=false
```
3. Publish the ground truth in TF
```
python3 gt_tf_broadcaster.py \
_file:=2012-01-25-12-33-29_part1_floor2.gt.laser.poses \
_frame_id:=scan_gt \
_fixed_frame_id:=world \
_offset_time:=82.2 \
_offset_x:=-0.275
```
4. Launch the rosbag. Make sure you are running the rosbag on a SSD directly connected inside the computer (not USB SSD) to limit the lags.
```
rosbag play --clock --pause 2012-01-25-12-33-29_rgbd.bag
```
# Stereo Camera with F2M odometry
1. To avoid lags when replaying the original rosbag, just extract the stereo data into another rosbag. With the script in this folder, do:
```
python3 extract_stereo.py 2012-01-25-12-33-29
```
This will create a new rosbag called `2012-01-25-12-33-29_stereo.bag`.
2. Launch rtabmap following config "Stereo Camera with F2M odometry"
```
roslaunch rtabmap_launch rtabmap.launch \
args:="-d \
--Rtabmap/PublishRAMUsage true \
--Rtabmap/StartNewMapOnLoopClosure true \
--Reg/Force3DoF false \
--RGBD/ProximityPathMaxNeighbors 0 \
--Mem/STMSize 15 \
--Mem/BinDataKept true \
--Kp/FlannRebalancingFactor 1.0 \
--RGBD/LinearUpdate 0 \
--RGBD/ProximityBySpace true \
--Odom/KeyFrameThr 0.3 \
--Odom/Strategy 0 \
--OdomF2M/MaxSize 2000 \
--RGBD/OptimizeMaxError 3 \
--GFTT/QualityLevel 0.01 \
--Vis/MinInliers 10" \
odom_args:="--Vis/CorType 0 --uwarn" \
frame_id:=base_footprint \
ground_truth_frame_id:=world \
ground_truth_base_frame_id:=scan_gt \
use_sim_time:=true \
stereo:=true \
stereo_namespace:=/wide_stereo \
left_camera_info_topic:=/wide_stereo/left/camera_info \
right_camera_info_topic:=/wide_stereo/right/camera_info_scaled \
odom_topic:=odom \
approx_sync:=false \
odom_guess_frame_id:=odom_combined \
odom_always_process_most_recent_frame:=false
```
3. Publish the ground truth in TF
```
python3 gt_tf_broadcaster.py \
_file:=2012-01-25-12-33-29_part1_floor2.gt.laser.poses \
_frame_id:=scan_gt \
_fixed_frame_id:=world \
_offset_time:=82.2 \
_offset_x:=-0.275
```
4. Republish the stereo camera calibration with baseline correctly scaled
```
python3 republish_camera_info.py \
camera_info_in:=/wide_stereo/right/camera_info \
camera_info_out:=/wide_stereo/right/camera_info_scaled
```
5. Rectify the raw stereo images
```
export ROS_NAMESPACE=wide_stereo
rosrun stereo_image_proc stereo_image_proc \
left/image_raw:=left/image_raw \
right/image_raw:=right/image_raw \
left/camera_info:=left/camera_info \
right/camera_info:=right/camera_info_scaled
```
6. Launch the rosbag. Make sure you are running the rosbag on a SSD directly connected inside the computer (not USB SSD) to limit the lags.
```
rosbag play --clock --pause 2012-01-25-12-33-29_stereo.bag
```
rtabmap_legacy/launch/jfr2018/extract_rgbd.py
+3 -3
View File
@@ -4,13 +4,13 @@ import sys
from tf.msg import tfMessage
if len(sys.argv) < 2:
print 'Usage: $ python extract_rgbd.py "2012-01-25-12-14-25"'
print('Usage: $ python extract_rgbd.py "2012-01-25-12-14-25"')
sys.exit(0)
bagName = sys.argv[1]
with rosbag.Bag(bagName + '_rgbd.bag', 'w') as outbag:
print 'Processing ' + bagName + '.bag...'
print(f'Processing {bagName}.bag...')
for topic, msg, t in rosbag.Bag(bagName + '.bag').read_messages():
if topic == "/tf" or topic == "/camera/depth/image_raw" or topic == "/camera/rgb/camera_info" or topic == "/camera/rgb/image_raw":
outbag.write(topic, msg, t)
print 'Output: ' + bagName + '_out.bag'
print(f'Output: {bagName}_rgbd.bag')
rtabmap_legacy/launch/jfr2018/extract_stereo.py
+7 -19
View File
@@ -4,34 +4,22 @@ import sys
from tf.msg import tfMessage
if len(sys.argv) < 2:
print 'Usage: $ python extract_stereo.py "2012-01-25-12-14-25"'
print('Usage: $ python extract_stereo.py "2012-01-25-12-14-25"')
sys.exit(0)
bagName = sys.argv[1]
with rosbag.Bag(bagName + '_stereo.bag', 'w') as outbag:
print 'Processing ' + bagName + '.bag...'
leftCamInfoStatus = True
leftImageStatus = True
rightCamInfoStatus = True
rightImageStatus = True
print(f'Processing {bagName}.bag...')
for topic, msg, t in rosbag.Bag(bagName + '.bag').read_messages():
if topic == "/tf":
outbag.write(topic, msg, t)
elif topic == "/wide_stereo/left/camera_info":
if leftCamInfoStatus:
outbag.write(topic, msg, t)
leftCamInfoStatus = not leftCamInfoStatus
outbag.write(topic, msg, t)
elif topic == "/wide_stereo/right/camera_info":
if rightCamInfoStatus:
outbag.write(topic, msg, t)
rightCamInfoStatus = not rightCamInfoStatus
outbag.write(topic, msg, t)
elif topic == "/wide_stereo/left/image_raw":
if leftImageStatus:
outbag.write(topic, msg, t)
leftImageStatus = not leftImageStatus
outbag.write(topic, msg, t)
elif topic == "/wide_stereo/right/image_raw":
if rightImageStatus:
outbag.write(topic, msg, t)
rightImageStatus = not rightImageStatus
outbag.write(topic, msg, t)
print 'Output: ' + bagName + '_out.bag'
print(f'Output: {bagName}_stereo.bag')
rtabmap_legacy/launch/jfr2018/throttle_bag.launch
+8 -5
View File
@@ -3,21 +3,20 @@
<launch>
<param name="use_sim_time" value="true"/>
<group ns="/wide_stereo" >
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_ros/stereo_throttle">
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_legacy/stereo_throttle">
<remap from="left/image" to="left/image_raw"/>
<remap from="right/image" to="right/image_raw"/>
<remap from="left/camera_info" to="left/camera_info"/>
<remap from="right/camera_info" to="right/camera_info"/>
<param name="queue_size" type="int" value="10"/>
<param name="queue_size" type="int" value="100"/>
<param name="rate" type="double" value="15"/>
<param name="expected_input_rate" type="double" value="30"/>
</node>
</group>
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="standalone rtabmap_ros/data_throttle">
<param name="rate" type="double" value="15.0"/>
<node pkg="nodelet" type="nodelet" name="data_throttle" args="standalone rtabmap_legacy/data_throttle">
<remap from="rgb/image_in" to="rgb/image_raw"/>
<remap from="depth/image_in" to="depth/image_raw"/>
<remap from="rgb/camera_info_in" to="rgb/camera_info"/>
@@ -25,6 +24,10 @@
<remap from="rgb/image_out" to="rgb/image_raw_throttle"/>
<remap from="depth/image_out" to="depth/image_raw_throttle"/>
<remap from="rgb/camera_info_out" to="rgb/camera_info_throttle"/>
<param name="queue_size" type="int" value="100"/>
<param name="rate" type="double" value="15.0"/>
<param name="expected_input_rate" type="double" value="30"/>
</node>
</group>
</launch>
rtabmap_legacy/src/nodelets/data_throttle.cpp
+22 -11
View File
@@ -51,7 +51,7 @@ class DataThrottleNodelet : public nodelet::Nodelet
public:
//Constructor
DataThrottleNodelet():
rate_(0),
period_(0),
approxSync_(0),
exactSync_(0),
decimation_(1)
@@ -72,7 +72,7 @@ public:
private:
ros::Time last_update_;
double rate_;
double period_;
virtual void onInit()
{
ros::NodeHandle& nh = getNodeHandle();
@@ -90,22 +90,33 @@ private:
int queueSize = 10;
bool approxSync = true;
double approxSyncMaxInterval = 0.0;
if(private_nh.getParam("max_rate", rate_))
double rate = 0.0;
double expectedInputRate = 0.0;
if(private_nh.getParam("max_rate", rate))
{
NODELET_WARN("\"max_rate\" is now known as \"rate\".");
}
private_nh.param("rate", rate_, rate_);
private_nh.param("rate", rate, rate);
private_nh.param("expected_input_rate", expectedInputRate, expectedInputRate);
private_nh.param("queue_size", queueSize, queueSize);
private_nh.param("approx_sync", approxSync, approxSync);
private_nh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
private_nh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1);
NODELET_INFO("rate=%f Hz", rate_);
NODELET_INFO("rate=%f Hz", rate);
NODELET_INFO("expected_input_rate=%f Hz", expectedInputRate);
NODELET_INFO("decimation=%d", decimation_);
NODELET_INFO("approx_sync = %s", approxSync?"true":"false");
if(approxSync)
NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval);
if(rate>0){
period_ = 1.0/rate;
}
if(expectedInputRate > 0 && expectedInputRate >= rate) {
period_ -= (1.0/expectedInputRate)/2.0;
}
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
@@ -132,19 +143,19 @@ private:
const sensor_msgs::ImageConstPtr& imageDepth,
const sensor_msgs::CameraInfoConstPtr& camInfo)
{
if (rate_ > 0.0)
if (period_ > 0.0)
{
NODELET_DEBUG("update set to %f", rate_);
if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
if (last_update_ != ros::Time() && (image->header.stamp - last_update_).toSec() < period_)
{
NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
NODELET_DEBUG("throttle last update at %f skipping (ref=%f)", image->header.stamp.toSec(), last_update_.toSec());
return;
}
}
else
else {
NODELET_DEBUG("rate unset continuing");
}
last_update_ = ros::Time::now();
last_update_ = image->header.stamp;
double rgbStamp = image->header.stamp.toSec();
double depthStamp = imageDepth->header.stamp.toSec();
rtabmap_legacy/src/nodelets/stereo_throttle.cpp
+22 -10
View File
@@ -50,7 +50,7 @@ class StereoThrottleNodelet : public nodelet::Nodelet
public:
//Constructor
StereoThrottleNodelet():
rate_(0),
period_(0),
approxSync_(0),
exactSync_(0),
decimation_(1)
@@ -71,7 +71,7 @@ public:
private:
ros::Time last_update_;
double rate_;
double period_;
virtual void onInit()
{
ros::NodeHandle& nh = getNodeHandle();
@@ -89,18 +89,29 @@ private:
int queueSize = 5;
bool approxSync = false;
double approxSyncMaxInterval = 0.0;
double rate = 0.0;
double expectedInputRate = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("rate", rate_, rate_);
pnh.param("rate", rate, rate);
pnh.param("expected_input_rate", expectedInputRate, expectedInputRate);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1);
NODELET_INFO("rate=%f Hz", rate_);
NODELET_INFO("rate=%f Hz", rate);
NODELET_INFO("expected_input_rate=%f Hz", expectedInputRate);
NODELET_INFO("decimation=%d", decimation_);
NODELET_INFO("approx_sync = %s", approxSync?"true":"false");
if(approxSync)
NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval);
if(rate>0){
period_ = 1.0/rate;
}
if(expectedInputRate > 0 && expectedInputRate >= rate) {
period_ -= (1.0/expectedInputRate)/2.0;
}
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
@@ -130,20 +141,21 @@ private:
const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
const sensor_msgs::CameraInfoConstPtr& camInfoRight)
{
if (rate_ > 0.0)
if (period_ > 0.0)
{
NODELET_DEBUG("update set to %f", rate_);
if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
if (last_update_ != ros::Time() && (imageLeft->header.stamp - last_update_).toSec() < period_)
{
NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
NODELET_DEBUG("throttle last update at %f skipping (ref=%f)", imageLeft->header.stamp.toSec(), last_update_.toSec());
return;
}
}
else
else {
NODELET_DEBUG("rate unset continuing");
}
last_update_ = ros::Time::now();
last_update_ = imageLeft->header.stamp;
double leftStamp = imageLeft->header.stamp.toSec();
double rightStamp = imageRight->header.stamp.toSec();
double leftInfoStamp = camInfoLeft->header.stamp.toSec();
rtabmap_odom/src/OdometryROS.cpp
+1 -1
View File
@@ -487,7 +487,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
{
double estimatedPeriod = clockNow - lastReceivedTopicClock_;
double topicPeriod = header.stamp.toSec() - lastReceivedTopicStamp_;
if(estimatedPeriod>0.0 && topicPeriod>0.0 && estimatedPeriod < topicPeriod*0.9) {
if(estimatedPeriod>0.0 && topicPeriod>0.0 && estimatedPeriod < topicPeriod*0.5) {
NODELET_WARN("Dropping image/scan data with stamp %f (delay=%f). Something is wrong "
"because the clock difference with the previous topic received (%fs) is much lower than the "
"expected one (%fs) estimated from the topic stamps (previous stamp=%f). If you are processing "
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+24 -23
View File
@@ -76,7 +76,8 @@ public:
exactSync6_(0),
topicQueueSize_(1),
syncQueueSize_(5),
keepColor_(false)
keepColor_(false),
approxSyncMaxInterval_(0.0)
{
}
@@ -108,9 +109,8 @@ private:
int rgbdCameras = 1;
bool approxSync = true;
bool subscribeRGBD = false;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval_, approxSyncMaxInterval_);
pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_);
if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
{
@@ -138,7 +138,7 @@ private:
NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
NODELET_INFO("RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
NODELET_INFO("RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval_);
NODELET_INFO("RGBDOdometry: topic_queue_size = %d", topicQueueSize_);
NODELET_INFO("RGBDOdometry: sync_queue_size = %d", syncQueueSize_);
NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
@@ -178,8 +178,8 @@ private:
MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
}
else
@@ -193,7 +193,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str());
}
@@ -206,8 +206,8 @@ private:
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
}
else
@@ -222,7 +222,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str());
@@ -237,8 +237,8 @@ private:
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
}
else
@@ -254,7 +254,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str(),
@@ -271,8 +271,8 @@ private:
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
}
else
@@ -289,7 +289,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str(),
@@ -308,8 +308,8 @@ private:
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
}
else
@@ -327,7 +327,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str(),
@@ -382,8 +382,8 @@ private:
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
}
else
@@ -396,7 +396,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str());
@@ -593,7 +593,7 @@ private:
infoMsgs.push_back(*cameraInfo);
double stampDiff = fabs(image->header.stamp.toSec() - depth->header.stamp.toSec());
if(stampDiff > 0.020)
if(approxSyncMaxInterval_==0.0 && stampDiff > 0.020)
{
NODELET_WARN("The time difference between rgb and depth frames is "
"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
@@ -934,6 +934,7 @@ private:
int topicQueueSize_;
int syncQueueSize_;
bool keepColor_;
double approxSyncMaxInterval_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_odom::RGBDOdometry, nodelet::Nodelet);
rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp
+11 -10
View File
@@ -75,6 +75,7 @@ public:
queueSize_(1),
syncQueueSize_(5),
keepColor_(false),
approxSyncMaxInterval_(0.0),
scanCloudMaxPoints_(0),
scanVoxelSize_(0.0),
scanNormalK_(0),
@@ -111,9 +112,8 @@ private:
bool approxSync = true;
bool subscribeScanCloud = false;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval_, approxSyncMaxInterval_);
pnh.param("topic_queue_size", queueSize_, queueSize_);
if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
{
@@ -142,7 +142,7 @@ private:
NODELET_INFO("RGBDIcpOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
NODELET_INFO("RGBDIcpOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
NODELET_INFO("RGBDIcpOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval_);
NODELET_INFO("RGBDIcpOdometry: topic_queue_size = %d", queueSize_);
NODELET_INFO("RGBDIcpOdometry: sync_queue_size = %d", syncQueueSize_);
NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud = %s", subscribeScanCloud?"true":"false");
@@ -172,8 +172,8 @@ private:
if(approxSync)
{
approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
if(approxSyncMaxInterval > 0.0)
approxCloudSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxCloudSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
}
else
@@ -185,7 +185,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s, \n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str(),
@@ -197,8 +197,8 @@ private:
if(approxSync)
{
approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
if(approxSyncMaxInterval > 0.0)
approxScanSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxScanSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
}
else
@@ -210,7 +210,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str(),
@@ -301,7 +301,7 @@ private:
}
double stampDiff = fabs(image->header.stamp.toSec() - depth->header.stamp.toSec());
if(stampDiff > 0.010)
if(approxSyncMaxInterval_==0.0 && stampDiff > 0.010)
{
NODELET_WARN("The time difference between rgb and depth frames is "
"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
@@ -518,6 +518,7 @@ private:
double scanVoxelSize_;
int scanNormalK_;
double scanNormalRadius_;
double approxSyncMaxInterval_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_odom::RGBDICPOdometry, nodelet::Nodelet);
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+24 -23
View File
@@ -76,7 +76,8 @@ public:
exactSync6_(0),
topicQueueSize_(1),
syncQueueSize_(5),
keepColor_(false)
keepColor_(false),
approxSyncMaxInterval_(0.0)
{
}
@@ -106,10 +107,9 @@ private:
bool approxSync = false;
bool subscribeRGBD = false;
double approxSyncMaxInterval = 0.0;
int rgbdCameras = 1;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval_, approxSyncMaxInterval_);
pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_);
if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
{
@@ -129,7 +129,7 @@ private:
NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
NODELET_INFO("StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
NODELET_INFO("StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval_);
NODELET_INFO("StereoOdometry: topic_queue_size = %d", topicQueueSize_);
NODELET_INFO("StereoOdometry: sync_queue_size = %d", syncQueueSize_);
NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
@@ -168,8 +168,8 @@ private:
MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
}
else
@@ -183,7 +183,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str());
}
@@ -196,8 +196,8 @@ private:
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
}
else
@@ -212,7 +212,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str());
@@ -227,8 +227,8 @@ private:
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
}
else
@@ -244,7 +244,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str(),
@@ -261,8 +261,8 @@ private:
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
}
else
@@ -279,7 +279,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str(),
@@ -298,8 +298,8 @@ private:
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
}
else
@@ -317,7 +317,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str(),
@@ -373,8 +373,8 @@ private:
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_>0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
}
else
@@ -387,7 +387,7 @@ private:
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
@@ -702,7 +702,7 @@ private:
rightInfoMsgs.push_back(*cameraInfoRight);
double stampDiff = fabs(imageLeft->header.stamp.toSec() - imageRight->header.stamp.toSec());
if(stampDiff > 0.010)
if(approxSyncMaxInterval_ == 0.0 && stampDiff > 0.010)
{
NODELET_WARN("The time difference between left and right frames is "
"high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware "
@@ -1075,6 +1075,7 @@ private:
int topicQueueSize_;
int syncQueueSize_;
bool keepColor_;
double approxSyncMaxInterval_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_odom::StereoOdometry, nodelet::Nodelet);
rtabmap_sync/src/nodelets/rgbd_sync.cpp
+8 -7
View File
@@ -65,6 +65,7 @@ public:
depthScale_(1.0),
decimation_(1),
compressedRate_(0),
approxSyncMaxInterval_(0.0),
approxSyncDepth_(0),
exactSyncDepth_(0)
{}
@@ -84,9 +85,8 @@ private:
int queueSize = 1;
int syncQueueSize = 10;
bool approxSync = true;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval_, approxSyncMaxInterval_);
pnh.param("topic_queue_size", queueSize, queueSize);
if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
{
@@ -111,7 +111,7 @@ private:
NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
if(approxSync)
NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval_);
NODELET_INFO("%s: topic_queue_size = %d", getName().c_str(), queueSize);
NODELET_INFO("%s: sync_queue_size = %d", getName().c_str(), syncQueueSize);
NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_);
@@ -124,8 +124,8 @@ private:
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
}
else
@@ -150,7 +150,7 @@ private:
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str());
@@ -181,7 +181,7 @@ private:
double infoStamp = cameraInfo->header.stamp.toSec();
double stampDiff = fabs(rgbStamp - depthStamp);
if(stampDiff > 0.010)
if(approxSyncMaxInterval_==0.0 && stampDiff > 0.010)
{
NODELET_WARN("The time difference between rgb and depth frames is "
"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
@@ -304,6 +304,7 @@ private:
double depthScale_;
int decimation_;
double compressedRate_;
double approxSyncMaxInterval_;
ros::Time lastCompressedPublished_;
rtabmap_sync/src/nodelets/stereo_sync.cpp
+8 -7
View File
@@ -61,6 +61,7 @@ class StereoSync : public nodelet::Nodelet
public:
StereoSync() :
compressedRate_(0),
approxSyncMaxInterval_(0.0),
approxSync_(0),
exactSync_(0)
{}
@@ -80,10 +81,9 @@ private:
int queueSize = 1;
int syncQueueSize = 10;
bool approxSync = false;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
if(approxSync)
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval_, approxSyncMaxInterval_);
pnh.param("topic_queue_size", queueSize, queueSize);
if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
{
@@ -100,7 +100,7 @@ private:
pnh.param("compressed_rate", compressedRate_, compressedRate_);
NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval_);
NODELET_INFO("%s: topic_queue_size = %d", getName().c_str(), queueSize);
NODELET_INFO("%s: sync_queue_size = %d", getName().c_str(), syncQueueSize);
NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
@@ -111,8 +111,8 @@ private:
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
if(approxSyncMaxInterval_>0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_));
approxSync_->registerCallback(boost::bind(&StereoSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
}
else
@@ -138,7 +138,7 @@ private:
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
imageLeftSub_.getTopic().c_str(),
imageRightSub_.getTopic().c_str(),
cameraInfoLeftSub_.getTopic().c_str(),
@@ -173,7 +173,7 @@ private:
double rightInfoStamp = cameraInfoRight->header.stamp.toSec();
double stampDiff = fabs(leftStamp - rightStamp);
if(stampDiff > 0.010)
if(approxSyncMaxInterval_==0.0 && stampDiff > 0.010)
{
NODELET_WARN("The time difference between left and right frames is "
"high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware "
@@ -240,6 +240,7 @@ private:
private:
double compressedRate_;
double approxSyncMaxInterval_;
ros::Time lastCompressedPublished_;
ros::Publisher rgbdImagePub_;