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

rtabmap/rtabmapviz: Refactored to support multi-stereo input

Browse Source
This commit is contained in:
matlabbe
2022-07-13 13:49:15 -04:00
parent 15d52fc0e0
commit bd727daab2
18 changed files with 579 additions and 585 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CommonDataSubscriber.cpp
+25 -45
View File
@@ -1011,7 +1011,7 @@ void CommonDataSubscriber::warningLoop()
}
}
void CommonDataSubscriber::commonSingleDepthCallback(
void CommonDataSubscriber::commonSingleCameraCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const cv_bridge::CvImageConstPtr & imageMsg,
@@ -1035,54 +1035,34 @@ void CommonDataSubscriber::commonSingleDepthCallback(
std::vector<cv::Mat> localDescriptorsMsgs;
localDescriptorsMsgs.push_back(localDescriptors);
if(depthMsg.get() == 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::MONO16) == 0)
std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs;
if(imageMsg.get())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
if(imageMsg.get())
{
imageMsgs.push_back(imageMsg);
}
if(depthMsg.get())
{
depthMsgs.push_back(depthMsg);
}
cameraInfoMsgs.push_back(rgbCameraInfoMsg);
commonDepthCallback(
odomMsg,
userDataMsg,
imageMsgs,
depthMsgs,
cameraInfoMsgs,
scanMsg,
scan3dMsg,
odomInfoMsg,
globalDescriptorMsgs,
localKeyPointsMsgs,
localPoints3dMsgs,
localDescriptorsMsgs);
imageMsgs.push_back(imageMsg);
}
else // assuming stereo
if(depthMsg.get())
{
commonStereoCallback(
odomMsg,
userDataMsg,
imageMsg,
depthMsg,
rgbCameraInfoMsg,
depthCameraInfoMsg,
scanMsg,
scan3dMsg,
odomInfoMsg,
globalDescriptorMsgs,
localKeyPoints,
localPoints3d,
localDescriptors);
depthMsgs.push_back(depthMsg);
}
cameraInfoMsgs.push_back(rgbCameraInfoMsg);
depthCameraInfoMsgs.push_back(depthCameraInfoMsg);
commonMultiCameraCallback(
odomMsg,
userDataMsg,
imageMsgs,
depthMsgs,
cameraInfoMsgs,
depthCameraInfoMsgs,
scanMsg,
scan3dMsg,
odomInfoMsg,
globalDescriptorMsgs,
localKeyPointsMsgs,
localPoints3dMsgs,
localDescriptorsMsgs);
}
} /* namespace rtabmap_ros */
src/CoreWrapper.cpp
+100 -262
View File
@@ -1146,12 +1146,13 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp)
return false;
}
void CoreWrapper::commonDepthCallback(
void CoreWrapper::commonMultiCameraCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::CameraInfo> & depthCameraInfoMsgs,
const sensor_msgs::LaserScan& scan2dMsg,
const sensor_msgs::PointCloud2& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
@@ -1202,11 +1203,12 @@ void CoreWrapper::commonDepthCallback(
return;
}
commonDepthCallbackImpl(odomFrameId,
commonMultiCameraCallbackImpl(odomFrameId,
userDataMsg,
imageMsgs,
depthMsgs,
cameraInfoMsgs,
depthCameraInfoMsgs,
scan2dMsg,
scan3dMsg,
odomInfoMsg,
@@ -1216,12 +1218,13 @@ void CoreWrapper::commonDepthCallback(
localDescriptors);
}
void CoreWrapper::commonDepthCallbackImpl(
void CoreWrapper::commonMultiCameraCallbackImpl(
const std::string & odomFrameId,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::CameraInfo> & depthCameraInfoMsgs,
const sensor_msgs::LaserScan& scan2dMsg,
const sensor_msgs::PointCloud2& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
@@ -1234,21 +1237,26 @@ void CoreWrapper::commonDepthCallbackImpl(
cv::Mat rgb;
cv::Mat depth;
std::vector<rtabmap::CameraModel> cameraModels;
std::vector<rtabmap::StereoCameraModel> stereoCameraModels;
std::vector<cv::KeyPoint> keypoints;
std::vector<cv::Point3f> points;
cv::Mat descriptors;
if(!rtabmap_ros::convertRGBDMsgs(
imageMsgs,
depthMsgs,
cameraInfoMsgs,
depthCameraInfoMsgs,
frameId_,
odomSensorSync_?odomFrameId:"",
lastPoseStamp_,
rgb,
depth,
cameraModels,
stereoCameraModels,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0,
alreadyRectifiedImages_,
localKeyPointsMsgs,
localPoints3dMsgs,
localDescriptorsMsgs,
@@ -1260,6 +1268,71 @@ void CoreWrapper::commonDepthCallbackImpl(
return;
}
if(stereoCameraModels.size() && stereoToDepth_)
{
UASSERT(depth.type() == CV_8UC1);
cv::Mat leftMono;
if(rgb.channels() == 3)
{
cv::cvtColor(rgb, leftMono, CV_BGR2GRAY);
}
else
{
leftMono = rgb;
}
cv::Mat rightMono = depth;
depth = cv::Mat();
UASSERT(int((leftMono.cols/stereoCameraModels.size())*stereoCameraModels.size()) == leftMono.cols);
UASSERT(int((rightMono.cols/stereoCameraModels.size())*stereoCameraModels.size()) == rightMono.cols);
int subImageWidth = leftMono.cols/stereoCameraModels.size();
for(size_t i=0; i<stereoCameraModels.size(); ++i)
{
cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows));
cv::Mat right(rightMono, cv::Rect(subImageWidth*i, 0, subImageWidth, rightMono.rows));
// cv::stereoBM() see "$ rosrun rtabmap_ros rtabmap --params | grep StereoBM" for parameters
cv::Mat disparity = rtabmap::util2d::disparityFromStereoImages(
left,
right,
parameters_);
if(disparity.empty())
{
NODELET_ERROR("Could not compute disparity image (\"stereo_to_depth\" is true)!");
return;
}
cv::Mat subDepth = rtabmap::util2d::depthFromDisparity(
disparity,
stereoCameraModels[i].left().fx(),
stereoCameraModels[i].baseline());
if(subDepth.empty())
{
NODELET_ERROR("Could not compute depth image (\"stereo_to_depth\" is true)!");
return;
}
UASSERT(subDepth.type() == CV_16UC1 || subDepth.type() == CV_32FC1);
if(depth.empty())
{
depth = cv::Mat(subDepth.rows, subDepth.cols*stereoCameraModels.size(), subDepth.type());
}
if(subDepth.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*subDepth.cols, 0, subDepth.cols, subDepth.rows)));
}
else
{
ROS_ERROR("Some Depth images are not the same type!");
return;
}
cameraModels.push_back(stereoCameraModels[i].left());
}
stereoCameraModels.clear();
}
UASSERT(uContains(parameters_, rtabmap::Parameters::kMemSaveDepth16Format()));
if(!depth.empty() && depth.type() == CV_32FC1 && uStr2Bool(parameters_.at(Parameters::kMemSaveDepth16Format())))
{
@@ -1277,7 +1350,7 @@ void CoreWrapper::commonDepthCallbackImpl(
LaserScan scan;
bool genMaxScanPts = 0;
if(scan2dMsg.ranges.empty() && scan3dMsg.data.empty() && !depth.empty() && genScan_)
if(scan2dMsg.ranges.empty() && scan3dMsg.data.empty() && !depth.empty() && stereoCameraModels.empty() && genScan_)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud2d(new pcl::PointCloud<pcl::PointXYZ>);
*scanCloud2d = util3d::laserScanFromDepthImages(
@@ -1388,14 +1461,29 @@ void CoreWrapper::commonDepthCallbackImpl(
userData_ = cv::Mat();
}
SensorData data(
scan,
rgb,
depth,
cameraModels,
lastPoseIntermediate_?-1:!cameraInfoMsgs.empty()?cameraInfoMsgs[0].header.seq:0,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
SensorData data;
if(!stereoCameraModels.empty())
{
data = SensorData(
scan,
rgb,
depth,
stereoCameraModels,
lastPoseIntermediate_?-1:!cameraInfoMsgs.empty()?cameraInfoMsgs[0].header.seq:0,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
}
else
{
data = SensorData(
scan,
rgb,
depth,
cameraModels,
lastPoseIntermediate_?-1:!cameraInfoMsgs.empty()?cameraInfoMsgs[0].header.seq:0,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
}
OdometryInfo odomInfo;
if(odomInfoMsg.get())
@@ -1426,256 +1514,6 @@ void CoreWrapper::commonDepthCallbackImpl(
covariance_ = cv::Mat();
}
void CoreWrapper::commonStereoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
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::LaserScan& scan2dMsg,
const sensor_msgs::PointCloud2& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
const std::vector<rtabmap_ros::KeyPoint> & localKeyPointsMsg,
const std::vector<rtabmap_ros::Point3f> & localPoints3dMsg,
const cv::Mat & localDescriptorsMsg)
{
UTimer timerConversion;
std::string odomFrameId = odomFrameId_;
if(odomMsg.get())
{
odomFrameId = odomMsg->header.frame_id;
if(!scan2dMsg.ranges.empty())
{
if(!odomUpdate(odomMsg, scan2dMsg.header.stamp))
{
return;
}
}
else if(!scan3dMsg.data.empty())
{
if(!odomUpdate(odomMsg, scan3dMsg.header.stamp))
{
return;
}
}
else if(leftImageMsg.get() == 0 || !odomUpdate(odomMsg, leftImageMsg->header.stamp))
{
return;
}
}
else if(!scan2dMsg.ranges.empty())
{
if(!odomTFUpdate(scan2dMsg.header.stamp))
{
return;
}
}
else if(!scan3dMsg.data.empty())
{
if(!odomTFUpdate(scan3dMsg.header.stamp))
{
return;
}
}
else if(leftImageMsg.get() == 0 || !odomTFUpdate(leftImageMsg->header.stamp))
{
return;
}
cv::Mat left;
cv::Mat right;
StereoCameraModel stereoModel;
if(!rtabmap_ros::convertStereoMsg(
leftImageMsg,
rightImageMsg,
leftCamInfoMsg,
rightCamInfoMsg,
frameId_,
odomSensorSync_?odomFrameId:"",
lastPoseStamp_,
left,
right,
stereoModel,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0,
alreadyRectifiedImages_))
{
NODELET_ERROR("Could not convert stereo msgs! Aborting rtabmap update...");
return;
}
if(stereoToDepth_)
{
// cv::stereoBM() see "$ rosrun rtabmap_ros rtabmap --params | grep StereoBM" for parameters
cv::Mat disparity = rtabmap::util2d::disparityFromStereoImages(
left,
right,
parameters_);
if(disparity.empty())
{
NODELET_ERROR("Could not compute disparity image (\"stereo_to_depth\" is true)!");
return;
}
cv::Mat depth = rtabmap::util2d::depthFromDisparity(
disparity,
stereoModel.left().fx(),
stereoModel.baseline());
if(depth.empty())
{
NODELET_ERROR("Could not compute depth image (\"stereo_to_depth\" is true)!");
return;
}
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
// move to common depth callback
cv_bridge::CvImagePtr imgDepth(new cv_bridge::CvImage);
if(depth.type() == CV_16UC1)
{
imgDepth->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
}
else // CV_32FC1
{
imgDepth->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
}
imgDepth->image = depth;
imgDepth->header = leftImageMsg->header;
std::vector<cv_bridge::CvImageConstPtr> rgbImages(1);
std::vector<cv_bridge::CvImageConstPtr> depthImages(1);
std::vector<sensor_msgs::CameraInfo> cameraInfos(1);
rgbImages[0] = leftImageMsg;
depthImages[0] = imgDepth;
cameraInfos[0] = leftCamInfoMsg;
std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPointsMsgs;
std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3dMsgs;
std::vector<cv::Mat> localDescriptorsMsgs;
if(!localKeyPointsMsg.empty())
{
localKeyPointsMsgs.push_back(localKeyPointsMsg);
}
if(!localPoints3dMsg.empty())
{
localPoints3dMsgs.push_back(localPoints3dMsg);
}
if(!localDescriptorsMsg.empty())
{
localDescriptorsMsgs.push_back(localDescriptorsMsg);
}
commonDepthCallbackImpl(odomFrameId,
rtabmap_ros::UserDataConstPtr(),
rgbImages, depthImages, cameraInfos,
scan2dMsg, scan3dMsg,
odomInfoMsg,
globalDescriptorMsgs, localKeyPointsMsgs, localPoints3dMsgs, localDescriptorsMsgs);
return;
}
LaserScan scan;
if(!scan2dMsg.ranges.empty())
{
if(!rtabmap_ros::convertScanMsg(
scan2dMsg,
frameId_,
odomSensorSync_?odomFrameId:"",
lastPoseStamp_,
scan,
tfListener_,
waitForTransform_?waitForTransformDuration_:0,
// backward compatibility, project 2D scan in /base_link frame
rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0))
{
NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update...");
return;
}
}
else if(!scan3dMsg.data.empty())
{
if(!rtabmap_ros::convertScan3dMsg(
scan3dMsg,
frameId_,
odomSensorSync_?odomFrameId:"",
lastPoseStamp_,
scan,
tfListener_,
waitForTransform_?waitForTransformDuration_:0,
scanCloudMaxPoints_))
{
NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update...");
return;
}
}
cv::Mat userData;
if(userDataMsg.get())
{
userData = rtabmap_ros::userDataFromROS(*userDataMsg);
UScopeMutex lock(userDataMutex_);
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
{
UScopeMutex lock(userDataMutex_);
userData = userData_;
userData_ = cv::Mat();
}
SensorData data(
scan,
left,
right,
stereoModel,
lastPoseIntermediate_?-1:leftImageMsg->header.seq,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
OdometryInfo odomInfo;
if(odomInfoMsg.get())
{
odomInfo = odomInfoFromROS(*odomInfoMsg);
}
if(!globalDescriptorMsgs.empty())
{
data.setGlobalDescriptors(rtabmap_ros::globalDescriptorsFromROS(globalDescriptorMsgs));
}
std::vector<cv::KeyPoint> keypoints;
std::vector<cv::Point3f> points;
if(!localKeyPointsMsg.empty())
{
keypoints = rtabmap_ros::keypointsFromROS(localKeyPointsMsg);
}
if(!localPoints3dMsg.empty())
{
// Points should be in base frame
points = rtabmap_ros::points3fFromROS(localPoints3dMsg, stereoModel.localTransform());
}
if(!keypoints.empty())
{
UASSERT(points.empty() || points.size() == keypoints.size());
UASSERT(localDescriptorsMsg.empty() || localDescriptorsMsg.rows == (int)keypoints.size());
data.setFeatures(keypoints, points, localDescriptorsMsg);
}
process(lastPoseStamp_,
data,
lastPose_,
lastPoseVelocity_,
odomFrameId,
covariance_,
odomInfo,
timerConversion.ticks());
covariance_ = cv::Mat();
}
void CoreWrapper::commonLaserScanCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
src/GuiWrapper.cpp
+26 -9
View File
@@ -433,12 +433,13 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
return false;
}
void GuiWrapper::commonDepthCallback(
void GuiWrapper::commonMultiCameraCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::CameraInfo> & depthCameraInfoMsgs,
const sensor_msgs::LaserScan& scan2dMsg,
const sensor_msgs::PointCloud2& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
@@ -524,6 +525,7 @@ void GuiWrapper::commonDepthCallback(
cv::Mat rgb;
cv::Mat depth;
std::vector<rtabmap::CameraModel> cameraModels;
std::vector<rtabmap::StereoCameraModel> stereoCameraModels;
LaserScan scan;
rtabmap::OdometryInfo info;
bool ignoreData = false;
@@ -538,18 +540,25 @@ void GuiWrapper::commonDepthCallback(
if(imageMsgs.size() && imageMsgs[0].get() && depthMsgs.size() && depthMsgs[0].get())
{
ParametersMap allParameters = prefDialog_->getAllParameters();
bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified();
Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified);
if(!rtabmap_ros::convertRGBDMsgs(
imageMsgs,
depthMsgs,
cameraInfoMsgs,
depthCameraInfoMsgs,
frameId,
odomSensorSync_?odomHeader.frame_id:"",
odomHeader.stamp,
rgb,
depth,
cameraModels,
stereoCameraModels,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0))
waitForTransform_?waitForTransformDuration_:0.0,
imagesAlreadyRectified))
{
ROS_ERROR("Could not convert rgb/depth msgs! Aborting rtabmapviz update...");
return;
@@ -606,13 +615,21 @@ void GuiWrapper::commonDepthCallback(
info.reg.covariance = covariance;
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
rgb,
depth,
cameraModels,
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
!stereoCameraModels.empty()?
rtabmap::SensorData(
scan,
rgb,
depth,
stereoCameraModels,
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)):
rtabmap::SensorData(
scan,
rgb,
depth,
cameraModels,
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
info);
src/MsgConversion.cpp
+181 -28
View File
@@ -1803,14 +1803,17 @@ bool convertRGBDMsgs(
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::CameraInfo> & depthCameraInfoMsgs,
const std::string & frameId,
const std::string & odomFrameId,
const ros::Time & odomStamp,
cv::Mat & rgb,
cv::Mat & depth,
std::vector<rtabmap::CameraModel> & cameraModels,
std::vector<rtabmap::StereoCameraModel> & stereoCameraModels,
tf::TransformListener & listener,
double waitForTransform,
bool alreadRectifiedImages,
const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPointsMsgs,
const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3dMsgs,
const std::vector<cv::Mat> & localDescriptorsMsgs,
@@ -1818,16 +1821,41 @@ bool convertRGBDMsgs(
std::vector<cv::Point3f> * localPoints3d,
cv::Mat * localDescriptors)
{
UASSERT(!cameraInfoMsgs.empty()>0 &&
UASSERT(!cameraInfoMsgs.empty() &&
(cameraInfoMsgs.size() == imageMsgs.size() || imageMsgs.empty()) &&
(cameraInfoMsgs.size() == depthMsgs.size() || depthMsgs.empty()));
(cameraInfoMsgs.size() == depthMsgs.size() || depthMsgs.empty()) &&
(cameraInfoMsgs.size() == depthCameraInfoMsgs.size() || depthCameraInfoMsgs.empty()));
int imageWidth = imageMsgs.size()?imageMsgs[0]->image.cols:cameraInfoMsgs[0].width;
int imageHeight = imageMsgs.size()?imageMsgs[0]->image.rows:cameraInfoMsgs[0].height;
int depthWidth = depthMsgs.size()?depthMsgs[0]->image.cols:0;
int depthHeight = depthMsgs.size()?depthMsgs[0]->image.rows:0;
if(!depthMsgs.empty())
bool isDepth = depthMsgs.empty() || (depthMsgs[0].get() != 0 && (
depthMsgs[0]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[0]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[0]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0));
// Note that right image can be also MONO16, check the camera info if Tx is set, if so assume it is stereo instead
if(isDepth &&
!depthMsgs.empty() &&
depthMsgs[0]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 &&
cameraInfoMsgs.size() == depthCameraInfoMsgs.size())
{
isDepth = cameraInfoMsgs[0].P.elems[3] == 0.0 && depthCameraInfoMsgs[0].P.elems[3] == 0.0;
static bool warned = false;
if(!warned && isDepth)
{
ROS_WARN("Input depth/left image has encoding \"mono16\" and "
"camera info P[3] is null for both cameras, thus image is "
"considered a depth image. If the depth image is in "
"fact the right image, please convert the right image to "
"\"mono8\". This warning is shown only once.");
warned = true;
}
}
if(isDepth)
{
UASSERT_MSG(
imageWidth/depthWidth == imageHeight/depthHeight,
@@ -1849,8 +1877,7 @@ bool convertRGBDMsgs(
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_RGGB8) == 0))
{
ROS_ERROR("Input rgb type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8. Current rgb=%s",
ROS_ERROR("Input rgb/left type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8. Current rgb/left=%s",
imageMsgs[i]->encoding.c_str());
return false;
}
@@ -1862,19 +1889,35 @@ bool convertRGBDMsgs(
imageHeight,
imageMsgs[i]->image.rows).c_str());
}
if(!depthMsgs.empty() &&
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
if(!depthMsgs.empty())
{
ROS_ERROR("Input depth type must be image_depth=32FC1,16UC1,mono16. Current depth=%s",
depthMsgs[i]->encoding.c_str());
return false;
if(isDepth &&
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{
ROS_ERROR("Input depth type must be image_depth=32FC1,16UC1,mono16. Current depth=%s",
depthMsgs[i]->encoding.c_str());
return false;
}
else if(!isDepth &&
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
{
ROS_ERROR("Input right type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8. Current right=%s",
depthMsgs[i]->encoding.c_str());
return false;
}
}
ros::Time stamp;
if(!depthMsgs.empty())
if(isDepth && !depthMsgs.empty())
{
UASSERT_MSG(depthMsgs[i]->image.cols == depthWidth && depthMsgs[i]->image.rows == depthHeight,
uFormat("depthWidth=%d vs %d imageHeight=%d vs %d",
@@ -1912,8 +1955,8 @@ bool convertRGBDMsgs(
waitForTransform);
if(sensorT.isNull())
{
ROS_WARN("Could not get odometry value for depth image stamp (%fs). Latest odometry "
"stamp is %fs. The depth image pose will not be synchronized with odometry.", stamp.toSec(), odomStamp.toSec());
ROS_WARN("Could not get odometry value for image stamp (%fs). Latest odometry "
"stamp is %fs. The image pose will not be synchronized with odometry.", stamp.toSec(), odomStamp.toSec());
}
else
{
@@ -1951,33 +1994,143 @@ bool convertRGBDMsgs(
}
else
{
ROS_ERROR("Some RGB images are not the same type!");
ROS_ERROR("Some RGB/left images are not the same type!");
return false;
}
}
if(!depthMsgs.empty())
{
cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i];
cv::Mat subDepth = ptrDepth->image;
if(depth.empty())
if(isDepth)
{
depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type());
}
cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i];
cv::Mat subDepth = ptrDepth->image;
if(subDepth.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
if(depth.empty())
{
depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type());
}
if(subDepth.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
}
else
{
ROS_ERROR("Some Depth images are not the same type!");
return false;
}
}
else
{
ROS_ERROR("Some Depth images are not the same type!");
return false;
cv_bridge::CvImageConstPtr ptrImage = depthMsgs[i];
if( depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
{
// do nothing
}
else if(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrImage = cv_bridge::cvtColor(depthMsgs[i], "mono8");
}
else
{
ptrImage = cv_bridge::cvtColor(depthMsgs[i], "bgr8");
}
// initialize
if(depth.empty())
{
depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrImage->image.type());
}
if(ptrImage->image.type() == depth.type())
{
ptrImage->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
}
else
{
ROS_ERROR("Some right images are not the same type!");
return false;
}
}
}
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform));
if(isDepth)
{
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform));
}
else //stereo
{
UASSERT(cameraInfoMsgs.size() == depthCameraInfoMsgs.size());
rtabmap::Transform stereoTransform;
if(!alreadRectifiedImages)
{
stereoTransform = getTransform(
depthCameraInfoMsgs[i].header.frame_id,
cameraInfoMsgs[i].header.frame_id,
cameraInfoMsgs[i].header.stamp,
listener,
waitForTransform);
if(stereoTransform.isNull())
{
ROS_ERROR("Parameter %s is false but we cannot get TF between the two cameras!", rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str());
return false;
}
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(cameraInfoMsgs[i], depthCameraInfoMsgs[i], localTransform, stereoTransform);
if(stereoModel.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). You may need to calibrate your camera. "
"This warning is printed only once.",
stereoModel.baseline());
shown = true;
}
}
else if(stereoModel.baseline() == 0 && alreadRectifiedImages)
{
rtabmap::Transform stereoTransform = getTransform(
cameraInfoMsgs[i].header.frame_id,
depthCameraInfoMsgs[i].header.frame_id,
cameraInfoMsgs[i].header.stamp,
listener,
waitForTransform);
if(stereoTransform.isNull() || stereoTransform.x()<=0)
{
ROS_WARN("We cannot estimated the baseline of the rectified images with tf! (%s->%s = %s)",
depthCameraInfoMsgs[i].header.frame_id.c_str(), cameraInfoMsgs[i].header.frame_id.c_str(), stereoTransform.prettyPrint().c_str());
}
else
{
static bool warned = false;
if(!warned)
{
ROS_WARN("Right camera info doesn't have Tx set but we are assuming that stereo images are already rectified (see %s parameter). While not "
"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
"a valid right camera info if stereo images are already rectified. This message is only printed once...",
rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
depthCameraInfoMsgs[i].header.frame_id.c_str(), cameraInfoMsgs[i].header.frame_id.c_str(), stereoTransform.x());
warned = true;
}
stereoModel = rtabmap::StereoCameraModel(
stereoModel.left().fx(),
stereoModel.left().fy(),
stereoModel.left().cx(),
stereoModel.left().cy(),
stereoTransform.x(),
stereoModel.localTransform(),
stereoModel.left().imageSize());
}
}
stereoCameraModels.push_back(stereoModel);
}
if(localKeyPoints && localKeyPointsMsgs.size() == cameraInfoMsgs.size())
{
src/impl/CommonDataSubscriberDepth.cpp
+32 -32
View File
@@ -40,7 +40,7 @@ void CommonDataSubscriber::depthCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScan2dCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -52,7 +52,7 @@ void CommonDataSubscriber::depthScan2dCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScan3dCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -64,7 +64,7 @@ void CommonDataSubscriber::depthScan3dCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scan2dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScanDescCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -80,7 +80,7 @@ void CommonDataSubscriber::depthScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::depthInfoCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -92,7 +92,7 @@ void CommonDataSubscriber::depthInfoCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scan2dMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScan2dInfoCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -104,7 +104,7 @@ void CommonDataSubscriber::depthScan2dInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScan3dInfoCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -116,7 +116,7 @@ void CommonDataSubscriber::depthScan3dInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scan2dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScanDescInfoCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -132,7 +132,7 @@ void CommonDataSubscriber::depthScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
// RGB + Depth + Odom
@@ -146,7 +146,7 @@ void CommonDataSubscriber::depthOdomCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -158,7 +158,7 @@ void CommonDataSubscriber::depthOdomScan2dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -170,7 +170,7 @@ void CommonDataSubscriber::depthOdomScan3dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scan2dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScanDescCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -186,7 +186,7 @@ void CommonDataSubscriber::depthOdomScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::depthOdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -198,7 +198,7 @@ void CommonDataSubscriber::depthOdomInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scan2dMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScan2dInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -210,7 +210,7 @@ void CommonDataSubscriber::depthOdomScan2dInfoCallback(
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScan3dInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -222,7 +222,7 @@ void CommonDataSubscriber::depthOdomScan3dInfoCallback(
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scan2dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScanDescInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -238,7 +238,7 @@ void CommonDataSubscriber::depthOdomScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -253,7 +253,7 @@ void CommonDataSubscriber::depthDataCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScan2dCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -265,7 +265,7 @@ void CommonDataSubscriber::depthDataScan2dCallback(
nav_msgs::OdometryConstPtr odomMsg; // null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScan3dCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -277,7 +277,7 @@ void CommonDataSubscriber::depthDataScan3dCallback(
nav_msgs::OdometryConstPtr odomMsg; // null
sensor_msgs::LaserScan scan2dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScanDescCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -293,7 +293,7 @@ void CommonDataSubscriber::depthDataScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::depthDataInfoCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -305,7 +305,7 @@ void CommonDataSubscriber::depthDataInfoCallback(
nav_msgs::OdometryConstPtr odomMsg; // null
sensor_msgs::LaserScan scan2dMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScan2dInfoCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -317,7 +317,7 @@ void CommonDataSubscriber::depthDataScan2dInfoCallback(
{
nav_msgs::OdometryConstPtr odomMsg; // null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScan3dInfoCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -329,7 +329,7 @@ void CommonDataSubscriber::depthDataScan3dInfoCallback(
{
nav_msgs::OdometryConstPtr odomMsg; // null
sensor_msgs::LaserScan scan2dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScanDescInfoCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -345,7 +345,7 @@ void CommonDataSubscriber::depthDataScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
// RGB + Depth + Odom + User Data
@@ -359,7 +359,7 @@ void CommonDataSubscriber::depthOdomDataCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -371,7 +371,7 @@ void CommonDataSubscriber::depthOdomDataScan2dCallback(
{
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -383,7 +383,7 @@ void CommonDataSubscriber::depthOdomDataScan3dCallback(
{
sensor_msgs::LaserScan scan2dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScanDescCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -399,7 +399,7 @@ void CommonDataSubscriber::depthOdomDataScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::depthOdomDataInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -411,7 +411,7 @@ void CommonDataSubscriber::depthOdomDataInfoCallback(
{
sensor_msgs::LaserScan scan2dMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -423,7 +423,7 @@ void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -435,7 +435,7 @@ void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
sensor_msgs::LaserScan scan2dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScanDescInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -451,7 +451,7 @@ void CommonDataSubscriber::depthOdomDataScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
#endif
src/impl/CommonDataSubscriberRGB.cpp
+32 -32
View File
@@ -40,7 +40,7 @@ void CommonDataSubscriber::rgbCallback(
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbScan2dCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -52,7 +52,7 @@ void CommonDataSubscriber::rgbScan2dCallback(
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbScan3dCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -64,7 +64,7 @@ void CommonDataSubscriber::rgbScan3dCallback(
sensor_msgs::LaserScan scan2dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbScanDescCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -80,7 +80,7 @@ void CommonDataSubscriber::rgbScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::rgbInfoCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -92,7 +92,7 @@ void CommonDataSubscriber::rgbInfoCallback(
sensor_msgs::LaserScan scan2dMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbScan2dInfoCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -104,7 +104,7 @@ void CommonDataSubscriber::rgbScan2dInfoCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbScan3dInfoCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -116,7 +116,7 @@ void CommonDataSubscriber::rgbScan3dInfoCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scan2dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbScanDescInfoCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -132,7 +132,7 @@ void CommonDataSubscriber::rgbScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
// RGB + Odom
@@ -146,7 +146,7 @@ void CommonDataSubscriber::rgbOdomCallback(
sensor_msgs::PointCloud2 scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -158,7 +158,7 @@ void CommonDataSubscriber::rgbOdomScan2dCallback(
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -170,7 +170,7 @@ void CommonDataSubscriber::rgbOdomScan3dCallback(
sensor_msgs::LaserScan scan2dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomScanDescCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -186,7 +186,7 @@ void CommonDataSubscriber::rgbOdomScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::rgbOdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -198,7 +198,7 @@ void CommonDataSubscriber::rgbOdomInfoCallback(
sensor_msgs::LaserScan scan2dMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -210,7 +210,7 @@ void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -222,7 +222,7 @@ void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scan2dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomScanDescInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -238,7 +238,7 @@ void CommonDataSubscriber::rgbOdomScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -253,7 +253,7 @@ void CommonDataSubscriber::rgbDataCallback(
sensor_msgs::PointCloud2 scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbDataScan2dCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -265,7 +265,7 @@ void CommonDataSubscriber::rgbDataScan2dCallback(
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbDataScan3dCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -277,7 +277,7 @@ void CommonDataSubscriber::rgbDataScan3dCallback(
sensor_msgs::LaserScan scan2dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbDataScanDescCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -293,7 +293,7 @@ void CommonDataSubscriber::rgbDataScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::rgbDataInfoCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -305,7 +305,7 @@ void CommonDataSubscriber::rgbDataInfoCallback(
sensor_msgs::LaserScan scan2dMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbDataScan2dInfoCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -317,7 +317,7 @@ void CommonDataSubscriber::rgbDataScan2dInfoCallback(
nav_msgs::OdometryConstPtr odomMsg; // null
sensor_msgs::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbDataScan3dInfoCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -329,7 +329,7 @@ void CommonDataSubscriber::rgbDataScan3dInfoCallback(
nav_msgs::OdometryConstPtr odomMsg; // null
sensor_msgs::LaserScan scan2dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbDataScanDescInfoCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -345,7 +345,7 @@ void CommonDataSubscriber::rgbDataScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
// RGB + Depth + Odom + User Data
@@ -359,7 +359,7 @@ void CommonDataSubscriber::rgbOdomDataCallback(
sensor_msgs::PointCloud2 scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomDataScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -371,7 +371,7 @@ void CommonDataSubscriber::rgbOdomDataScan2dCallback(
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomDataScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -383,7 +383,7 @@ void CommonDataSubscriber::rgbOdomDataScan3dCallback(
sensor_msgs::LaserScan scan2dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomDataScanDescCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -399,7 +399,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::rgbOdomDataInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -411,7 +411,7 @@ void CommonDataSubscriber::rgbOdomDataInfoCallback(
sensor_msgs::LaserScan scan2dMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -423,7 +423,7 @@ void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
{
sensor_msgs::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -435,7 +435,7 @@ void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
{
sensor_msgs::LaserScan scan2dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -451,7 +451,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
#endif
src/impl/CommonDataSubscriberRGBD.cpp
+20 -20
View File
@@ -52,7 +52,7 @@ void CommonDataSubscriber::rgbdCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -76,7 +76,7 @@ void CommonDataSubscriber::rgbdScan2dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
*scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -100,7 +100,7 @@ void CommonDataSubscriber::rgbdScan3dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, *scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -123,7 +123,7 @@ void CommonDataSubscriber::rgbdScanDescCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -147,7 +147,7 @@ void CommonDataSubscriber::rgbdInfoCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -173,7 +173,7 @@ void CommonDataSubscriber::rgbdOdomCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -197,7 +197,7 @@ void CommonDataSubscriber::rgbdOdomScan2dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
*scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -221,7 +221,7 @@ void CommonDataSubscriber::rgbdOdomScan3dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, *scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -248,7 +248,7 @@ void CommonDataSubscriber::rgbdOdomScanDescCallback(
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -272,7 +272,7 @@ void CommonDataSubscriber::rgbdOdomInfoCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -299,7 +299,7 @@ void CommonDataSubscriber::rgbdDataCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -323,7 +323,7 @@ void CommonDataSubscriber::rgbdDataScan2dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
*scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -347,7 +347,7 @@ void CommonDataSubscriber::rgbdDataScan3dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, *scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -374,7 +374,7 @@ void CommonDataSubscriber::rgbdDataScanDescCallback(
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -398,7 +398,7 @@ void CommonDataSubscriber::rgbdDataInfoCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, *scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -424,7 +424,7 @@ void CommonDataSubscriber::rgbdOdomDataCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -448,7 +448,7 @@ void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
*scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -472,7 +472,7 @@ void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg ,rgb,
commonSingleCameraCallback(odomMsg, userDataMsg ,rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, *scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -499,7 +499,7 @@ void CommonDataSubscriber::rgbdOdomDataScanDescCallback(
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg ,rgb,
commonSingleCameraCallback(odomMsg, userDataMsg ,rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -523,7 +523,7 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
src/impl/CommonDataSubscriberRGBD2.cpp
+23 -20
View File
@@ -44,6 +44,9 @@ namespace rtabmap_ros {
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
@@ -71,7 +74,7 @@ void CommonDataSubscriber::rgbd2Callback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2Scan2dCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -84,7 +87,7 @@ void CommonDataSubscriber::rgbd2Scan2dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2Scan3dCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -97,7 +100,7 @@ void CommonDataSubscriber::rgbd2Scan3dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2ScanDescCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -113,7 +116,7 @@ void CommonDataSubscriber::rgbd2ScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2InfoCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -126,7 +129,7 @@ void CommonDataSubscriber::rgbd2InfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 2 RGBD + Odom
void CommonDataSubscriber::rgbd2OdomCallback(
@@ -140,7 +143,7 @@ void CommonDataSubscriber::rgbd2OdomCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -153,7 +156,7 @@ void CommonDataSubscriber::rgbd2OdomScan2dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -166,7 +169,7 @@ void CommonDataSubscriber::rgbd2OdomScan3dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomScanDescCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -182,7 +185,7 @@ void CommonDataSubscriber::rgbd2OdomScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -195,7 +198,7 @@ void CommonDataSubscriber::rgbd2OdomInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -211,7 +214,7 @@ void CommonDataSubscriber::rgbd2DataCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2DataScan2dCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -224,7 +227,7 @@ void CommonDataSubscriber::rgbd2DataScan2dCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2DataScan3dCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -237,7 +240,7 @@ void CommonDataSubscriber::rgbd2DataScan3dCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2DataScanDescCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -253,7 +256,7 @@ void CommonDataSubscriber::rgbd2DataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2DataInfoCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -266,7 +269,7 @@ void CommonDataSubscriber::rgbd2DataInfoCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 2 RGBD + Odom + User Data
@@ -281,7 +284,7 @@ void CommonDataSubscriber::rgbd2OdomDataCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
@@ -294,7 +297,7 @@ void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
@@ -307,7 +310,7 @@ void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomDataScanDescCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
@@ -323,7 +326,7 @@ void CommonDataSubscriber::rgbd2OdomDataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
@@ -336,7 +339,7 @@ void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#endif
src/impl/CommonDataSubscriberRGBD3.cpp
+44 -40
View File
@@ -46,6 +46,10 @@ namespace rtabmap_ros {
cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
@@ -79,8 +83,8 @@ void CommonDataSubscriber::rgbd3Callback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -96,8 +100,8 @@ void CommonDataSubscriber::rgbd3Scan2dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, *scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -113,8 +117,8 @@ void CommonDataSubscriber::rgbd3Scan3dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -133,8 +137,8 @@ void CommonDataSubscriber::rgbd3ScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -150,8 +154,8 @@ void CommonDataSubscriber::rgbd3InfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -169,8 +173,8 @@ void CommonDataSubscriber::rgbd3OdomCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -186,8 +190,8 @@ void CommonDataSubscriber::rgbd3OdomScan2dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, *scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -203,8 +207,8 @@ void CommonDataSubscriber::rgbd3OdomScan3dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -223,8 +227,8 @@ void CommonDataSubscriber::rgbd3OdomScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -240,8 +244,8 @@ void CommonDataSubscriber::rgbd3OdomInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -260,8 +264,8 @@ void CommonDataSubscriber::rgbd3DataCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -277,8 +281,8 @@ void CommonDataSubscriber::rgbd3DataScan2dCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, *scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -294,8 +298,8 @@ void CommonDataSubscriber::rgbd3DataScan3dCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -314,8 +318,8 @@ void CommonDataSubscriber::rgbd3DataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -331,8 +335,8 @@ void CommonDataSubscriber::rgbd3DataInfoCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -349,8 +353,8 @@ void CommonDataSubscriber::rgbd3OdomDataCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -366,8 +370,8 @@ void CommonDataSubscriber::rgbd3OdomDataScan2dCallback(
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, *scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -383,8 +387,8 @@ void CommonDataSubscriber::rgbd3OdomDataScan3dCallback(
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -403,8 +407,8 @@ void CommonDataSubscriber::rgbd3OdomDataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -420,8 +424,8 @@ void CommonDataSubscriber::rgbd3OdomDataInfoCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
src/impl/CommonDataSubscriberRGBD4.cpp
+25 -20
View File
@@ -48,6 +48,11 @@ namespace rtabmap_ros {
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image4Msg->depth_camera_info); \
std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
@@ -87,7 +92,7 @@ void CommonDataSubscriber::rgbd4Callback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4Scan2dCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -102,7 +107,7 @@ void CommonDataSubscriber::rgbd4Scan2dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4Scan3dCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -117,7 +122,7 @@ void CommonDataSubscriber::rgbd4Scan3dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4ScanDescCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -135,7 +140,7 @@ void CommonDataSubscriber::rgbd4ScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4InfoCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -150,7 +155,7 @@ void CommonDataSubscriber::rgbd4InfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 2 RGBD + Odom
@@ -167,7 +172,7 @@ void CommonDataSubscriber::rgbd4OdomCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -182,7 +187,7 @@ void CommonDataSubscriber::rgbd4OdomScan2dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -197,7 +202,7 @@ void CommonDataSubscriber::rgbd4OdomScan3dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomScanDescCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -215,7 +220,7 @@ void CommonDataSubscriber::rgbd4OdomScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -230,7 +235,7 @@ void CommonDataSubscriber::rgbd4OdomInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -248,7 +253,7 @@ void CommonDataSubscriber::rgbd4DataCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataScan2dCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -263,7 +268,7 @@ void CommonDataSubscriber::rgbd4DataScan2dCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataScan3dCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -278,7 +283,7 @@ void CommonDataSubscriber::rgbd4DataScan3dCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataScanDescCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -296,7 +301,7 @@ void CommonDataSubscriber::rgbd4DataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataInfoCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -311,7 +316,7 @@ void CommonDataSubscriber::rgbd4DataInfoCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 2 RGBD + Odom + User Data
@@ -328,7 +333,7 @@ void CommonDataSubscriber::rgbd4OdomDataCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
@@ -343,7 +348,7 @@ void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
@@ -358,7 +363,7 @@ void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataScanDescCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
@@ -376,7 +381,7 @@ void CommonDataSubscriber::rgbd4OdomDataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
@@ -391,7 +396,7 @@ void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#endif
src/impl/CommonDataSubscriberRGBD5.cpp
+16 -10
View File
@@ -50,6 +50,12 @@ namespace rtabmap_ros {
cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image4Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image5Msg->depth_camera_info); \
std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
@@ -95,7 +101,7 @@ void CommonDataSubscriber::rgbd5Callback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5Scan2dCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -111,7 +117,7 @@ void CommonDataSubscriber::rgbd5Scan2dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5Scan3dCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -127,7 +133,7 @@ void CommonDataSubscriber::rgbd5Scan3dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5ScanDescCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -146,7 +152,7 @@ void CommonDataSubscriber::rgbd5ScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5InfoCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -162,7 +168,7 @@ void CommonDataSubscriber::rgbd5InfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 5 RGBD + Odom
@@ -180,7 +186,7 @@ void CommonDataSubscriber::rgbd5OdomCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5OdomScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -196,7 +202,7 @@ void CommonDataSubscriber::rgbd5OdomScan2dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5OdomScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -212,7 +218,7 @@ void CommonDataSubscriber::rgbd5OdomScan3dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5OdomScanDescCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -231,7 +237,7 @@ void CommonDataSubscriber::rgbd5OdomScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5OdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -247,7 +253,7 @@ void CommonDataSubscriber::rgbd5OdomInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::setupRGBD5Callbacks(
src/impl/CommonDataSubscriberRGBD6.cpp
+17 -10
View File
@@ -52,6 +52,13 @@ namespace rtabmap_ros {
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image6Msg->rgb_camera_info); \
std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image4Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image5Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image6Msg->depth_camera_info); \
std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
@@ -103,7 +110,7 @@ void CommonDataSubscriber::rgbd6Callback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6Scan2dCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -120,7 +127,7 @@ void CommonDataSubscriber::rgbd6Scan2dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6Scan3dCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -137,7 +144,7 @@ void CommonDataSubscriber::rgbd6Scan3dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6ScanDescCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -157,7 +164,7 @@ void CommonDataSubscriber::rgbd6ScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6InfoCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -174,7 +181,7 @@ void CommonDataSubscriber::rgbd6InfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 6 RGBD + Odom
@@ -193,7 +200,7 @@ void CommonDataSubscriber::rgbd6OdomCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6OdomScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -210,7 +217,7 @@ void CommonDataSubscriber::rgbd6OdomScan2dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6OdomScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -227,7 +234,7 @@ void CommonDataSubscriber::rgbd6OdomScan3dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6OdomScanDescCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -247,7 +254,7 @@ void CommonDataSubscriber::rgbd6OdomScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6OdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -264,7 +271,7 @@ void CommonDataSubscriber::rgbd6OdomInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::setupRGBD6Callbacks(
src/impl/CommonDataSubscriberRGBDX.cpp
+22 -20
View File
@@ -39,6 +39,7 @@ namespace rtabmap_ros {
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(imagesMsg->rgbd_images.size()); \
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(imagesMsg->rgbd_images.size()); \
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
@@ -47,6 +48,7 @@ namespace rtabmap_ros {
{ \
rtabmap_ros::toCvShare(imagesMsg->rgbd_images[i], imagesMsg, imageMsgs[i], depthMsgs[i]); \
cameraInfoMsgs.push_back(imagesMsg->rgbd_images[i].rgb_camera_info); \
depthCameraInfoMsgs.push_back(imagesMsg->rgbd_images[i].depth_camera_info); \
if(!imagesMsg->rgbd_images[i].global_descriptor.data.empty()) \
globalDescriptorMsgs.push_back(imagesMsg->rgbd_images[i].global_descriptor); \
localKeyPoints.push_back(imagesMsg->rgbd_images[i].key_points); \
@@ -67,7 +69,7 @@ void CommonDataSubscriber::rgbdXCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXScan2dCallback(
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
@@ -79,7 +81,7 @@ void CommonDataSubscriber::rgbdXScan2dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXScan3dCallback(
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
@@ -91,7 +93,7 @@ void CommonDataSubscriber::rgbdXScan3dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXScanDescCallback(
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
@@ -106,7 +108,7 @@ void CommonDataSubscriber::rgbdXScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXInfoCallback(
const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
@@ -118,7 +120,7 @@ void CommonDataSubscriber::rgbdXInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// X RGBD + Odom
void CommonDataSubscriber::rgbdXOdomCallback(
@@ -131,7 +133,7 @@ void CommonDataSubscriber::rgbdXOdomCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -143,7 +145,7 @@ void CommonDataSubscriber::rgbdXOdomScan2dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -155,7 +157,7 @@ void CommonDataSubscriber::rgbdXOdomScan3dCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomScanDescCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -170,7 +172,7 @@ void CommonDataSubscriber::rgbdXOdomScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -182,7 +184,7 @@ void CommonDataSubscriber::rgbdXOdomInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -197,7 +199,7 @@ void CommonDataSubscriber::rgbdXDataCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXDataScan2dCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -209,7 +211,7 @@ void CommonDataSubscriber::rgbdXDataScan2dCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXDataScan3dCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -221,7 +223,7 @@ void CommonDataSubscriber::rgbdXDataScan3dCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXDataScanDescCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -236,7 +238,7 @@ void CommonDataSubscriber::rgbdXDataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXDataInfoCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -248,7 +250,7 @@ void CommonDataSubscriber::rgbdXDataInfoCallback(
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// X RGBD + Odom + User Data
@@ -262,7 +264,7 @@ void CommonDataSubscriber::rgbdXOdomDataCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomDataScan2dCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
@@ -274,7 +276,7 @@ void CommonDataSubscriber::rgbdXOdomDataScan2dCallback(
sensor_msgs::PointCloud2 scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomDataScan3dCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
@@ -286,7 +288,7 @@ void CommonDataSubscriber::rgbdXOdomDataScan3dCallback(
sensor_msgs::LaserScan scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomDataScanDescCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
@@ -301,7 +303,7 @@ void CommonDataSubscriber::rgbdXOdomDataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomDataInfoCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
@@ -313,7 +315,7 @@ void CommonDataSubscriber::rgbdXOdomDataInfoCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#endif
src/impl/CommonDataSubscriberStereo.cpp
+4 -4
View File
@@ -42,7 +42,7 @@ void CommonDataSubscriber::stereoCallback(
sensor_msgs::LaserScan scanMsg; // null
sensor_msgs::PointCloud2 scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::stereoInfoCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
@@ -56,7 +56,7 @@ void CommonDataSubscriber::stereoInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scan2dMsg; // null
sensor_msgs::PointCloud2 scan3dMsg; // null
commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
// Stereo + Odom
@@ -72,7 +72,7 @@ void CommonDataSubscriber::stereoOdomCallback(
sensor_msgs::LaserScan scanMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::stereoOdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -86,7 +86,7 @@ void CommonDataSubscriber::stereoOdomInfoCallback(
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScan scan2dMsg; // Null
sensor_msgs::PointCloud2 scan3dMsg; // Null
commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::setupStereoCallbacks(