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

Added imu input topic to rtabmap node. Updated for rtabmap 0.19.3.

Browse Source
This commit is contained in:
matlabbe
2019-05-31 19:35:49 -04:00
parent 2eb8ba1be1
commit 48bcda84ab
8 changed files with 223 additions and 339 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.1 REQUIRED)
find_package(RTABMap 0.19.3 REQUIRED)
find_package(OpenCV REQUIRED)
include/rtabmap_ros/CoreWrapper.h
+5 -1
View File
@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <nav_msgs/GetMap.h>
#include <nav_msgs/GetPlan.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/Imu.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Rtabmap.h>
@@ -141,6 +142,7 @@ private:
#ifdef WITH_APRILTAGS2_ROS
void tagDetectionsAsyncCallback(const apriltags2_ros::AprilTagDetectionArray & tagDetections);
#endif
void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections);
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
@@ -156,7 +158,7 @@ private:
void process(
const ros::Time & stamp,
const rtabmap::SensorData & data,
rtabmap::SensorData & data,
const rtabmap::Transform & odom = rtabmap::Transform(),
const std::string & odomFrameId = "",
const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
@@ -315,6 +317,8 @@ private:
rtabmap::GPS gps_;
ros::Subscriber tagDetectionsSub_;
std::map<int, geometry_msgs::PoseWithCovarianceStamped> tags_;
ros::Subscriber imuSub_;
std::map<double, rtabmap::Transform> imus_;
bool stereoToDepth_;
bool odomSensorSync_;
launch/rtabmap.launch
+6 -1
View File
@@ -94,7 +94,8 @@
<arg name="odom_guess_frame_id" default=""/>
<arg name="odom_guess_min_translation" default="0"/>
<arg name="odom_guess_min_rotation" default="0"/>
<arg name="imu_topic" default="/imu"/> <!-- only used with VIO approaches -->
<arg name="imu_topic" default="/imu/data"/> <!-- only used with VIO approaches -->
<arg name="wait_imu_to_init" default="false"/>
<arg name="subscribe_user_data" default="false"/> <!-- user data synchronized subscription -->
<arg name="user_data_topic" default="/user_data"/>
@@ -190,6 +191,7 @@
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
@@ -215,6 +217,7 @@
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
@@ -238,6 +241,7 @@
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
@@ -293,6 +297,7 @@
<remap from="gps/fix" to="$(arg gps_topic)"/>
<remap from="tag_detections" to="$(arg tag_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
launch/tests/euroc_datasets.launch
+26 -10
View File
@@ -11,29 +11,35 @@ Examples:
$ rosbag play -.-clock MH_01_easy.bag
MSCKF (VIO):
$ roslaunch rtabmap_ros euroc_datasets.launch args:="-d RGBD/CreateOccupancyGrid false Odom/Strategy 8"
$ roslaunch rtabmap_ros euroc_datasets.launch args:="--Odom/Strategy 8"
$ rosbag play -.-clock V1_01_easy.bag
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
$ roslaunch rtabmap_ros euroc_datasets.launch args:="--Odom/Strategy 8" MH_seq:=true
$ rosbag play -.-clock -s 24 MH_01_easy.bag
OKVIS (VIO):
$ roslaunch rtabmap_ros euroc_datasets.launch args:="-d RGBD/CreateOccupancyGrid false Odom/Strategy 6 OdomOKVIS/ConfigPath ~/okvis/config/config_fpga_p2_euroc.yaml" MH_seq:=true raw_images_for_odom:=true
$ roslaunch rtabmap_ros euroc_datasets.launch args:="--Odom/Strategy 6 OdomOKVIS/ConfigPath ~/okvis/config/config_fpga_p2_euroc.yaml" MH_seq:=true raw_images_for_odom:=true
$ rosbag play -.-clock 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
$ roslaunch rtabmap_ros euroc_datasets.launch args:="--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
VINS (VO):
$ 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_config.yaml" MH_seq:=true raw_images_for_odom:=true
$ roslaunch rtabmap_ros euroc_datasets.launch args:="--Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml" MH_seq:=true raw_images_for_odom:=true
$ rosbag play -.-clock MH_01_easy.bag
-->
<param name="use_sim_time" value="true"/>
<arg name="args" default="-d --RGBD/CreateOccupancyGrid false"/>
<arg name="feature_type" default="6"/>
<arg name="gravity_opt" default="false"/> <!-- Rtabmap will use IMU data to add gravity constraints to graph -->
<arg name="args" default=""/>
<arg if="$(arg gravity_opt)" name="common_args" default="-d --RGBD/CreateOccupancyGrid false --Odom/FeatureType $(arg feature_type) --Kp/DetectorStrategy $(arg feature_type) --Optimizer/GravitySigma 0.3 $(arg args)"/>
<arg unless="$(arg gravity_opt)" name="common_args" default="-d --RGBD/CreateOccupancyGrid false --Odom/FeatureType $(arg feature_type) --Kp/DetectorStrategy $(arg feature_type) $(arg args) "/>
<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"/>
@@ -66,9 +72,11 @@ 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="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 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"/>
<!-- For MH sequences, /leica/position doesn't give the orientation, so minimal ground truth error could be as high as 12 cm -->
<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"/>
<node unless="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="vicon_base_link" args="0.12395 -0.02781 -0.06901 0 0 0 vicon/firefly_sbx/firefly_sbx base_link_gt 100"/>
<node if="$(arg MH_seq)" pkg="rtabmap_ros" type="point_to_tf.py" name="point_to_tf">
<remap from="point" to="/leica/position"/>
@@ -82,6 +90,13 @@ Examples:
</node>
<node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />
<node pkg="imu_complementary_filter" type="complementary_filter_node" name="imu_filter" output="screen">
<remap from="imu/data_raw" to="/imu0"/>
<param name="use_mag" value="false"/>
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
<!-- RTAB-Map -->
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg if="$(arg raw_images_for_odom)" name="rtabmap_args" value="$(arg args) --Rtabmap/ImagesAlreadyRectified false"/>
@@ -91,13 +106,14 @@ Examples:
<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="wait_for_transform" value="0.1"/>
<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="imu_topic" value="/imu/data"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
<arg name="rviz" value="$(arg rviz)"/>
<arg name="wait_imu_to_init" value="true"/>
</include>
</launch>
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap_ros</name>
<version>0.19.1</version>
<version>0.19.3</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>
src/CoreWrapper.cpp
+124 -273
View File
@@ -653,6 +653,7 @@ void CoreWrapper::onInit()
#ifdef WITH_APRILTAGS2_ROS
tagDetectionsSub_ = nh.subscribe("tag_detections", 1, &CoreWrapper::tagDetectionsAsyncCallback, this);
#endif
imuSub_ = nh.subscribe("imu", 100, &CoreWrapper::imuAsyncCallback, this);
}
CoreWrapper::~CoreWrapper()
@@ -1126,12 +1127,6 @@ void CoreWrapper::commonDepthCallbackImpl(
}
}
Transform groundTruthPose;
if(!groundTruthFrameId_.empty())
{
groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
}
cv::Mat userData;
if(userDataMsg.get())
{
@@ -1158,68 +1153,6 @@ void CoreWrapper::commonDepthCallbackImpl(
lastPoseIntermediate_?-1:imageMsgs[0]->header.seq,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
data.setGroundTruth(groundTruthPose);
//global pose
if(!globalPose_.header.stamp.isZero())
{
// assume sensor is fixed
Transform sensorToBase = rtabmap_ros::getTransform(
globalPose_.header.frame_id,
frameId_,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
if(!sensorToBase.isNull())
{
Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
// Correction of the global pose accounting the odometry movement since we received it
Transform correction = rtabmap_ros::getTransform(
frameId_,
odomFrameId,
globalPose_.header.stamp,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
if(!correction.isNull())
{
globalPose *= correction;
}
else
{
NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
"If odometry is small since it received the global pose and "
"covariance is large, this should not be a problem.");
}
cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
data.setGlobalPose(globalPose, globalPoseCovariance);
}
}
globalPose_.header.stamp = ros::Time(0);
if(gps_.stamp() > 0.0)
{
data.setGPS(gps_);
}
gps_ = rtabmap::GPS();
//tag detections
Landmarks landmarks = rtabmap_ros::landmarksFromROS(
tags_,
frameId_,
odomFrameId,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0,
landmarkDefaultLinVariance_,
landmarkDefaultAngVariance_);
tags_.clear();
if(!landmarks.empty())
{
data.setLandmarks(landmarks);
}
OdometryInfo odomInfo;
if(odomInfoMsg.get())
@@ -1392,12 +1325,6 @@ void CoreWrapper::commonStereoCallback(
}
}
Transform groundTruthPose;
if(!groundTruthFrameId_.empty())
{
groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
}
cv::Mat userData;
if(userDataMsg.get())
{
@@ -1424,68 +1351,6 @@ void CoreWrapper::commonStereoCallback(
lastPoseIntermediate_?-1:leftImageMsg->header.seq,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
data.setGroundTruth(groundTruthPose);
//global pose
if(!globalPose_.header.stamp.isZero())
{
// assume sensor is fixed
Transform sensorToBase = rtabmap_ros::getTransform(
globalPose_.header.frame_id,
frameId_,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
if(!sensorToBase.isNull())
{
Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
// Correction of the global pose accounting the odometry movement since we received it
Transform correction = rtabmap_ros::getTransform(
frameId_,
odomFrameId,
globalPose_.header.stamp,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
if(!correction.isNull())
{
globalPose *= correction;
}
else
{
NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
"If odometry is small since it received the global pose and "
"covariance is large, this should not be a problem.");
}
cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
data.setGlobalPose(globalPose, globalPoseCovariance);
}
}
globalPose_.header.stamp = ros::Time(0);
if(gps_.stamp() > 0.0)
{
data.setGPS(gps_);
}
gps_ = rtabmap::GPS();
//tag detections
Landmarks landmarks = rtabmap_ros::landmarksFromROS(
tags_,
frameId_,
odomFrameId,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0,
landmarkDefaultLinVariance_,
landmarkDefaultAngVariance_);
tags_.clear();
if(!landmarks.empty())
{
data.setLandmarks(landmarks);
}
OdometryInfo odomInfo;
if(odomInfoMsg.get())
@@ -1588,12 +1453,6 @@ void CoreWrapper::commonLaserScanCallback(
}
}
Transform groundTruthPose;
if(!groundTruthFrameId_.empty())
{
groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
}
cv::Mat userData;
if(userDataMsg.get())
{
@@ -1631,52 +1490,6 @@ void CoreWrapper::commonLaserScanCallback(
lastPoseIntermediate_?-1:scan2dMsg.get() != 0?scan2dMsg->header.seq:scan3dMsg->header.seq,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
data.setGroundTruth(groundTruthPose);
//global pose
if(!globalPose_.header.stamp.isZero())
{
// assume sensor is fixed
Transform sensorToBase = rtabmap_ros::getTransform(
globalPose_.header.frame_id,
frameId_,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
if(!sensorToBase.isNull())
{
Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
// Correction of the global pose accounting the odometry movement since we received it
Transform correction = rtabmap_ros::getTransform(
frameId_,
odomFrameId,
globalPose_.header.stamp,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
if(!correction.isNull())
{
globalPose *= correction;
}
else
{
NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
"If odometry is small since it received the global pose and "
"covariance is large, this should not be a problem.");
}
cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
data.setGlobalPose(globalPose, globalPoseCovariance);
}
}
globalPose_.header.stamp = ros::Time(0);
if(gps_.stamp() > 0.0)
{
data.setGPS(gps_);
}
gps_ = rtabmap::GPS();
OdometryInfo odomInfo;
if(odomInfoMsg.get())
@@ -1684,22 +1497,6 @@ void CoreWrapper::commonLaserScanCallback(
odomInfo = odomInfoFromROS(*odomInfoMsg);
}
//tag detections
Landmarks landmarks = rtabmap_ros::landmarksFromROS(
tags_,
frameId_,
odomFrameId,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0,
landmarkDefaultLinVariance_,
landmarkDefaultAngVariance_);
tags_.clear();
if(!landmarks.empty())
{
data.setLandmarks(landmarks);
}
process(lastPoseStamp_,
data,
lastPose_,
@@ -1724,12 +1521,6 @@ void CoreWrapper::commonOdomCallback(
return;
}
Transform groundTruthPose;
if(!groundTruthFrameId_.empty())
{
groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
}
cv::Mat userData;
if(userDataMsg.get())
{
@@ -1766,52 +1557,6 @@ void CoreWrapper::commonOdomCallback(
lastPoseIntermediate_?-1:odomMsg->header.seq,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
data.setGroundTruth(groundTruthPose);
//global pose
if(!globalPose_.header.stamp.isZero())
{
// assume sensor is fixed
Transform sensorToBase = rtabmap_ros::getTransform(
globalPose_.header.frame_id,
frameId_,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
if(!sensorToBase.isNull())
{
Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
// Correction of the global pose accounting the odometry movement since we received it
Transform correction = rtabmap_ros::getTransform(
frameId_,
odomFrameId,
globalPose_.header.stamp,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
if(!correction.isNull())
{
globalPose *= correction;
}
else
{
NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
"If odometry is small since it received the global pose and "
"covariance is large, this should not be a problem.");
}
cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
data.setGlobalPose(globalPose, globalPoseCovariance);
}
}
globalPose_.header.stamp = ros::Time(0);
if(gps_.stamp() > 0.0)
{
data.setGPS(gps_);
}
gps_ = rtabmap::GPS();
OdometryInfo odomInfo;
if(odomInfoMsg.get())
@@ -1819,22 +1564,6 @@ void CoreWrapper::commonOdomCallback(
odomInfo = odomInfoFromROS(*odomInfoMsg);
}
//tag detections
Landmarks landmarks = rtabmap_ros::landmarksFromROS(
tags_,
frameId_,
odomFrameId,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0,
landmarkDefaultLinVariance_,
landmarkDefaultAngVariance_);
tags_.clear();
if(!landmarks.empty())
{
data.setLandmarks(landmarks);
}
process(lastPoseStamp_,
data,
lastPose_,
@@ -1847,7 +1576,7 @@ void CoreWrapper::commonOdomCallback(
void CoreWrapper::process(
const ros::Time & stamp,
const SensorData & data,
SensorData & data,
const Transform & odom,
const std::string & odomFrameId,
const cv::Mat & odomCovariance,
@@ -1856,6 +1585,96 @@ void CoreWrapper::process(
UTimer timer;
if(rtabmap_.isIDsGenerated() || data.id() > 0)
{
//Add async stuff
Transform groundTruthPose;
if(!groundTruthFrameId_.empty())
{
groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
}
data.setGroundTruth(groundTruthPose);
//global pose
if(!globalPose_.header.stamp.isZero())
{
// assume sensor is fixed
Transform sensorToBase = rtabmap_ros::getTransform(
globalPose_.header.frame_id,
frameId_,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
if(!sensorToBase.isNull())
{
Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
// Correction of the global pose accounting the odometry movement since we received it
Transform correction = rtabmap_ros::getTransform(
frameId_,
odomFrameId,
globalPose_.header.stamp,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
if(!correction.isNull())
{
globalPose *= correction;
}
else
{
NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
"If odometry is small since it received the global pose and "
"covariance is large, this should not be a problem.");
}
cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
data.setGlobalPose(globalPose, globalPoseCovariance);
}
}
globalPose_.header.stamp = ros::Time(0);
if(gps_.stamp() > 0.0)
{
data.setGPS(gps_);
}
gps_ = rtabmap::GPS();
//tag detections
Landmarks landmarks = rtabmap_ros::landmarksFromROS(
tags_,
frameId_,
odomFrameId,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0,
landmarkDefaultLinVariance_,
landmarkDefaultAngVariance_);
tags_.clear();
if(!landmarks.empty())
{
data.setLandmarks(landmarks);
}
// IMU
if(!imus_.empty())
{
double stampDiff = 0.0;
Transform t = Transform::getClosestTransform(imus_, data.stamp(), &stampDiff);
if(!t.isNull() && stampDiff == 0.0)
{
Eigen::Quaterniond q = t.getQuaterniond();
data.setIMU(IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
cv::Vec3d(), cv::Mat(),
cv::Vec3d(), cv::Mat(),
Transform::getIdentity()));
}
else
{
ROS_WARN("We are receiving imu data (buffer=%d), but cannot interpolate "
"imu transform at time %f (closest is at %f). IMU won't be added to graph.",
(int)imus_.size(), data.stamp(), stampDiff);
}
}
double timeRtabmap = 0.0;
double timeUpdateMaps = 0.0;
double timePublishMaps = 0.0;
@@ -2198,6 +2017,37 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltags2_ros::AprilTagDetec
}
#endif
void CoreWrapper::imuAsyncCallback(const sensor_msgs::ImuConstPtr & msg)
{
if(!paused_)
{
if(msg->orientation.x == 0 && msg->orientation.y == 0 && msg->orientation.z == 0 && msg->orientation.w == 0)
{
UERROR("IMU received doesn't have orientation set, it is ignored.");
}
else
{
double stamp = msg->header.stamp.toSec();
rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
if(frameId_.compare(msg->header.frame_id) != 0)
{
localTransform = getTransform(frameId_, msg->header.frame_id, msg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
if(localTransform.isNull())
{
return;
}
}
Transform orientation(0,0,0, msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
imus_.insert(std::make_pair(msg->header.stamp.toSec(), orientation*localTransform.inverse()));
if(imus_.size() > 1000)
{
imus_.erase(imus_.begin());
}
}
}
}
void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
{
@@ -2471,6 +2321,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
userDataMutex_.lock();
userData_ = cv::Mat();
userDataMutex_.unlock();
imus_.clear();
return true;
}
src/GuiNode.cpp
+1
View File
@@ -71,6 +71,7 @@ int main(int argc, char** argv)
// Now wait for application to finish
int r = app->exec();// MUST be called by the Main Thread
ROS_INFO("rtabmapviz stopping spinner...");
spinner->stop();
delete spinner;
src/OdometryROS.cpp
+59 -52
View File
@@ -343,10 +343,7 @@ void OdometryROS::onInit()
odomStrategy_ = 0;
Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
if(waitIMUToinit_ ||
odomStrategy_ == Odometry::kTypeMSCKF ||
odomStrategy_ == Odometry::kTypeOkvis ||
odomStrategy_ == Odometry::kTypeVINS)
if(waitIMUToinit_ || odometry_->canProcessIMU())
{
int queueSize = 10;
pnh.param("queue_size", queueSize, queueSize);
@@ -415,9 +412,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
{
if(!this->isPaused())
{
if(odomStrategy_ != Odometry::kTypeOkvis &&
odomStrategy_ != Odometry::kTypeMSCKF &&
odomStrategy_ != Odometry::kTypeVINS &&
if(!odometry_->canProcessIMU() &&
!odometry_->getPose().isIdentity())
{
// For non-inertial odometry approaches, IMU is only used to initialize the initial orientation below
@@ -437,16 +432,15 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
return;
}
IMU imu(
IMU imu(cv::Vec4d(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w),
cv::Mat(3,3,CV_64FC1,(void*)msg->orientation_covariance.data()).clone(),
cv::Vec3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z),
cv::Mat(3,3,CV_64FC1,(void*)msg->angular_velocity_covariance.data()).clone(),
cv::Vec3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z),
cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(),
localTransform);
if(odomStrategy_ != Odometry::kTypeOkvis &&
odomStrategy_ != Odometry::kTypeMSCKF &&
odomStrategy_ != Odometry::kTypeVINS)
if(!odometry_->canProcessIMU())
{
if(!odometry_->getPose().isIdentity())
{
@@ -454,27 +448,39 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
return;
}
if( imu.linearAcceleration()[0]!=0.0 &&
imu.linearAcceleration()[1]!=0.0 &&
imu.linearAcceleration()[2]!=0.0 &&
!imu.localTransform().isNull())
// align with gravity
if(!imu.localTransform().isNull())
{
// align with gravity
Eigen::Vector3f n(imu.linearAcceleration()[0], imu.linearAcceleration()[1], imu.linearAcceleration()[2]);
n = imu.localTransform().rotation().toEigen3f() * n;
n.normalize();
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), 0);
this->reset(rotation);
float r,p,y;
rotation.getEulerAngles(r,p,y);
NODELET_WARN("odometry: Initialized odometry orientation with IMU (rpy = %f %f %f).", r,p,y);
if(imu.orientation()[0] != 0 || imu.orientation()[1] != 0 || imu.orientation()[2] != 0 || imu.orientation()[3] != 0)
{
Transform rotation(0,0,0, imu.orientation()[0], imu.orientation()[1], imu.orientation()[2], imu.orientation()[3]);
rotation = rotation * imu.localTransform().rotation().inverse();
this->reset(rotation);
float r,p,y;
rotation.getEulerAngles(r,p,y);
NODELET_WARN("odometry: Initialized odometry with IMU's orientation (rpy = %f %f %f).", r,p,y);
}
else if(imu.linearAcceleration()[0]!=0.0 &&
imu.linearAcceleration()[1]!=0.0 &&
imu.linearAcceleration()[2]!=0.0 &&
!imu.localTransform().isNull())
{
Eigen::Vector3f n(imu.linearAcceleration()[0], imu.linearAcceleration()[1], imu.linearAcceleration()[2]);
n = imu.localTransform().rotation().toEigen3f() * n;
n.normalize();
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), 0);
this->reset(rotation);
float r,p,y;
rotation.getEulerAngles(r,p,y);
NODELET_WARN("odometry: Initialized odometry with IMU's accelerometer (rpy = %f %f %f).", r,p,y);
}
}
}
else
@@ -493,6 +499,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
return;
}
Transform groundTruth;
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
{
if(previousStamp_>0.0 && previousStamp_ >= stamp.toSec())
@@ -509,36 +516,36 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
1.0/(stamp.toSec()-previousStamp_), expectedUpdateRate_, previousStamp_, stamp.toSec());
return;
}
}
Transform groundTruth;
if(!groundTruthFrameId_.empty())
{
groundTruth = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
if(!groundTruthFrameId_.empty())
{
if(odometry_->getPose().isIdentity())
groundTruth = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
{
// sync with the first value of the ground truth
if(groundTruth.isNull())
if(odometry_->getPose().isIdentity())
{
NODELET_WARN("Ground truth frames \"%s\" -> \"%s\" are set but failed to "
"get them, odometry won't be initialized with ground truth.",
groundTruthFrameId_.c_str(), groundTruthBaseFrameId_.c_str());
}
else
{
NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
groundTruth.prettyPrint().c_str(),
groundTruthFrameId_.c_str(),
groundTruthBaseFrameId_.c_str());
odometry_->reset(groundTruth);
// sync with the first value of the ground truth
if(groundTruth.isNull())
{
NODELET_WARN("Ground truth frames \"%s\" -> \"%s\" are set but failed to "
"get them, odometry won't be initialized with ground truth.",
groundTruthFrameId_.c_str(), groundTruthBaseFrameId_.c_str());
}
else
{
NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
groundTruth.prettyPrint().c_str(),
groundTruthFrameId_.c_str(),
groundTruthBaseFrameId_.c_str());
odometry_->reset(groundTruth);
}
}
}
}
}
Transform guessCurrentPose;
if(!guessFrameId_.empty())
{