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

CoreWrapper/GuiWrapper: removed fake camera model when only lidar is used

Browse Source
This commit is contained in:
matlabbe
2021-01-22 12:53:49 -05:00
parent d21b03ae7a
commit 21090899d4
2 changed files with 23 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+17 -42
View File
@@ -1262,7 +1262,7 @@ void CoreWrapper::commonDepthCallbackImpl(
return;
}
ROS_WARN("%d %d %d %d", rgb.empty()?1:0, depth.empty()?1:0, scan.isEmpty()?1:0, genDepth_?1:0);
ROS_DEBUG("%d %d %d %d", rgb.empty()?1:0, depth.empty()?1:0, scan.isEmpty()?1:0, genDepth_?1:0);
if(!rgb.empty() && depth.empty() && !scan.isEmpty() && genDepth_)
{
for(size_t i=0; i<cameraModels.size(); ++i)
@@ -1285,13 +1285,20 @@ void CoreWrapper::commonDepthCallbackImpl(
}
}
cv::Mat depthProjected = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), scan.data(), model.localTransform());
cv::Mat depthProjected = util3d::projectCloudToCamera(
model.imageSize(),
model.K(),
scan.data(),
scan.localTransform().inverse()*model.localTransform());
if(genDepthFillHolesSize_ > 0 && genDepthFillIterations_ > 0)
{
for(int i=0; i<genDepthFillIterations_;++i)
{
depthProjected = rtabmap::util2d::fillDepthHoles(depthProjected, genDepthFillHolesSize_, genDepthFillHolesError_);
depthProjected = util2d::fillDepthHoles(
depthProjected,
genDepthFillHolesSize_,
genDepthFillHolesError_);
}
}
@@ -1712,22 +1719,11 @@ void CoreWrapper::commonLaserScanCallback(
userData_ = cv::Mat();
}
cv::Mat rgb = cv::Mat::zeros(3,4,CV_8UC1);
cv::Mat depth = cv::Mat::zeros(3,4,CV_16UC1);
CameraModel model(
2,
2,
2,
1.5,
scan.localTransform()*Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
0,
cv::Size(4,3));
SensorData data(
scan,
rgb,
depth,
model,
cv::Mat(),
cv::Mat(),
CameraModel(),
lastPoseIntermediate_?-1:!scan2dMsg.ranges.empty()?scan2dMsg.header.seq:scan3dMsg.header.seq,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
@@ -1787,21 +1783,10 @@ void CoreWrapper::commonOdomCallback(
userData_ = cv::Mat();
}
cv::Mat rgb = cv::Mat::zeros(3,4,CV_8UC1);
cv::Mat depth = cv::Mat::zeros(3,4,CV_16UC1);
CameraModel model(
2,
2,
2,
1.5,
Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
0,
cv::Size(4,3));
SensorData data(
rgb,
depth,
model,
cv::Mat(),
cv::Mat(),
CameraModel(),
lastPoseIntermediate_?-1:odomMsg->header.seq,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
@@ -1872,17 +1857,7 @@ void CoreWrapper::process(
}
}
cv::Mat rgb = cv::Mat::zeros(3,4,CV_8UC1);
cv::Mat depth = cv::Mat::zeros(3,4,CV_16UC1);
CameraModel model(
2,
2,
2,
1.5,
Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
0,
cv::Size(4,3));
SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
SensorData interData(cv::Mat(), cv::Mat(), CameraModel(), -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
Transform gt;
if(!groundTruthFrameId_.empty())
{
src/GuiWrapper.cpp
+6 -39
View File
@@ -835,7 +835,6 @@ void GuiWrapper::commonLaserScanCallback(
LaserScan scan;
rtabmap::OdometryInfo info;
bool ignoreData = false;
Transform fakeCameraLocalTransform;
// limit update rate
if(maxOdomUpdateRate_<=0.0 ||
@@ -884,16 +883,6 @@ void GuiWrapper::commonLaserScanCallback(
}
else if(odomInfoMsg.get())
{
//just get scan local transform to adjust camera frame
if(!scan2dMsg.ranges.empty())
{
fakeCameraLocalTransform = getTransform(frameId, scan2dMsg.header.frame_id, scan2dMsg.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
}
else if(!scan3dMsg.data.empty())
{
fakeCameraLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
}
info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData();
ignoreData = true;
}
@@ -903,24 +892,13 @@ void GuiWrapper::commonLaserScanCallback(
return;
}
cv::Mat rgb;
cv::Mat depth;
CameraModel model(
2,
2,
2,
1.5,
(fakeCameraLocalTransform.isNull()?scan.localTransform():fakeCameraLocalTransform)*Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
0,
cv::Size(4,3));
info.reg.covariance = covariance;
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
rgb,
depth,
model,
cv::Mat(),
cv::Mat(),
CameraModel(),
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
@@ -987,23 +965,12 @@ void GuiWrapper::commonOdomCallback(
return;
}
cv::Mat rgb;
cv::Mat depth;
CameraModel model(
2,
2,
2,
1.5,
Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
0,
cv::Size(4,3));
info.reg.covariance = covariance;
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
rgb,
depth,
model,
cv::Mat(),
cv::Mat(),
CameraModel(),
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,