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

Leaving the depth format check to rtabmap library. Added warning on large baseline detected. Added "convert_depth_to_mm"=true argument to rgbd_mapping.launch

Browse Source
This commit is contained in:
matlabbe
2015-10-09 12:28:20 -04:00
parent 96fc6c81d6
commit 3269ccd196
7 changed files with 44 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/rgbd_mapping.launch
+2
View File
@@ -24,6 +24,7 @@
<arg name="depth_registered_topic" default="/camera/depth_registered/image_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<arg name="compressed" default="false"/>
<arg name="convert_depth_to_mm" default="true"/>
<arg name="subscribe_scan" default="false"/> <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
<arg name="scan_topic" default="/scan"/>
@@ -92,6 +93,7 @@
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="LccBow/EstimationType" type="string" value="$(arg estimation)"/>
<param name="LccBow/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>
<param name="Mem/SaveDepth16Format" type="string" value="$(arg convert_depth_to_mm)"/>
<!-- when 2D scan is set -->
<param if="$(arg subscribe_scan)" name="RGBD/OptimizeSlam2D" type="string" value="true"/>
src/CoreWrapper.cpp
+13 -11
View File
@@ -817,17 +817,6 @@ void CoreWrapper::commonDepthCallback(
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
cv::Mat subDepth = ptrDepth->image;
if(subDepth.type() == CV_32FC1)
{
subDepth = util2d::cvtDepthFromFloat(subDepth);
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;
}
}
// initialize
if(rgb.empty())
@@ -1046,6 +1035,19 @@ void CoreWrapper::commonStereoCallback(
model.baseline(),
localTransform);
if(model.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
{
ROS_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
model.baseline());
shown = true;
}
}
ros::Time stamp = scanMsg.get() != 0?scanMsg->header.stamp:leftImageMsg->header.stamp;
process(stamp,
SensorData(scan,
src/GuiWrapper.cpp
+13 -11
View File
@@ -612,17 +612,6 @@ void GuiWrapper::commonDepthCallback(
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
cv::Mat subDepth = ptrDepth->image;
if(subDepth.type() == CV_32FC1)
{
subDepth = util2d::cvtDepthFromFloat(subDepth);
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;
}
}
// initialize
if(rgb.empty())
@@ -823,6 +812,19 @@ void GuiWrapper::commonStereoCallback(
model.baseline(),
localTransform);
if(model.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
{
ROS_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
model.baseline());
shown = true;
}
}
// left
cv_bridge::CvImageConstPtr ptrImage;
cv::Mat left;
src/MsgConversion.cpp
+1
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <ros/ros.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/ULogger.h>
#include <pcl_conversions/pcl_conversions.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf_conversions/tf_eigen.h>
src/OdometryROS.cpp
+2 -1
View File
@@ -308,7 +308,8 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::
//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
{
ROS_WARN("odometry: Could not get transform from %s to %s after %f seconds!", fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_);
ROS_WARN("odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)!",
fromFrameId.c_str(), toFrameId.c_str(), stamp.toSec(), waitForTransformDuration_, waitForTransformDuration_);
return transform;
}
}
src/RGBDOdometryNode.cpp
-21
View File
@@ -178,16 +178,6 @@ public:
image->encoding.c_str(), depth->encoding.c_str());
return;
}
else if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0)
{
static bool warned = false;
if(!warned)
{
ROS_WARN("Input depth type is 32FC1, please use type 16UC1 or mono16 for depth. The depth images "
"will be processed anyway but with a conversion. This warning is only be printed once...");
warned = true;
}
}
ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
@@ -300,17 +290,6 @@ public:
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
cv::Mat subDepth = ptrDepth->image;
if(subDepth.type() == CV_32FC1)
{
subDepth = rtabmap::util2d::cvtDepthFromFloat(subDepth);
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;
}
}
// initialize
if(rgb.empty())
src/StereoOdometryNode.cpp
+13
View File
@@ -162,6 +162,19 @@ public:
model.baseline(),
localTransform);
if(model.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
{
ROS_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
model.baseline());
shown = true;
}
}
cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");