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

fixed compilation with new multicam branch

Browse Source
This commit is contained in:
matlabbe
2022-07-07 13:44:19 -04:00
parent c4a6fba8e8
commit eb932a86ee
13 changed files with 128 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+3 -13
View File
@@ -521,16 +521,6 @@ void CoreWrapper::onInit()
bool subscribeRGBD = false;
pnh.param("rgbd_cameras", cameras, cameras);
pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
if(subscribeRGBD && cameras> 1 && estimationType>0)
{
NODELET_WARN("Setting \"%s\" parameter to 0 (%d is not supported "
"for multi-cameras) as \"subscribe_rgbd\" is "
"true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.",
Parameters::kVisEstimationType().c_str(),
estimationType,
Parameters::kVisEstimationType().c_str());
uInsert(parameters_, ParametersPair(Parameters::kVisEstimationType(), "0"));
}
// modify default parameters with those in the database
if(!deleteDbOnStart)
@@ -1794,7 +1784,7 @@ void CoreWrapper::commonLaserScanCallback(
scan,
cv::Mat(),
cv::Mat(),
CameraModel(),
rtabmap::CameraModel(),
lastPoseIntermediate_?-1:!scan2dMsg.ranges.empty()?scan2dMsg.header.seq:scan3dMsg.header.seq,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
@@ -1858,7 +1848,7 @@ void CoreWrapper::commonOdomCallback(
SensorData data(
cv::Mat(),
cv::Mat(),
CameraModel(),
rtabmap::CameraModel(),
lastPoseIntermediate_?-1:odomMsg->header.seq,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
@@ -1943,7 +1933,7 @@ void CoreWrapper::process(
covariance.at<double>(4,4) = uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
}
SensorData interData(cv::Mat(), cv::Mat(), CameraModel(), -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
SensorData interData(cv::Mat(), cv::Mat(), rtabmap::CameraModel(), -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
Transform gt;
if(!groundTruthFrameId_.empty())
{
src/DbPlayerNode.cpp
+29 -22
View File
@@ -293,30 +293,37 @@ int main(int argc, char** argv)
}
else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
{
//stereo
if(odom.data().stereoCameraModel().isValidForProjection())
if(odom.data().stereoCameraModels().size() > 1)
{
camInfoA.D.resize(8,0);
camInfoA.P[0] = odom.data().stereoCameraModel().left().fx();
camInfoA.K[0] = odom.data().stereoCameraModel().left().fx();
camInfoA.P[5] = odom.data().stereoCameraModel().left().fy();
camInfoA.K[4] = odom.data().stereoCameraModel().left().fy();
camInfoA.P[2] = odom.data().stereoCameraModel().left().cx();
camInfoA.K[2] = odom.data().stereoCameraModel().left().cx();
camInfoA.P[6] = odom.data().stereoCameraModel().left().cy();
camInfoA.K[5] = odom.data().stereoCameraModel().left().cy();
camInfoB = camInfoA;
camInfoB.P[3] = odom.data().stereoCameraModel().right().Tx(); // Right_Tx = -baseline*fx
ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
}
else
{
//stereo
if(odom.data().stereoCameraModels()[0].isValidForProjection())
{
camInfoA.D.resize(8,0);
type=1;
camInfoA.P[0] = odom.data().stereoCameraModels()[0].left().fx();
camInfoA.K[0] = odom.data().stereoCameraModels()[0].left().fx();
camInfoA.P[5] = odom.data().stereoCameraModels()[0].left().fy();
camInfoA.K[4] = odom.data().stereoCameraModels()[0].left().fy();
camInfoA.P[2] = odom.data().stereoCameraModels()[0].left().cx();
camInfoA.K[2] = odom.data().stereoCameraModels()[0].left().cx();
camInfoA.P[6] = odom.data().stereoCameraModels()[0].left().cy();
camInfoA.K[5] = odom.data().stereoCameraModels()[0].left().cy();
if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
camInfoB = camInfoA;
camInfoB.P[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx
}
type=1;
if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
}
}
else
@@ -383,9 +390,9 @@ int main(int argc, char** argv)
{
localTransform = odom.data().cameraModels()[0].localTransform();
}
else if(odom.data().stereoCameraModel().isValidForProjection())
else if(odom.data().stereoCameraModels().size() == 1)
{
localTransform = odom.data().stereoCameraModel().left().localTransform();
localTransform = odom.data().stereoCameraModels()[0].left().localTransform();
}
if(!localTransform.isNull())
{
src/GuiWrapper.cpp
+3 -3
View File
@@ -523,7 +523,7 @@ void GuiWrapper::commonDepthCallback(
cv::Mat rgb;
cv::Mat depth;
std::vector<CameraModel> cameraModels;
std::vector<rtabmap::CameraModel> cameraModels;
LaserScan scan;
rtabmap::OdometryInfo info;
bool ignoreData = false;
@@ -937,7 +937,7 @@ void GuiWrapper::commonLaserScanCallback(
scan,
cv::Mat(),
cv::Mat(),
CameraModel(),
rtabmap::CameraModel(),
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
@@ -1009,7 +1009,7 @@ void GuiWrapper::commonOdomCallback(
rtabmap::SensorData(
cv::Mat(),
cv::Mat(),
CameraModel(),
rtabmap::CameraModel(),
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
src/MsgConversion.cpp
+64 -43
View File
@@ -225,14 +225,14 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_ros::RGBDImage & m
msg.rgb_camera_info.header = header;
localTransform = data.cameraModels().front().localTransform();
}
else
else if(data.stereoCameraModels().size() == 1)
{
//stereo
rtabmap_ros::cameraModelToROS(data.stereoCameraModel().left(), msg.rgb_camera_info);
rtabmap_ros::cameraModelToROS(data.stereoCameraModel().right(), msg.depth_camera_info);
rtabmap_ros::cameraModelToROS(data.stereoCameraModels()[0].left(), msg.rgb_camera_info);
rtabmap_ros::cameraModelToROS(data.stereoCameraModels()[0].right(), msg.depth_camera_info);
msg.rgb_camera_info.header = header;
msg.depth_camera_info.header = header;
localTransform = data.stereoCameraModel().localTransform();
localTransform = data.stereoCameraModels()[0].localTransform();
}
if(!data.imageRaw().empty())
@@ -1109,27 +1109,30 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
}
}
rtabmap::StereoCameraModel stereoModel;
std::vector<rtabmap::StereoCameraModel> stereoModels;
std::vector<rtabmap::CameraModel> models;
if(msg.baseline > 0.0f)
if(msg.baseline.size())
{
// stereo model
if(msg.fx.size() == 1 &&
msg.fy.size() == 1 &&
msg.cx.size() == 1 &&
msg.cy.size() == 1 &&
msg.width.size() == 1 &&
msg.height.size() == 1 &&
msg.localTransform.size() == 1)
if(msg.fx.size() == msg.baseline.size() &&
msg.fy.size() == msg.baseline.size() &&
msg.cx.size() == msg.baseline.size() &&
msg.cy.size() == msg.baseline.size() &&
msg.width.size() == msg.baseline.size() &&
msg.height.size() == msg.baseline.size() &&
msg.localTransform.size() == msg.baseline.size())
{
stereoModel = rtabmap::StereoCameraModel(
msg.fx[0],
msg.fy[0],
msg.cx[0],
msg.cy[0],
msg.baseline,
transformFromGeometryMsg(msg.localTransform[0]),
cv::Size(msg.width[0], msg.height[0]));
for(unsigned int i=0; i<msg.fx.size(); ++i)
{
stereoModels.push_back(rtabmap::StereoCameraModel(
msg.fx[i],
msg.fy[i],
msg.cx[i],
msg.cy[i],
msg.baseline[i],
transformFromGeometryMsg(msg.localTransform[i]),
cv::Size(msg.width[i], msg.height[i])));
}
}
}
else
@@ -1170,7 +1173,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
msg.label,
transformFromPoseMsg(msg.pose),
transformFromPoseMsg(msg.groundTruthPose),
stereoModel.isValidForProjection()?
stereoModels.size()?
rtabmap::SensorData(
rtabmap::LaserScan(compressedMatFromBytes(msg.laserScan),
msg.laserScanMaxPts,
@@ -1179,7 +1182,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
transformFromGeometryMsg(msg.laserScanLocalTransform)),
compressedMatFromBytes(msg.image),
compressedMatFromBytes(msg.depth),
stereoModel,
stereoModels,
msg.id,
msg.stamp,
compressedMatFromBytes(msg.userData)):
@@ -1236,7 +1239,6 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
msg.laserScanMaxRange = signature.sensorData().laserScanCompressed().rangeMax();
msg.laserScanFormat = signature.sensorData().laserScanCompressed().format();
transformToGeometryMsg(signature.sensorData().laserScanCompressed().localTransform(), msg.laserScanLocalTransform);
msg.baseline = 0;
if(signature.sensorData().cameraModels().size())
{
msg.fx.resize(signature.sensorData().cameraModels().size());
@@ -1257,17 +1259,27 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.localTransform[i]);
}
}
else if(signature.sensorData().stereoCameraModel().isValidForProjection())
else if(signature.sensorData().stereoCameraModels().size())
{
msg.fx.push_back(signature.sensorData().stereoCameraModel().left().fx());
msg.fy.push_back(signature.sensorData().stereoCameraModel().left().fy());
msg.cx.push_back(signature.sensorData().stereoCameraModel().left().cx());
msg.cy.push_back(signature.sensorData().stereoCameraModel().left().cy());
msg.width.push_back(signature.sensorData().stereoCameraModel().left().imageWidth());
msg.height.push_back(signature.sensorData().stereoCameraModel().left().imageHeight());
msg.baseline = signature.sensorData().stereoCameraModel().baseline();
msg.localTransform.resize(1);
transformToGeometryMsg(signature.sensorData().stereoCameraModel().left().localTransform(), msg.localTransform[0]);
msg.fx.resize(signature.sensorData().stereoCameraModels().size());
msg.fy.resize(signature.sensorData().stereoCameraModels().size());
msg.cx.resize(signature.sensorData().stereoCameraModels().size());
msg.cy.resize(signature.sensorData().stereoCameraModels().size());
msg.width.resize(signature.sensorData().stereoCameraModels().size());
msg.height.resize(signature.sensorData().stereoCameraModels().size());
msg.baseline.resize(signature.sensorData().stereoCameraModels().size());
msg.localTransform.resize(signature.sensorData().stereoCameraModels().size());
for(unsigned int i=0; i<signature.sensorData().stereoCameraModels().size(); ++i)
{
msg.fx[i] = signature.sensorData().stereoCameraModels()[i].left().fx();
msg.fy[i] = signature.sensorData().stereoCameraModels()[i].left().fy();
msg.cx[i] = signature.sensorData().stereoCameraModels()[i].left().cx();
msg.cy[i] = signature.sensorData().stereoCameraModels()[i].left().cy();
msg.width[i] = signature.sensorData().stereoCameraModels()[i].left().imageWidth();
msg.height[i] = signature.sensorData().stereoCameraModels()[i].left().imageHeight();
msg.baseline[i] = signature.sensorData().stereoCameraModels()[i].baseline();
transformToGeometryMsg(signature.sensorData().stereoCameraModels()[i].left().localTransform(), msg.localTransform[i]);
}
}
//Features stuff...
@@ -1466,11 +1478,15 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg, bool ig
info.localBundleConstraints = msg.localBundleConstraints;
info.localBundleTime = msg.localBundleTime;
UASSERT(msg.localBundleModels.size() == msg.localBundleIds.size());
UASSERT(msg.localBundleModels.size() == msg.localBundleModelTransforms.size());
UASSERT(msg.localBundleModels.size() == msg.localBundlePoses.size());
for(size_t i=0; i<msg.localBundleIds.size(); ++i)
{
info.localBundleModels.insert(std::make_pair(msg.localBundleIds[i], cameraModelFromROS(msg.localBundleModels[i], transformFromGeometryMsg(msg.localBundleModelTransforms[i]))));
std::vector<rtabmap::CameraModel> models;
for(size_t j=0; j<msg.localBundleModels[i].models.size(); ++j)
{
models.push_back(cameraModelFromROS(msg.localBundleModels[i].models[j].camera_info, transformFromGeometryMsg(msg.localBundleModels[i].models[j].local_transform)));
}
info.localBundleModels.insert(std::make_pair(msg.localBundleIds[i], models));
info.localBundlePoses.insert(std::make_pair(msg.localBundleIds[i], transformFromPoseMsg(msg.localBundlePoses[i])));
}
info.keyFrameAdded = msg.keyFrameAdded;
@@ -1541,21 +1557,26 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.localBundleConstraints = info.localBundleConstraints;
msg.localBundleTime = info.localBundleTime;
UASSERT(info.localBundleModels.size() == info.localBundlePoses.size());
for(std::map<int, rtabmap::CameraModel>::const_iterator iter=info.localBundleModels.begin();
for(std::map<int, std::vector<rtabmap::CameraModel> >::const_iterator iter=info.localBundleModels.begin();
iter!=info.localBundleModels.end();
++iter)
{
msg.localBundleIds.push_back(iter->first);
sensor_msgs::CameraInfo camInfo;
cameraModelToROS(iter->second, camInfo);
msg.localBundleModels.push_back(camInfo);
geometry_msgs::Transform localT;
transformToGeometryMsg(iter->second.localTransform(), localT);
msg.localBundleModelTransforms.push_back(localT);
UASSERT(info.localBundlePoses.find(iter->first)!=info.localBundlePoses.end());
geometry_msgs::Pose pose;
transformToPoseMsg(info.localBundlePoses.at(iter->first), pose);
msg.localBundlePoses.push_back(pose);
rtabmap_ros::CameraModels models;
for(size_t i=0; i<iter->second.size(); ++i)
{
rtabmap_ros::CameraModel modelMsg;
cameraModelToROS(iter->second[i], modelMsg.camera_info);
transformToGeometryMsg(iter->second[i].localTransform(), modelMsg.local_transform);
models.models.push_back(modelMsg);
}
msg.localBundleModels.push_back(models);
}
msg.keyFrameAdded = info.keyFrameAdded;
msg.timeEstimation = info.timeEstimation;
src/StereoCameraNode.cpp
+14 -7
View File
@@ -86,13 +86,20 @@ int main(int argc, char** argv)
cv::resize(data.rightRaw(), imageRight.image, cv::Size(0,0), scale, scale, CV_INTER_AREA);
imageRightPub.publish(imageRight.toImageMsg());
sensor_msgs::CameraInfo infoLeft, infoRight;
rtabmap_ros::cameraModelToROS(data.stereoCameraModel().left().scaled(scale), infoLeft);
rtabmap_ros::cameraModelToROS(data.stereoCameraModel().right().scaled(scale), infoRight);
infoLeft.header = imageLeft.header;
infoRight.header = imageLeft.header;
infoLeftPub.publish(infoLeft);
infoRightPub.publish(infoRight);
if(data.stereoCameraModels().size())
{
sensor_msgs::CameraInfo infoLeft, infoRight;
rtabmap_ros::cameraModelToROS(data.stereoCameraModels()[0].left().scaled(scale), infoLeft);
rtabmap_ros::cameraModelToROS(data.stereoCameraModels()[0].right().scaled(scale), infoRight);
infoLeft.header = imageLeft.header;
infoRight.header = imageLeft.header;
infoLeftPub.publish(infoLeft);
infoRightPub.publish(infoRight);
}
else
{
ROS_ERROR("No calibration loaded!");
}
ros::spinOnce();
}
src/nodelets/icp_odometry.cpp
+2 -2
View File
@@ -478,7 +478,7 @@ private:
localScanTransform),
cv::Mat(),
cv::Mat(),
CameraModel(),
rtabmap::CameraModel(),
0,
rtabmap_ros::timestampFromROS(scanMsg->header.stamp));
@@ -724,7 +724,7 @@ private:
laserScan,
cv::Mat(),
cv::Mat(),
CameraModel(),
rtabmap::CameraModel(),
0,
rtabmap_ros::timestampFromROS(cloudMsg.header.stamp));
src/nodelets/rgbd_odometry.cpp
+1 -11
View File
@@ -376,16 +376,6 @@ private:
bool subscribeRGBD = false;
pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
if(subscribeRGBD && rgbdCameras> 1 && estimationType>0)
{
NODELET_WARN("Setting \"%s\" parameter to 0 (%d is not supported "
"for multi-cameras) as \"subscribe_rgbd\" is "
"true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.",
Parameters::kVisEstimationType().c_str(),
estimationType,
Parameters::kVisEstimationType().c_str());
uInsert(parameters, ParametersPair(Parameters::kVisEstimationType(), "0"));
}
}
void commonCallback(
@@ -408,7 +398,7 @@ private:
cv::Mat rgb;
cv::Mat depth;
pcl::PointCloud<pcl::PointXYZ> scanCloud;
std::vector<CameraModel> cameraModels;
std::vector<rtabmap::CameraModel> cameraModels;
double stampDiff = 0;
for(unsigned int i=0; i<rgbImages.size(); ++i)
{
src/rviz/MapCloudDisplay.cpp
+1 -1
View File
@@ -292,7 +292,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
if((fromDepth &&
!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() &&
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection())) ||
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModels().size())) ||
(!fromDepth && !s.sensorData().laserScanCompressed().isEmpty()))
{
cv::Mat image, depth;