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

Updated RGBDImage msg with depth camera info, also made changes so that RGBDImage can be used also to sync stereo images.

Browse Source
This commit is contained in:
matlabbe
2018-02-13 21:35:15 -05:00
parent 6b14693a95
commit 438fad643c
24 changed files with 384 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+25 -11
View File
@@ -754,7 +754,7 @@ void CoreWrapper::rgbCallback(
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CoreWrapper::rgbOdomCallback(
@@ -767,7 +767,7 @@ void CoreWrapper::rgbOdomCallback(
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
@@ -1145,10 +1145,11 @@ void CoreWrapper::commonDepthCallbackImpl(
void CoreWrapper::commonStereoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const cv_bridge::CvImageConstPtr& leftImageMsg,
const cv_bridge::CvImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfo& leftCamInfoMsg,
const sensor_msgs::CameraInfo& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scan2dMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
@@ -1244,9 +1245,9 @@ void CoreWrapper::commonStereoCallback(
std::vector<cv_bridge::CvImageConstPtr> rgbImages(1);
std::vector<cv_bridge::CvImageConstPtr> depthImages(1);
std::vector<sensor_msgs::CameraInfo> cameraInfos(1);
rgbImages[0] = cv_bridge::toCvShare(leftImageMsg);
rgbImages[0] = leftImageMsg;
depthImages[0] = imgDepth;
cameraInfos[0] = *leftCamInfoMsg;
cameraInfos[0] = leftCamInfoMsg;
commonDepthCallbackImpl(odomFrameId, rtabmap_ros::UserDataConstPtr(), rgbImages, depthImages, cameraInfos, scan2dMsg, scan3dMsg, odomInfoMsg);
return;
@@ -1300,15 +1301,28 @@ void CoreWrapper::commonStereoCallback(
}
}
cv::Mat userData = userData_;
userData_ = cv::Mat();
Transform groundTruthPose;
if(!groundTruthFrameId_.empty())
{
groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
}
cv::Mat userData;
if(userDataMsg.get())
{
userData = rtabmap_ros::userDataFromROS(*userDataMsg);
if(!userData_.empty())
{
NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
userData_ = cv::Mat();
}
}
else
{
userData = userData_;
userData_ = cv::Mat();
}
SensorData data(scan,
LaserScanInfo(
scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0,