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

Updated for rtabmap 0.19.1. Added VINS example in euroc_datasets.launch

Browse Source
This commit is contained in:
matlabbe
2019-03-18 20:47:13 -04:00
parent 6bd420bc54
commit ab23e651bf
8 changed files with 91 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -31,7 +31,7 @@ find_package(find_object_2d)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.19.0 REQUIRED)
find_package(RTABMap 0.19.1 REQUIRED)
find_package(OpenCV REQUIRED)
include/rtabmap_ros/MsgConversion.h
+8 -1
View File
@@ -113,7 +113,14 @@ void cameraModelToROS(
rtabmap::StereoCameraModel stereoCameraModelFromROS(
const sensor_msgs::CameraInfo & leftCamInfo,
const sensor_msgs::CameraInfo & rightCamInfo,
const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity(),
const rtabmap::Transform & stereoTransform = rtabmap::Transform());
rtabmap::StereoCameraModel stereoCameraModelFromROS(
const sensor_msgs::CameraInfo & leftCamInfo,
const sensor_msgs::CameraInfo & rightCamInfo,
const std::string & frameId,
tf::TransformListener & listener,
double waitForTransform);
void mapDataFromROS(
const rtabmap_ros::MapData & msg,
launch/tests/euroc_datasets.launch
+17 -5
View File
@@ -17,6 +17,10 @@ Examples:
We need to ignore the first 24 seconds for correct VIO initialization (drone should not move).
$ roslaunch rtabmap_ros euroc_datasets.launch args:="-d RGBD/CreateOccupancyGrid false Odom/Strategy 8" MH_seq:=true
$ rosbag play -.-clock -s 24 MH_01_easy.bag
VINS (VIO):
$ roslaunch rtabmap_ros euroc_datasets.launch args:="-d RGBD/CreateOccupancyGrid false Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml" MH_seq:=true raw_images_for_odom:=true
$ rosbag play -.-clock MH_01_easy.bag
-->
<param name="use_sim_time" value="true"/>
@@ -24,6 +28,8 @@ Examples:
<arg name="args" default="-d --RGBD/CreateOccupancyGrid false"/>
<arg name="cfg" default=""/>
<arg name="MH_seq" default="false"/> <!-- For MH sequences, the ground truth is coming from a different topic -->
<arg name="raw_images_for_odom" default="false"/>
<arg name="record_ground_truth" default="true"/>
<arg name="rtabmapviz" default="true"/>
<arg name="rviz" default="false"/>
@@ -37,11 +43,12 @@ Examples:
</node>
<node pkg="rtabmap_ros" type="yaml_to_camera_info.py" name="yaml_to_camera_info_right">
<param name="yaml_path" value="$(find rtabmap_ros)/launch/calibration/euroc_right.yaml"/>
<param name="frame_id" value="cam1"/>
<remap from="image" to="/cam1/image_raw"/>
<remap from="camera_info" to="right/camera_info"/>
</node>
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
<node unless="$(arg raw_images_for_odom)" pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
<remap from="left/image_raw" to="/cam0/image_raw"/>
<remap from="right/image_raw" to="/cam1/image_raw"/>
</node>
@@ -50,7 +57,8 @@ Examples:
<!-- TF frames -->
<node pkg="tf" type="static_transform_publisher" name="imu_base_link" args="0 0 0 3.1415926 -1.570796 0 base_link imu4 5"/>
<node pkg="tf" type="static_transform_publisher" name="camera_imu_link" args="-0.021640 -0.064677 0.009811 1.555925 0.025777 0.003757 imu4 cam0 50"/>
<node pkg="tf" type="static_transform_publisher" name="cam0_imu_link" args="-0.021640 -0.064677 0.009811 1.555925 0.025777 0.003757 imu4 cam0 50"/>
<node pkg="tf" type="static_transform_publisher" name="cam1_imu_link" args="-0.019844 0.045369 0.007862 1.558237 0.025393 0.017907 imu4 cam1 50"/>
<node if="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="leica_base_link" args="0.120209 -0.0184772 -0.0748903 0 0 0 leica base_link_gt 100"/>
<node unless="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="vicon_base_link" args="0.120209 -0.0184772 -0.0748903 0 0 0 vicon/firefly_sbx/firefly_sbx base_link_gt 100"/>
@@ -68,12 +76,16 @@ Examples:
<!-- RTAB-Map -->
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="args" value="$(arg args)"/>
<arg if="$(arg raw_images_for_odom)" name="rtabmap_args" value="$(arg args) --Rtabmap/ImagesAlreadyRectified false"/>
<arg unless="$(arg raw_images_for_odom)" name="rtabmap_args" value="$(arg args)"/>
<arg if="$(arg raw_images_for_odom)" name="odom_args" value="--Rtabmap/ImagesAlreadyRectified false"/>
<arg if="$(arg raw_images_for_odom)" name="left_image_topic" value="/cam0/image_raw"/>
<arg if="$(arg raw_images_for_odom)" name="right_image_topic" value="/cam1/image_raw"/>
<arg name="stereo" value="true"/>
<arg name="frame_id" value="base_link"/>
<arg name="wait_for_transform" value="0.01"/>
<arg name="ground_truth_frame_id" value="world"/>
<arg name="ground_truth_base_frame_id" value="base_link_gt"/>
<arg if="$(arg record_ground_truth)" name="ground_truth_frame_id" value="world"/>
<arg if="$(arg record_ground_truth)" name="ground_truth_base_frame_id" value="base_link_gt"/>
<arg name="cfg" value="$(arg cfg)"/>
<arg name="imu_topic" value="/imu0"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap_ros</name>
<version>0.19.0</version>
<version>0.19.1</version>
<description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
scripts/yaml_to_camera_info.py
+4
View File
@@ -21,7 +21,10 @@ def yaml_to_CameraInfo(yaml_fname):
def callback(image):
global publisher
global camera_info_msg
global frameId
camera_info_msg.header = image.header
if frameId:
camera_info_msg.header.frame_id = frameId
publisher.publish(camera_info_msg)
if __name__ == "__main__":
@@ -33,6 +36,7 @@ if __name__ == "__main__":
print 'yaml_path parameter should be set to path of the calibration file!'
sys.exit(1)
frameId = rospy.get_param('~frame_id', '')
camera_info_msg = yaml_to_CameraInfo(yaml_path)
publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=1)
src/MsgConversion.cpp
+33 -2
View File
@@ -700,13 +700,44 @@ void cameraModelToROS(
rtabmap::StereoCameraModel stereoCameraModelFromROS(
const sensor_msgs::CameraInfo & leftCamInfo,
const sensor_msgs::CameraInfo & rightCamInfo,
const rtabmap::Transform & localTransform)
const rtabmap::Transform & localTransform,
const rtabmap::Transform & stereoTransform)
{
return rtabmap::StereoCameraModel(
"ros",
cameraModelFromROS(leftCamInfo, localTransform),
cameraModelFromROS(rightCamInfo, localTransform),
rtabmap::Transform());
stereoTransform);
}
rtabmap::StereoCameraModel stereoCameraModelFromROS(
const sensor_msgs::CameraInfo & leftCamInfo,
const sensor_msgs::CameraInfo & rightCamInfo,
const std::string & frameId,
tf::TransformListener & listener,
double waitForTransform)
{
rtabmap::Transform localTransform = getTransform(
frameId,
leftCamInfo.header.frame_id,
leftCamInfo.header.stamp,
listener,
waitForTransform);
if(localTransform.isNull())
{
return rtabmap::StereoCameraModel();
}
rtabmap::Transform stereoTransform = getTransform(
leftCamInfo.header.frame_id,
rightCamInfo.header.frame_id,
leftCamInfo.header.stamp,
listener,
waitForTransform);
if(stereoTransform.isNull())
{
return rtabmap::StereoCameraModel();
}
return stereoCameraModelFromROS(leftCamInfo, rightCamInfo, localTransform, stereoTransform);
}
void mapDataFromROS(
src/OdometryROS.cpp
+7 -2
View File
@@ -343,7 +343,10 @@ void OdometryROS::onInit()
odomStrategy_ = 0;
Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
if(waitIMUToinit_ || odomStrategy_ == Odometry::kTypeMSCKF || odomStrategy_ == Odometry::kTypeOkvis)
if(waitIMUToinit_ ||
odomStrategy_ == Odometry::kTypeMSCKF ||
odomStrategy_ == Odometry::kTypeOkvis ||
odomStrategy_ == Odometry::kTypeVINS)
{
int queueSize = 10;
pnh.param("queue_size", queueSize, queueSize);
@@ -414,6 +417,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
{
if(odomStrategy_ != Odometry::kTypeOkvis &&
odomStrategy_ != Odometry::kTypeMSCKF &&
odomStrategy_ != Odometry::kTypeVINS &&
!odometry_->getPose().isIdentity())
{
// For non-inertial odometry approaches, IMU is only used to initialize the initial orientation below
@@ -441,7 +445,8 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
localTransform);
if(odomStrategy_ != Odometry::kTypeOkvis &&
odomStrategy_ != Odometry::kTypeMSCKF)
odomStrategy_ != Odometry::kTypeMSCKF &&
odomStrategy_ != Odometry::kTypeVINS)
{
if(!odometry_->getPose().isIdentity())
{
src/nodelets/stereo_odometry.cpp
+20 -3
View File
@@ -194,10 +194,27 @@ private:
int quality = -1;
if(imageRectLeft->data.size() && imageRectRight->data.size())
{
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform);
if(stereoModel.baseline() <= 0)
bool alreadyRectified = true;
Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
rtabmap::Transform stereoTransform;
if(!alreadyRectified)
{
NODELET_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
stereoTransform = getTransform(
cameraInfoLeft->header.frame_id,
cameraInfoRight->header.frame_id,
cameraInfoLeft->header.stamp);
if(stereoTransform.isNull())
{
NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str());
return;
}
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform, stereoTransform);
if(alreadyRectified && stereoModel.baseline() <= 0)
{
NODELET_ERROR("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
return;
}