Re: Problem fusing LiDAR and depth (hector_slam)
Posted by utrecht on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Problem-fusing-LiDAR-and-depth-hector-slam-tp4115p4121.html
Hey Mathieu,
I've succeeded in integrating the LiDAR and IMU. Well, sort of... couple of questions:
1. When running without the IMU I'm running into a bit of trouble with saving the map. It keeps disappearing after losing odometry (the R200 really sucks sometimes...) as seen below:
Before:
After:
Is this normal? It's very annoying, and I just wanted to know if I am getting it right. Even if it's not possible to keep the combined clouds from the R200, is it possible to not erase the assembled clouds from the LiDAR? (note, I used the hector_laser_to_pointcloud for convenience, check it out if you have the time https://github.com/team-vigir/vigir_lidar_proc/blob/master/vigir_laserscan_to_pointcloud/src/laserscan_to_pointcloud_node.cpp)
2. After combining with the LiDAR, I've come across a duplication problem once more, as seen below:
I've managed to correct it, thing is the pointcloud topic reverses in its yaw. If I remove it everything looks fine:
In the picture the pointcloud topic is the weird out of place one. If I uncheck the box in RVIZ everything looks ok.
Why do you think this is happening? Instead of using the robot_localization package I've decided to write my own node to publish the odometry information from the odom topic. It's unclean at the moment, but take a look:
In addition, here is the TF tree and the launch file:
BTW, maple is the name of my package. The nodes are pretty self-explanatory I assume. Does anything jump at you? Something obviously wrong?
Many gratitudes :)
URL: http://official-rtab-map-forum.206.s1.nabble.com/Problem-fusing-LiDAR-and-depth-hector-slam-tp4115p4121.html
Hey Mathieu,
I've succeeded in integrating the LiDAR and IMU. Well, sort of... couple of questions:
1. When running without the IMU I'm running into a bit of trouble with saving the map. It keeps disappearing after losing odometry (the R200 really sucks sometimes...) as seen below:
Before:
After:
Is this normal? It's very annoying, and I just wanted to know if I am getting it right. Even if it's not possible to keep the combined clouds from the R200, is it possible to not erase the assembled clouds from the LiDAR? (note, I used the hector_laser_to_pointcloud for convenience, check it out if you have the time https://github.com/team-vigir/vigir_lidar_proc/blob/master/vigir_laserscan_to_pointcloud/src/laserscan_to_pointcloud_node.cpp)
2. After combining with the LiDAR, I've come across a duplication problem once more, as seen below:
I've managed to correct it, thing is the pointcloud topic reverses in its yaw. If I remove it everything looks fine:
In the picture the pointcloud topic is the weird out of place one. If I uncheck the box in RVIZ everything looks ok.
Why do you think this is happening? Instead of using the robot_localization package I've decided to write my own node to publish the odometry information from the odom topic. It's unclean at the moment, but take a look:
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
class State {
private:
ros::NodeHandle nh_;
ros::Subscriber imu_sub_;
ros::Subscriber realsense_pose_sub_;
ros::Subscriber lidar_pose_sub_;
geometry_msgs::PoseStamped lidar_msg_;
nav_msgs::Odometry realsense_msg_;
tf::TransformBroadcaster br_;
tf::Transform transform_;
public:
State() {
imu_sub_ = nh_.subscribe("imu", 1, &State::imuCallback, this);
realsense_pose_sub_ = nh_.subscribe("rtabmap/odom", 1, &State::RealsenseCallback, this);
transform_.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
transform_.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
}
void imuCallback(const sensor_msgs::Imu::ConstPtr &imu){
tf::Quaternion quat = tf::Quaternion(imu->orientation.x,
imu->orientation.y,
imu->orientation.z,
imu->orientation.w);
transform_.setRotation(quat);
br_.sendTransform(tf::StampedTransform(transform_, ros::Time::now(), "map", "base_footprint"));
}
void RealsenseCallback(const nav_msgs::Odometry::ConstPtr &msg){
transform_.setOrigin(tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z));
br_.sendTransform(tf::StampedTransform(transform_, ros::Time::now(), "map", "base_footprint"));
}
};
int main(int argc, char** argv){
ros::init(argc, argv, "state_tf");
State s;
ros::spin();
return 0;
}
In addition, here is the TF tree and the launch file:
<?xml version="1.0"?>
<launch>
<arg name="rtabmapviz" default="false" />
<arg name="rviz" default="true" />
<arg name="localization" default="false"/>
<node pkg="maple" type="ScanToPointcloud2" name="ScanToPointcloud2" respawn="true">
<param name="min_range" value="0.25" />
<param name="use_high_fidelity_projection" value="true" />
<param name="target_frame" value="base_link" />
</node>
<node pkg="maple" type="IMUPositionEstimate" name="IMUPositionEstimate" />
<!-- sim time for convenience, if playing a rosbag -->
<arg name="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<!-- Corresponding config files -->
<arg name="cfg" default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
<arg name="gui_cfg" default="~/.ros/rtabmap_gui.ini" />
<arg name="rviz_cfg" default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
<arg name="frame_id" default="base_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic -->
<arg name="map_frame_id" default="map"/>
<arg name="ground_truth_frame_id" default=""/> <!-- e.g., "world" -->
<arg name="ground_truth_base_frame_id" default=""/> <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
<arg name="namespace" default="rtabmap"/>
<arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="queue_size" default="10"/>
<arg name="wait_for_transform" default="0.2"/>
<arg name="args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="rtabmap_args" default="--delete_db_on_start"/> <!-- deprecated, use "args" argument -->
<arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
<arg name="output" default="screen"/> <!--Control node output (screen or log)-->
<arg name="approx_sync" default="true"/>
<!-- RGB-D related topics -->
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="depth_topic" default="/camera/depth_registered/sw_registered/image_rect_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<arg name="image_raw_topic" default="/camera/depth/points/image_raw" />
<arg name="cloud_topic" default="/camera/depth/points" />
<arg name="image_topic" default="/camera/depth/image" />
<arg name="_approx" default="false" />
<arg name="_fill_holes_size" default="2" />
<!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
<arg name="subscribe_rgbd" default="false"/>
<arg name="rgbd_topic" default="/camera/rgbd_image" />
<arg name="compressed" default="true"/> <!-- If you want to subscribe to compressed image topics -->
<arg name="rgb_image_transport" default="compressed"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
<arg name="depth_image_transport" default="compressedDepth"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
<arg name="subscribe_scan" default="false"/>
<arg name="scan_topic" default="/scan"/>
<arg name="subscribe_scan_cloud" default="true"/>
<arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="scan_normal_k" default="0"/>
<arg name="visual_odometry" default="true"/> <!-- Launch rtabmap visual odometry node -->
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="vo_frame_id" default="odom"/>
<arg name="odom_tf_angular_variance" default="1"/> <!-- If TF is used to get odometry, this is the default angular variance -->
<arg name="odom_tf_linear_variance" default="1"/> <!-- If TF is used to get odometry, this is the default linear variance -->
<arg name="odom_args" default="$(arg rtabmap_args)"/>
<arg name="odom_sensor_sync" default="false"/>
<arg name="subscribe_user_data" default="false"/> <!-- user data synchronized subscription -->
<arg name="user_data_topic" default="/user_data"/>
<arg name="user_data_async_topic" default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
<!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
<arg if="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)_relay"/>
<arg unless="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)"/>
<arg if="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)_relay"/>
<arg unless="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)"/>
<!-- Nodes -->
<group ns="$(arg namespace)">
<!-- RGB-D Odometry -->
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic)"/>
<param name="publish_tf" type="bool" value="true"/>
<param name="guess_from_tf" type="bool" value="false"/>
<param name="guess_frame_id" type="string" value="base_footprint"/>
<param name="Odom/FillInfoData" type="string" value="true"/>
<param name="Vis/FeatureType" type="string" value="6"/>
<param name="OdomF2M/MaxSize" type="string" value="1000"/>
<param name="Vis/MinInliers" value="8"/>
<param name="Odom/ResetCountdown" value="1" />
<!-- 0: 3D - 3D, 1: 3D-2D -->
<param name="Odom/EstimationType" value="1"/>
<!-- Minimum Eucliedean distance in Mode 0 (3D-3D) -->
<param name="Odom/InlinerDistance" value="0.2" />
<!-- Reprojection error in Mode 1 (3D->2D) -->
<param name="Odom/PnPReprojError" value="5.0" />
<!-- Maximum depth -->
<param name="Odom/MaxDepth" value="10.0" />
<param name="GFFT/QualityLevel" value="0.001" />
<param name="GFFT/MinDistance" value="3" />
<param name="Kp/DetectorStrategy" value="6" />
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<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="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)"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_user_data" type="bool" value="$(arg subscribe_user_data)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="map_frame_id" type="string" value="$(arg map_frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<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="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
<param name="odom_tf_linear_variance" type="double" value="$(arg odom_tf_linear_variance)"/>
<param name="odom_sensor_sync" type="bool" value="$(arg odom_sensor_sync)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<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)"/>
<param name="scan_normal_k" type="int" value="$(arg scan_normal_k)"/>
<param name="Rtabmap/DetectionRate" type="string" value="0"/>
<param name="RGBD/LinearUpdate" type="string" value="0"/>
<param name="RGBD/AngularUpdate" type="string" value="0"/>
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
<param name="Grid/FromDepth" type="string" value="true"/>
<param name="RGBD/OptimizeMaxError" value="0.2" />
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="user_data" to="$(arg user_data_topic)"/>
<remap from="user_data_async" to="$(arg user_data_async_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="cloud" to="voxel_cloud" />
<param name="decimation" type="double" value="2"/>
<param name="voxel_size" type="double" value="0.05"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
</node>
<node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0 0 0 0 base_footprint base_link 100"/>
<node pkg="tf" type="static_transform_publisher" name="base_link_to_camera" args="0 0 0 0 0 0 base_link camera_link 100"/>
</launch>BTW, maple is the name of my package. The nodes are pretty self-explanatory I assume. Does anything jump at you? Something obviously wrong?
Many gratitudes :)
| Free forum by Nabble | Edit this page |