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