mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-07 10:17:45 +08:00
rtabmap/rtabmapviz: Refactored to support multi-stereo input
This commit is contained in:
matlabbe
+100
-262
@@ -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,
|
||||
|
||||
Reference in New Issue
Block a user