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

merged master to devel

Browse Source
This commit is contained in:
matlabbe
2015-06-19 20:46:05 -04:00
parent 206dda272e fa06d3050d
commit de70790e5a
7 changed files with 67 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+4
View File
@@ -19,6 +19,8 @@ find_package(octomap_ros)
# find_package(Boost REQUIRED COMPONENTS system) # find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.10.0 REQUIRED) find_package(RTABMap 0.10.0 REQUIRED)
find_package(OpenCV REQUIRED)
#Qt stuff #Qt stuff
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED) FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
INCLUDE(${QT_USE_FILE}) INCLUDE(${QT_USE_FILE})
@@ -101,11 +103,13 @@ catkin_package(
include_directories( include_directories(
${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/include
${RTABMap_INCLUDE_DIRS} ${RTABMap_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
) )
# libraries # libraries
SET(Libraries SET(Libraries
${OpenCV_LIBRARIES}
${catkin_LIBRARIES} ${catkin_LIBRARIES}
${RTABMap_LIBRARIES} ${RTABMap_LIBRARIES}
) )
launch/demo/demo_turtlebot_mapping.launch
+8 -2
View File
@@ -25,6 +25,7 @@
<arg name="localization" default="false"/> <arg name="localization" default="false"/>
<arg name="rgbd_odometry" default="false"/> <arg name="rgbd_odometry" default="false"/>
<arg name="args" default=""/> <arg name="args" default=""/>
<arg name="version083" default="false"/>
<!-- Navigation stuff (move_base) --> <!-- Navigation stuff (move_base) -->
<include file="$(find turtlebot_bringup)/launch/3dsensor.launch"/> <include file="$(find turtlebot_bringup)/launch/3dsensor.launch"/>
@@ -48,8 +49,8 @@
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/> <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<!-- output --> <!-- output -->
<remap from="grid_map" to="/map"/> <remap unless="$(arg version083)" from="grid_map" to="/map"/>
<!-- <remap from="proj_map" to="/map"/> --> <!-- <remap unless="$(arg version083)" from="proj_map" to="/map"/> -->
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. --> <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM --> <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
@@ -80,5 +81,10 @@
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/> <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
</node> </node>
<!-- backward compatibility with hydro 0.8.3 only -->
<node if="$(arg version083)" pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler">
<remap from="grid_map" to="/map"/>
</node>
</group> </group>
</launch> </launch>
launch/rgbd_mapping_kinect2.launch
+24 -22
View File
@@ -5,19 +5,21 @@
Install Kinect2 : Follow ALL directives at https://github.com/code-iai/iai_kinect2 Install Kinect2 : Follow ALL directives at https://github.com/code-iai/iai_kinect2
Make sure it is calibrated! Make sure it is calibrated!
Run: Run:
$ roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true
$ roslaunch rtabmap_ros rgbd_mapping_kinect2.launch $ roslaunch rtabmap_ros rgbd_mapping_kinect2.launch
$ rosrun kinect2_bridge kinect2_bridge _publish_tf:=true
Prefixes:
sd: rgb_lowres / depth_lowres
hd: rgb_rect / depth_highres
ir: ir_rect / depth_rect
--> -->
<arg name="rgb_prefix" default="rgb_lowres" />
<arg name="depth_prefix" default="depth_lowres" /> <!-- Which image resolution to process in rtabmap: sd, qhd, hd -->
<arg name="resolution" default="qhd" />
<!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published --> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="frame_id" default="kinect2_link"/> <arg name="frame_id" default="kinect2_base_link"/>
<!-- Rotate the camera -->
<arg name="pi/2" value="1.5707963267948966"/>
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="kinect2_base_link"
args="$(arg optical_rotate) kinect2_base_link kinect2_link 100" />
<!-- Choose visualization --> <!-- Choose visualization -->
<arg name="rviz" default="false" /> <arg name="rviz" default="false" />
@@ -38,7 +40,7 @@
<arg name="strategy" default="0" /> <arg name="strategy" default="0" />
<arg name="feature" default="6" /> <arg name="feature" default="6" />
<arg name="nn" default="3" /> <arg name="nn" default="3" />
<arg name="max_depth" default="8.0" /> <arg name="max_depth" default="10.0" />
<arg name="min_inliers" default="20" /> <arg name="min_inliers" default="20" />
<arg name="inlier_distance" default="0.02" /> <arg name="inlier_distance" default="0.02" />
<arg name="local_map" default="1000" /> <arg name="local_map" default="1000" />
@@ -49,9 +51,9 @@
<!-- Odometry --> <!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen"> <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen">
<remap from="rgb/image" to="/kinect2/$(arg rgb_prefix)/image"/> <remap from="rgb/image" to="/kinect2/$(arg resolution)/image_mono_rect"/>
<remap from="depth/image" to="/kinect2/$(arg depth_prefix)/image"/> <remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/>
<remap from="rgb/camera_info" to="/kinect2/$(arg rgb_prefix)/camera_info"/> <remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="approx_sync" type="bool" value="true"/> <param name="approx_sync" type="bool" value="true"/>
@@ -74,9 +76,9 @@
<param name="subscribe_depth" type="bool" value="true"/> <param name="subscribe_depth" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
<remap from="rgb/image" to="/kinect2/$(arg rgb_prefix)/image"/> <remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/>
<remap from="depth/image" to="/kinect2/$(arg depth_prefix)/image"/> <remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/>
<remap from="rgb/camera_info" to="/kinect2/$(arg rgb_prefix)/camera_info"/> <remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
<param name="approx_sync" type="bool" value="true"/> <param name="approx_sync" type="bool" value="true"/>
@@ -91,9 +93,9 @@
<param name="approx_sync" type="bool" value="true"/> <param name="approx_sync" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
<remap from="rgb/image" to="/kinect2/$(arg rgb_prefix)/image"/> <remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/>
<remap from="depth/image" to="/kinect2/$(arg depth_prefix)/image"/> <remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/>
<remap from="rgb/camera_info" to="/kinect2/$(arg rgb_prefix)/camera_info"/> <remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
</node> </node>
</group> </group>
@@ -103,9 +105,9 @@
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) --> <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/> <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet"> <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="/kinect2/$(arg rgb_prefix)/image"/> <remap from="rgb/image_in" to="/kinect2/$(arg resolution)/image_color_rect"/>
<remap from="depth/image_in" to="/kinect2/$(arg depth_prefix)/image"/> <remap from="depth/image_in" to="/kinect2/$(arg resolution)/image_depth_rect"/>
<remap from="rgb/camera_info_in" to="/kinect2/$(arg rgb_prefix)/camera_info"/> <remap from="rgb/camera_info_in" to="/kinect2/$(arg resolution)/camera_info"/>
<remap from="odom_in" to="rtabmap/odom"/> <remap from="odom_in" to="rtabmap/odom"/>
src/CoreWrapper.cpp
+7 -2
View File
@@ -752,7 +752,8 @@ void CoreWrapper::commonDepthCallback(
std::vector<CameraModel> cameraModels; std::vector<CameraModel> cameraModels;
for(unsigned int i=0; i<imageMsgs.size(); ++i) for(unsigned int i=0; i<imageMsgs.size(); ++i)
{ {
if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
@@ -786,7 +787,11 @@ void CoreWrapper::commonDepthCallback(
} }
cv_bridge::CvImageConstPtr ptrImage; cv_bridge::CvImageConstPtr ptrImage;
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
}
else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{ {
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8"); ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
src/GuiWrapper.cpp
+7 -2
View File
@@ -500,7 +500,8 @@ void GuiWrapper::commonDepthCallback(
std::vector<CameraModel> cameraModels; std::vector<CameraModel> cameraModels;
for(unsigned int i=0; i<imageMsgs.size(); ++i) for(unsigned int i=0; i<imageMsgs.size(); ++i)
{ {
if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
@@ -534,7 +535,11 @@ void GuiWrapper::commonDepthCallback(
} }
cv_bridge::CvImageConstPtr ptrImage; cv_bridge::CvImageConstPtr ptrImage;
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
}
else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{ {
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8"); ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
src/RGBDOdometryNode.cpp
+13 -5
View File
@@ -163,7 +163,8 @@ public:
{ {
if(!this->isPaused()) if(!this->isPaused())
{ {
if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
@@ -171,7 +172,9 @@ public:
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 || depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0)) depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
{ {
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended) and image_depth=16UC1,32FC1,mono16"); ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 "
"recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s",
image->encoding.c_str(), depth->encoding.c_str());
return; return;
} }
else if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0) else if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0)
@@ -214,7 +217,7 @@ public:
model.cx(), model.cx(),
model.cy(), model.cy(),
rtabmap_ros::transformFromTF(localTransform)); rtabmap_ros::transformFromTF(localTransform));
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, "mono8"); cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth); cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
rtabmap::SensorData data( rtabmap::SensorData data(
@@ -258,7 +261,8 @@ public:
std::vector<CameraModel> cameraModels; std::vector<CameraModel> cameraModels;
for(unsigned int i=0; i<imageMsgs.size(); ++i) for(unsigned int i=0; i<imageMsgs.size(); ++i)
{ {
if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
@@ -292,7 +296,11 @@ public:
} }
cv_bridge::CvImageConstPtr ptrImage; cv_bridge::CvImageConstPtr ptrImage;
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
}
else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{ {
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8"); ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
src/nodelets/point_cloud_xyzrgb.cpp
+3 -2
View File
@@ -156,7 +156,8 @@ private:
const sensor_msgs::ImageConstPtr& imageDepth, const sensor_msgs::ImageConstPtr& imageDepth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo) const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{ {
if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) && image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
@@ -170,7 +171,7 @@ private:
if(cloudPub_.getNumSubscribers()) if(cloudPub_.getNumSubscribers())
{ {
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image, "bgr8"); cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth); cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
image_geometry::PinholeCameraModel model; image_geometry::PinholeCameraModel model;