i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

rtabmap.launch: added odom_topic argument to odometry nodes. OdometryROS: using odom info local map as output local map.

Browse Source
This commit is contained in:
matlabbe
2017-12-18 15:44:31 -05:00
parent ea5707e267
commit acc607a8e6
4 changed files with 17 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+4
View File
@@ -359,6 +359,8 @@ install(TARGETS
map_assembler
map_optimizer
data_player
odom_msg_to_tf
pointcloud_to_depthimage
camera
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@@ -374,6 +376,8 @@ install(TARGETS
map_assembler
map_optimizer
data_player
odom_msg_to_tf
pointcloud_to_depthimage
camera
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
launch/demo/demo_stereo_outdoor.launch
-1
View File
@@ -57,7 +57,6 @@
<param name="Odom/GuessMotion" type="string" value="true"/>
<param name="Vis/MinInliers" type="string" value="10"/>
<param if="$(arg local_bundle)" name="OdomF2M/BundleAdjustment" type="string" value="1"/>
<param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
<param name="OdomF2M/MaxSize" type="string" value="1000"/>
<param name="GFTT/MinDistance" type="string" value="10"/>
<param name="GFTT/QualityLevel" type="string" value="0.00001"/>
launch/rtabmap.launch
+4 -3
View File
@@ -82,7 +82,7 @@
<arg name="visual_odometry" default="true"/> <!-- Launch rtabmap visual odometry node -->
<arg name="icp_odometry" default="false"/> <!-- Launch rtabmap icp odometry node -->
<arg name="odom_topic" default="odom"/> <!-- Odometry topic name -->
<arg name="vo_frame_id" default="odom"/>
<arg name="vo_frame_id" default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF -->
<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=""/> <!-- More arguments for odometry (overwrite same parameters in rtabmap_args) -->
@@ -135,8 +135,8 @@
<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="odom" to="$(arg odom_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
@@ -158,6 +158,7 @@
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
@@ -178,6 +179,7 @@
<node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
@@ -191,7 +193,6 @@
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
</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)">
src/OdometryROS.cpp
+9 -5
View File
@@ -531,13 +531,17 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
}
// local map / reference frame
if(odomLocalMap_.getNumSubscribers() && odometry_->getType() == Odometry::kTypeF2M)
if(odomLocalMap_.getNumSubscribers() && !info.localMap.empty())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3();
for(std::multimap<int, cv::Point3f>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
pcl::PointCloud<pcl::PointXYZRGB> cloud;
for(std::map<int, cv::Point3f>::const_iterator iter=info.localMap.begin(); iter!=info.localMap.end(); ++iter)
{
cloud.push_back(pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z));
bool inlier = info.words.find(iter->first) != info.words.end();
pcl::PointXYZRGB pt(inlier?0:255, 255, 0);
pt.x = iter->second.x;
pt.y = iter->second.y;
pt.z = iter->second.z;
cloud.push_back(pt);
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);