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

ros-pkg: updated with new RTAB-Map stereo features

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1862 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-16 00:17:11 +00:00
parent 2599e3ecfb
commit 8bd2447ca8
10 changed files with 364 additions and 315 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/azimut3/az3_mapping_client_rviz.launch
+1 -1
View File
@@ -2,7 +2,7 @@
<launch> <launch>
<!-- Remote teleop --> <!-- Remote teleop -->
<include file="$(find az3_bringup)/joystick.launch"/> <!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
<!-- We have two nodes (grid_map_assembler and rviz) subscribing to /rtabmap/mapData, so use --> <!-- We have two nodes (grid_map_assembler and rviz) subscribing to /rtabmap/mapData, so use -->
<!-- a relay on this machine, same for images --> <!-- a relay on this machine, same for images -->
launch/tests/test_stereo_mapping.launch
+12 -28
View File
@@ -21,12 +21,6 @@
</node> </node>
</group> </group>
<!-- Run the ROS package stereo_image_proc for image rectification and disparity computation -->
<group ns="/stereo_camera">
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="disparity2depth" args="load rtabmap/disparity_to_depth standalone_nodelet"/>
</group>
<!-- Odometry: Choose between viso2_ros, fovis_ros and homemade approach (default) --> <!-- Odometry: Choose between viso2_ros, fovis_ros and homemade approach (default) -->
<node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen"> <node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen">
<remap from="stereo" to="/stereo_camera"/> <remap from="stereo" to="/stereo_camera"/>
@@ -52,7 +46,6 @@
<remap from="left/camera_info" to="/stereo_camera/left/camera_info"/> <remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info"/> <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>
<remap from="odom" to="/stereo_odometer/odometry"/> <remap from="odom" to="/stereo_odometer/odometry"/>
<remap from="odom_depth" to="/stereo_camera/depth2"/>
<param name="frame_id" type="string" value="base_link"/> <param name="frame_id" type="string" value="base_link"/>
<param name="approx_sync" type="bool" value="false"/> <param name="approx_sync" type="bool" value="false"/>
@@ -60,17 +53,6 @@
<param name="Odom/Strategy" type="string" value="1"/> <param name="Odom/Strategy" type="string" value="1"/>
<param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/> <param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Odom/MaxDepth" type="string" value="10"/> <param name="Odom/MaxDepth" type="string" value="10"/>
<param name="generate_depth" type="bool" value="false"/>
<!-- Parameters below only used when "generate_depth"=true -->
<param name="depth_patch_size" type="int" value="1"/>
<param name="subpix_win_size" type="int" value="3"/>
<param name="subpix_iterations" type="int" value="20"/>
<param name="subpix_epsilon" type="double" value="0.02"/>
<param name="flow_win_size" type="int" value="9"/>
<param name="flow_max_level" type="int" value="4"/>
<param name="flow_iterations" type="int" value="20"/>
<param name="flow_epsilon" type="double" value="0.02"/>
</node> </node>
--> -->
@@ -78,13 +60,14 @@
<!-- Visual SLAM (robot side) --> <!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" --> <!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start"> <node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/> <param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="false"/> <param name="subscribe_laserScan" type="bool" value="false"/>
<remap from="rgb/image" to="/stereo_camera/left/image_rect_color"/> <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
<remap from="rgb/camera_info" to="/stereo_camera/left/camera_info"/> <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
<remap from="depth/image" to="/stereo_camera/depth"/> <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>
<remap from="odom" to="/stereo_odometer/odometry"/> <remap from="odom" to="/stereo_odometer/odometry"/>
@@ -93,9 +76,10 @@
<param name="Rtabmap/TimeThr" type="string" value="700"/> <param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/> <param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Kp/DetectorStrategy" type="string" value="6"/> <param name="Kp/DetectorStrategy" type="string" value="0"/>
<param name="NN/NNStrategy" type="string" value="3"/> <param name="Kp/WordsPerImage" type="string" value="200"/>
<param name="GFTT/MaxCorners" type="string" value="200"/> <param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="NN/NNStrategy" type="string" value="1"/>
<param name="LccBow/MinInliers" type="string" value="10"/> <param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.02"/> <param name="LccBow/InlierDistance" type="string" value="0.02"/>
@@ -103,7 +87,7 @@
<param name="LccReextract/FeatureType" type="string" value="4"/> <param name="LccReextract/FeatureType" type="string" value="4"/>
<param name="LccReextract/Activated" type="string" value="true"/> <param name="LccReextract/Activated" type="string" value="true"/>
<param name="LccReextract/MaxDepth" type="string" value="3"/> <param name="LccReextract/MaxDepth" type="string" value="10"/>
<param name="LccReextract/NNDR" type="string" value="0.8"/> <param name="LccReextract/NNDR" type="string" value="0.8"/>
<param name="LccReextract/NNType" type="string" value="3"/> <param name="LccReextract/NNType" type="string" value="3"/>
</node> </node>
@@ -111,7 +95,7 @@
<!-- Visualisation (client side) --> <!-- Visualisation (client side) -->
<!-- <!--
<node pkg="rtabmap" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap)/launch/config/rgbd_gui.ini" output="screen"> <node pkg="rtabmap" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/> <param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_laserScan" type="bool" value="false"/> <param name="subscribe_laserScan" type="bool" value="false"/>
<param name="queue_size" type="int" value="30"/> <param name="queue_size" type="int" value="30"/>
launch/tests/test_stereo_mapping_bag.launch
-10
View File
@@ -46,16 +46,6 @@
<param name="Odom/Strategy" type="string" value="1"/> <param name="Odom/Strategy" type="string" value="1"/>
<param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/> <param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Odom/MaxDepth" type="string" value="10"/> <param name="Odom/MaxDepth" type="string" value="10"/>
<param name="generate_depth" type="bool" value="false"/>
<param name="depth_patch_size" type="int" value="1"/>
<param name="subpix_win_size" type="int" value="3"/>
<param name="subpix_iterations" type="int" value="20"/>
<param name="subpix_epsilon" type="double" value="0.02"/>
<param name="flow_win_size" type="int" value="9"/>
<param name="flow_max_level" type="int" value="4"/>
<param name="flow_iterations" type="int" value="20"/>
<param name="flow_epsilon" type="double" value="0.02"/>
</node> </node>
--> -->
launch/tests/test_stereo_odometry.launch
+1 -11
View File
@@ -38,17 +38,7 @@
<param name="Odom/Strategy" type="string" value="1"/> <param name="Odom/Strategy" type="string" value="1"/>
<param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/> <param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Odom/MaxDepth" type="string" value="10"/> <param name="Odom/MaxDepth" type="string" value="10"/>
<param name="OdomFlow/SubPixIterations" type="string" value="0"/>
<param name="generate_depth" type="bool" value="false"/>
<!-- Parameters below only used when "generate_depth"=true -->
<param name="depth_patch_size" type="int" value="1"/>
<param name="subpix_win_size" type="int" value="3"/>
<param name="subpix_iterations" type="int" value="20"/>
<param name="subpix_epsilon" type="double" value="0.02"/>
<param name="flow_win_size" type="int" value="9"/>
<param name="flow_max_level" type="int" value="4"/>
<param name="flow_iterations" type="int" value="20"/>
<param name="flow_epsilon" type="double" value="0.02"/>
</node> </node>
</group> </group>
src/CoreWrapper.cpp
+288 -63
View File
@@ -75,12 +75,27 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
bool subscribeLaserScan = false; bool subscribeLaserScan = false;
bool subscribeDepth = true; bool subscribeDepth = true;
bool subscribeStereo = false;
int queueSize = 10; int queueSize = 10;
double tfDelay = 0.05; // 20 Hz double tfDelay = 0.05; // 20 Hz
// ROS related parameters (private) // ROS related parameters (private)
pnh.param("subscribe_depth", subscribeDepth, subscribeDepth); pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan); pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
if(subscribeDepth && subscribeStereo)
{
UWARN("Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
subscribeDepth = false;
}
if(subscribeLaserScan)
{
if(!subscribeDepth && !subscribeStereo)
{
ROS_WARN("When subscribing to laser scan, you should subscribe to depth or stereo too. Subscribing to depth by default...");
subscribeDepth = true;
}
}
pnh.param("config_path", configPath_, configPath_); pnh.param("config_path", configPath_, configPath_);
@@ -187,10 +202,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
if(isRGBD) if(isRGBD)
{ {
// RGBD SLAM // RGBD SLAM
if(!subscribeDepth) if(!subscribeDepth && !subscribeStereo)
{ {
ROS_WARN("ROS param subscribe_depth is false, but RTAB-Map " ROS_WARN("ROS param subscribe_depth and subscribe_stereo are false, but RTAB-Map "
"parameter \"RGBD/Enabled\" is true! Please set subscribe_depth " "parameter \"RGBD/Enabled\" is true! Please set subscribe_depth or subscribe_stereo "
"to true to use rtabmap node for RGB-D SLAM, or set \"RGBD/Enabled\" to false for loop closure " "to true to use rtabmap node for RGB-D SLAM, or set \"RGBD/Enabled\" to false for loop closure "
"detection on images-only."); "detection on images-only.");
} }
@@ -198,10 +213,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
else else
{ {
// loop closure detection (images-only) // loop closure detection (images-only)
if(subscribeDepth || subscribeLaserScan) if(subscribeDepth || subscribeLaserScan || subscribeStereo)
{ {
ROS_WARN("ROS param subscribe_depth or subscribe_laserScan is true, but RTAB-Map " ROS_WARN("ROS param subscribe_depth, subscribe_laserScan or subscribe_stereo is true, but RTAB-Map "
"parameter \"RGBD/Enabled\" is false! Please set subscribe_depth and subscribe_laserScan " "parameter \"RGBD/Enabled\" is false! Please set subscribe_depth, subscribe_laserScan and subscribe_stereo "
"to false to use rtabmap node for loop closure detection on images-only, or set \"RGBD/Enabled\" to true " "to false to use rtabmap node for loop closure detection on images-only, or set \"RGBD/Enabled\" to true "
"for RGB-D SLAM."); "for RGB-D SLAM.");
} }
@@ -227,7 +242,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this); getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this); publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize); setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize);
transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay)); transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay));
} }
@@ -382,9 +397,9 @@ void CoreWrapper::depthCallback(
if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) && imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 || !(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0)) depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0))
{ {
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1"); ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1");
return; return;
@@ -460,9 +475,9 @@ void CoreWrapper::depthScanCallback(
if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) && imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 || !(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0)) depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0))
{ {
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1"); ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1");
return; return;
@@ -526,14 +541,186 @@ void CoreWrapper::depthScanCallback(
} }
} }
void CoreWrapper::stereoCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg)
{
if(!paused_)
{
if(rate_>0.0f)
{
if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_))
{
return;
}
}
time_ = ros::Time::now();
if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
return;
}
// TF ready?
Transform localTransform;
try
{
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "mono8");
}
else
{
ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
}
ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg);
float fx = model.left().fx();
float cx = model.left().cx();
float cy = model.left().cy();
float baseline = model.baseline();
process(leftImageMsg->header.seq,
ptrLeftImage->image,
odom,
odomMsg->header.frame_id,
ptrRightImage->image,
fx,
baseline,
cx,
cy,
localTransform,
cv::Mat());
}
}
void CoreWrapper::stereoScanCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg)
{
if(!paused_)
{
if(rate_>0.0f)
{
if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_))
{
return;
}
}
time_ = ros::Time::now();
if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
return;
}
// TF ready?
Transform localTransform;
try
{
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
//transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ> pclScan;
pcl::fromROSMsg(scanOut, pclScan);
cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "mono8");
}
else
{
ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
}
ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg);
float fx = model.left().fx();
float cx = model.left().cx();
float cy = model.left().cy();
float baseline = model.baseline();
process(leftImageMsg->header.seq,
ptrLeftImage->image,
odom,
odomMsg->header.frame_id,
ptrRightImage->image,
fx,
baseline,
cx,
cy,
localTransform,
scan);
}
}
void CoreWrapper::process( void CoreWrapper::process(
int id, int id,
const cv::Mat & image, const cv::Mat & image,
const Transform & odom, const Transform & odom,
const std::string & odomFrameId, const std::string & odomFrameId,
const cv::Mat & depth, const cv::Mat & depthOrRightImage,
float fx, float fx,
float fy, float fyOrBaseline,
float cx, float cx,
float cy, float cy,
const Transform & localTransform, const Transform & localTransform,
@@ -542,37 +729,47 @@ void CoreWrapper::process(
UTimer timer; UTimer timer;
if(rtabmap_.isIDsGenerated() || id > 0) if(rtabmap_.isIDsGenerated() || id > 0)
{ {
cv::Mat depth16; cv::Mat imageB;
if(!depth.empty() && depth.type() != CV_16UC1) if(!depthOrRightImage.empty())
{ {
if(depth.type() == CV_32FC1) if(depthOrRightImage.type() == CV_8UC1)
{ {
//convert to 16 bits //right image
depth16 = util3d::cvtDepthFromFloat(depth); imageB = depthOrRightImage.clone();
static bool shown = false; }
if(!shown) else if(depthOrRightImage.type() != CV_16UC1)
{
// depth float
if(depthOrRightImage.type() == CV_32FC1)
{ {
ROS_WARN("Use depth image with \"unsigned short\" type to " //convert to 16 bits
"avoid conversion. This message is only printed once..."); imageB = util3d::cvtDepthFromFloat(depthOrRightImage);
shown = true; static bool shown = false;
if(!shown)
{
ROS_WARN("Use depth image with \"unsigned short\" type to "
"avoid conversion. This message is only printed once...");
shown = true;
}
}
else
{
ROS_ERROR("Depth image must be of type \"unsigned short\"!");
return;
} }
} }
else else
{ {
ROS_ERROR("Depth image must be of type \"unsigned short\"!"); // depth short
return; imageB = depthOrRightImage.clone();
} }
} }
else
{
depth16 = depth.clone();
}
SensorData data(image.clone(), SensorData data(image.clone(),
depth16, imageB,
scan, scan,
fx, fx,
fy, fyOrBaseline,
cx, cx,
cy, cy,
odom, odom,
@@ -1248,52 +1445,80 @@ void CoreWrapper::publishStats(const Statistics & stats)
void CoreWrapper::setupCallbacks( void CoreWrapper::setupCallbacks(
bool subscribeDepth, bool subscribeDepth,
bool subscribeLaserScan, bool subscribeLaserScan,
bool subscribeStereo,
int queueSize) int queueSize)
{ {
if(subscribeLaserScan)
{
if(!subscribeDepth)
{
ROS_WARN("When subscribing to laser scan, you should subscribe to depth too. Subscribing to depth...");
subscribeDepth = true;
}
}
ros::NodeHandle nh; // public ros::NodeHandle nh; // public
ros::NodeHandle pnh("~"); // private ros::NodeHandle pnh("~"); // private
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
if(subscribeDepth && subscribeLaserScan) if(subscribeDepth)
{ {
ROS_INFO("Registering Depth+LaserScan callback..."); ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
scanSub_.subscribe(nh, "scan", 1);
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_); if(subscribeLaserScan)
depthScanSync_->registerCallback(boost::bind(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5)); {
ROS_INFO("Registering Depth+LaserScan callback...");
scanSub_.subscribe(nh, "scan", 1);
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
depthScanSync_->registerCallback(boost::bind(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
}
else //!subscribeLaserScan
{
ROS_INFO("Registering Depth callback...");
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
}
} }
else if(subscribeDepth && !subscribeLaserScan) else if(subscribeStereo)
{ {
ROS_INFO("Registering Depth callback..."); ros::NodeHandle left_nh(nh, "left");
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); ros::NodeHandle right_nh(nh, "right");
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); ros::NodeHandle left_pnh(pnh, "left");
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); ros::NodeHandle right_pnh(pnh, "right");
image_transport::ImageTransport left_it(left_nh);
image_transport::ImageTransport right_it(right_nh);
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4)); if(subscribeLaserScan)
{
ROS_INFO("Registering Stereo+LaserScan callback...");
scanSub_.subscribe(nh, "scan", 1);
stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(MyStereoScanSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, scanSub_, odomSub_);
stereoScanSync_->registerCallback(boost::bind(&CoreWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
}
else //!subscribeLaserScan
{
ROS_INFO("Registering Stereo callback...");
stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(MyStereoSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomSub_);
stereoSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
}
} }
else else
{ {
ROS_INFO("Registering default callback..."); ROS_INFO("Registering image-only callback...");
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle rgb_pnh(pnh, "rgb");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this); defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);
} }
} }
src/CoreWrapper.h
+41 -5
View File
@@ -68,7 +68,7 @@ public:
virtual ~CoreWrapper(); virtual ~CoreWrapper();
private: private:
void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, int queueSize); void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeStereo, int queueSize);
void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
void depthCallback(const sensor_msgs::ImageConstPtr& imageMsg, void depthCallback(const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg, const nav_msgs::OdometryConstPtr & odomMsg,
@@ -79,15 +79,26 @@ private:
const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg, const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg); const sensor_msgs::LaserScanConstPtr& scanMsg);
void stereoCallback(const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg);
void stereoScanCallback(const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg);
void process( void process(
int id, int id,
const cv::Mat & image, const cv::Mat & image,
const rtabmap::Transform & odom = rtabmap::Transform(), const rtabmap::Transform & odom = rtabmap::Transform(),
const std::string & odomFrameId = "", const std::string & odomFrameId = "",
const cv::Mat & depth = cv::Mat(), const cv::Mat & depthOrRightImage = cv::Mat(),
float fx = 0.0f, float fx = 0.0f,
float fy = 0.0f, float fyOrBaseline = 0.0f,
float cx = 0.0f, float cx = 0.0f,
float cy = 0.0f, float cy = 0.0f,
const rtabmap::Transform & localTransform = rtabmap::Transform(), const rtabmap::Transform & localTransform = rtabmap::Transform(),
@@ -126,12 +137,20 @@ private:
ros::Publisher infoPubEx_; ros::Publisher infoPubEx_;
ros::Publisher mapData_; ros::Publisher mapData_;
// for loop closure detection only
image_transport::Subscriber defaultSub_; image_transport::Subscriber defaultSub_;
//for depth callback
image_transport::SubscriberFilter imageSub_; image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_; image_transport::SubscriberFilter imageDepthSub_;
image_transport::SubscriberFilter imageSubRight_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_; message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSubRight_;
//stereo callback
image_transport::SubscriberFilter imageRectLeft_;
image_transport::SubscriberFilter imageRectRight_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
message_filters::Subscriber<nav_msgs::Odometry> odomSub_; message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_; message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
@@ -150,6 +169,23 @@ private:
sensor_msgs::CameraInfo> MyDepthSyncPolicy; sensor_msgs::CameraInfo> MyDepthSyncPolicy;
message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_; message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo,
sensor_msgs::LaserScan,
nav_msgs::Odometry> MyStereoScanSyncPolicy;
message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo,
nav_msgs::Odometry> MyStereoSyncPolicy;
message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
tf::TransformBroadcaster tfBroadcaster_; tf::TransformBroadcaster tfBroadcaster_;
tf::TransformListener tfListener_; tf::TransformListener tfListener_;
src/MapAssemblerNode.cpp
+9 -1
View File
@@ -130,7 +130,15 @@ public:
if(!image.empty() && !depth.empty() && depthFx > 0.0f && depthFy > 0.0f && depthCx >= 0.0f && depthCy >= 0.0f) if(!image.empty() && !depth.empty() && depthFx > 0.0f && depthFy > 0.0f && depthCx >= 0.0f && depthCy >= 0.0f)
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloudDecimation_); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(depth.type() == CV_8UC1)
{
cloud = util3d::cloudFromStereoImages(image, depth, depthCx, depthCy, depthFx, depthFy, cloudDecimation_);
}
else
{
cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloudDecimation_);
}
if(cloudMaxDepth_ > 0) if(cloudMaxDepth_ > 0)
{ {
src/OdometryROS.cpp
-1
View File
@@ -61,7 +61,6 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1); odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1); odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1); odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
//odomMatches_ = nh.advertise<sensor_msgs::Image>("odom_matches", 1);
ros::NodeHandle pnh("~"); ros::NodeHandle pnh("~");
src/StereoOdometryNode.cpp
+3 -193
View File
@@ -60,22 +60,11 @@ public:
StereoOdometry(int argc, char * argv[]) : StereoOdometry(int argc, char * argv[]) :
OdometryROS(argc, argv), OdometryROS(argc, argv),
feature2D_(0), feature2D_(0),
depthPatchSize_(1),
generateDepth_(false),
stereoFlowWinSize_(21),
stereoFlowIterations_(30),
stereoFlowEpsilon_(0.01),
stereoFlowMaxLevel_(3),
stereoSubPixWinSize_(5),
stereoSubPixIterations_(20),
stereoSubPixEps_(0.03),
approxSync_(0), approxSync_(0),
exactSync_(0) exactSync_(0)
{ {
ros::NodeHandle nh; ros::NodeHandle nh;
odomDepth_ = nh.advertise<sensor_msgs::Image>("odom_depth", 1);
ros::NodeHandle pnh("~"); ros::NodeHandle pnh("~");
bool approxSync = false; bool approxSync = false;
@@ -84,27 +73,9 @@ public:
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
pnh.param("generate_depth", generateDepth_, generateDepth_);
pnh.param("depth_patch_size", depthPatchSize_, depthPatchSize_);
ROS_INFO("Generate depth = %s", generateDepth_?"true":"false");
pnh.param("flow_win_size", stereoFlowWinSize_, stereoFlowWinSize_);
pnh.param("flow_iterations", stereoFlowIterations_, stereoFlowIterations_);
pnh.param("flow_epsilon", stereoFlowEpsilon_, stereoFlowEpsilon_);
pnh.param("flow_max_level", stereoFlowMaxLevel_, stereoFlowMaxLevel_);
pnh.param("subpix_win_size", stereoSubPixWinSize_, stereoSubPixWinSize_);
pnh.param("subpix_iterations", stereoSubPixIterations_, stereoSubPixIterations_);
pnh.param("subpix_eps", stereoSubPixEps_, stereoSubPixEps_);
UASSERT_MSG(!this->isOdometryBOW() || (this->isOdometryBOW() && generateDepth_),
"Odom/Strategy=0 (OdometryBOW) requires depth generation (generate_depth=true).");
UASSERT(depthPatchSize_ >= 0);
//Keypoint detector //Keypoint detector
ParametersMap::const_iterator iter; ParametersMap::const_iterator iter;
Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef; Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
if((iter=this->parameters().find(Parameters::kOdomFeatureType())) != this->parameters().end()) if((iter=this->parameters().find(Parameters::kOdomFeatureType())) != this->parameters().end())
{ {
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str()); detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
@@ -195,173 +166,28 @@ public:
model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight); model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight);
float fx = model.left().fx(); float fx = model.left().fx();
float fy = model.left().fy();
float cx = model.left().cx(); float cx = model.left().cx();
float cy = model.left().cy(); float cy = model.left().cy();
float baseline = model.baseline(); float baseline = model.baseline();
cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8"); cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8"); cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
cv::Mat depthOrRightImage;
std::vector<cv::KeyPoint> kptsLeft, kptsRight;
cv::Mat descLeft, descRight;
UTimer stepTimer; UTimer stepTimer;
if(!generateDepth_)
{
// copy right image in depth
depthOrRightImage = ptrImageRight->image;
}
else
{
//generate depth
depthOrRightImage = cv::Mat::zeros(ptrImageLeft->image.rows, ptrImageLeft->image.cols, CV_32FC1);
std::vector<cv::Point2f> cornersLeft, cornersRight;
cv::Rect roi = Feature2D::computeRoi(ptrImageLeft->image, roiRatios_);
kptsLeft = feature2D_->generateKeypoints(ptrImageLeft->image, 0, roi);
UDEBUG("time generate left kpts=%fs", stepTimer.ticks());
if(!kptsLeft.size())
{
ROS_WARN("No left keypoints extracted!");
return;
}
int stereoFeaturesAdded = 0;
int stereoFeaturesMatched = 0;
int stereoFeaturesExtracted = 0;
cv::KeyPoint::convert(kptsLeft, cornersLeft);
if(stereoSubPixWinSize_ > 0 && stereoSubPixIterations_ > 0)
{
cv::cornerSubPix( ptrImageLeft->image, cornersLeft,
cv::Size( stereoSubPixWinSize_, stereoSubPixWinSize_ ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, stereoSubPixIterations_, stereoSubPixEps_ ) );
UDEBUG("time subpix left kpts=%fs", stepTimer.ticks());
}
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
ptrImageLeft->image,
ptrImageRight->image,
cornersLeft,
cornersRight,
status,
err,
cv::Size(stereoFlowWinSize_, stereoFlowWinSize_), stereoFlowMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoFlowIterations_, stereoFlowEpsilon_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
UDEBUG("time optical flow=%fs", stepTimer.ticks());
std::vector<cv::KeyPoint> kptsLeftFiltered(kptsLeft.size());
int oi = 0;
for(int i=0; i<status.size(); ++i)
{
if(status[i] &&
uIsInBounds(cornersLeft[i].x, 0.0f, float(depthOrRightImage.cols)-1.0f) &&
uIsInBounds(cornersLeft[i].y, 0.0f, float(depthOrRightImage.rows)-1.0f) &&
uIsInBounds(cornersRight[i].x, 0.0f, float(depthOrRightImage.cols)-1.0f) &&
uIsInBounds(cornersRight[i].y, 0.0f, float(depthOrRightImage.rows)-1.0f))
{
float disparity = cornersLeft[i].x - cornersRight[i].x;
if(disparity >= 0)
{
float d = model.getZ(disparity);
if(d>0)
{
bool depthAdded = false;
int u = int(cornersLeft[i].x+0.5f);
int v = int(cornersLeft[i].y+0.5f);
for(int j=-depthPatchSize_; j<=depthPatchSize_; ++j)
{
for(int k=-depthPatchSize_; k<=depthPatchSize_; ++k)
{
if(uIsInBounds(u+j, 0, depthOrRightImage.cols-1) &&
uIsInBounds(v+k, 0, depthOrRightImage.rows-1))
{
depthOrRightImage.at<float>(v+j, u+k) = d;
depthAdded = true;
}
}
}
if(depthAdded)
{
kptsLeftFiltered[oi] = kptsLeft[i];
kptsLeftFiltered[oi].pt = cornersLeft[i];
++oi;
}
}
}
++stereoFeaturesMatched;
}
}
stereoFeaturesAdded = oi;
stereoFeaturesExtracted = kptsLeft.size();
UDEBUG("stereoFeaturesExtracted=%d", stereoFeaturesExtracted);
UDEBUG("stereoFeaturesMatched=%d", stereoFeaturesMatched);
UDEBUG("stereoFeaturesAdded=%d", stereoFeaturesAdded);
kptsLeftFiltered.resize(oi);
kptsLeft = kptsLeftFiltered;
if(!kptsLeft.size())
{
ROS_WARN("No left keypoints extracted!");
return;
}
// For OdometryBOW, we must generate descriptors
int odomStrategy = Parameters::defaultOdomStrategy();
Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy);
if(odomStrategy == 0)
{
descLeft = feature2D_->generateDescriptors(ptrImageLeft->image, kptsLeft);
UDEBUG("time generate left descriptors=%fs, remaining kpts=%d", stepTimer.ticks(), (int)kptsLeft.size());
if(!kptsLeft.size())
{
ROS_WARN("No left descriptors extracted!");
return;
}
}
}
// //
UDEBUG("localTransform = %s", rtabmap::transformFromTF(localTransform).prettyPrint().c_str()); UDEBUG("localTransform = %s", rtabmap::transformFromTF(localTransform).prettyPrint().c_str());
UDEBUG("kptsLeft=%d descLeft=%d", (int)kptsLeft.size(), descLeft.rows);
rtabmap::SensorData data(ptrImageLeft->image, rtabmap::SensorData data(ptrImageLeft->image,
depthOrRightImage, ptrImageRight->image,
fx, fx,
generateDepth_?fy:baseline, baseline,
cx, cx,
cy, cy,
rtabmap::Transform(), rtabmap::Transform(),
rtabmap::transformFromTF(localTransform)); rtabmap::transformFromTF(localTransform));
data.setFeatures(kptsLeft, descLeft);
quality=0; quality=0;
this->processData(data, imageRectLeft->header, quality); this->processData(data, imageRectLeft->header, quality);
UDEBUG("time odometry->process()=%fs", stepTimer.ticks()); UDEBUG("time odometry->process()=%fs", stepTimer.ticks());
if(generateDepth_ && odomDepth_.getNumSubscribers())
{
cv_bridge::CvImage img;
img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
img.image = depthOrRightImage;
sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
rosMsg->header= imageRectLeft->header;
odomDepth_.publish(rosMsg);
}
//ROS_INFO("Odom: quality=%d, update time=%fs, stereo matches: added/matched/extracted %d/%d/%d", //ROS_INFO("Odom: quality=%d, update time=%fs, stereo matches: added/matched/extracted %d/%d/%d",
// quality, (ros::WallTime::now()-time).toSec(), // quality, (ros::WallTime::now()-time).toSec(),
// stereoFeaturesAdded, stereoFeaturesMatched, stereoFeaturesExtracted); // stereoFeaturesAdded, stereoFeaturesMatched, stereoFeaturesExtracted);
@@ -379,22 +205,6 @@ private:
Feature2D * feature2D_; Feature2D * feature2D_;
std::string roiRatios_; std::string roiRatios_;
// ROS parameters
int depthPatchSize_;
bool generateDepth_;
int stereoFlowWinSize_;
int stereoFlowIterations_;
double stereoFlowEpsilon_;
int stereoFlowMaxLevel_;
int stereoSubPixWinSize_;
int stereoSubPixIterations_;
double stereoSubPixEps_;
ros::Publisher odomDepth_;
image_transport::SubscriberFilter imageRectLeft_; image_transport::SubscriberFilter imageRectLeft_;
image_transport::SubscriberFilter imageRectRight_; image_transport::SubscriberFilter imageRectRight_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_; message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
src/rviz/MapCloudDisplay.cpp
+9 -2
View File
@@ -308,8 +308,15 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
if(!image.empty() && !depth.empty() && depthFx > 0.0f && depthFy > 0.0f && depthCx >= 0.0f && depthCy >= 0.0f) if(!image.empty() && !depth.empty() && depthFx > 0.0f && depthFy > 0.0f && depthCx >= 0.0f && depthCy >= 0.0f)
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloud_decimation_->getInt()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(depth.type() == CV_8UC1)
{
cloud = util3d::cloudFromStereoImages(image, depth, depthCx, depthCy, depthFx, depthFy, cloud_decimation_->getInt());
}
else
{
cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloud_decimation_->getInt());
}
if(cloud_max_depth_->getFloat() > 0.0f) if(cloud_max_depth_->getFloat() > 0.0f)
{ {
cloud = util3d::passThrough(cloud, "z", 0, cloud_max_depth_->getFloat()); cloud = util3d::passThrough(cloud, "z", 0, cloud_max_depth_->getFloat());