i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

MainWindow: add fake frustum when only lidar is received

Browse Source
This commit is contained in:
matlabbe
2021-01-22 12:51:47 -05:00
parent aa31a900fb
commit 47e40ef34d
2 changed files with 48 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
guilib/src/MainWindow.cpp
View File

@@ -1410,6 +1410,30 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
{
_cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), odom.data().stereoCameraModel());
}
else if(!odom.data().laserScanRaw().isEmpty() ||
!odom.data().laserScanCompressed().isEmpty())
{
Transform scanLocalTransform;
if(!odom.data().laserScanRaw().isEmpty())
{
scanLocalTransform = odom.data().laserScanRaw().localTransform();
}
else
{
scanLocalTransform = odom.data().laserScanCompressed().localTransform();
}
//fake frustum
CameraModel model(
2,
2,
2,
1.5,
scanLocalTransform*CameraModel::opticalRotation(),
0,
cv::Size(4,3));
_cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), model);
}
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
if(_preferencesDialog->isFramesShown())
{
@@ -2182,6 +2206,29 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
{
_cloudViewer->updateCameraFrustum(poses.rbegin()->second, stat.getLastSignatureData().sensorData().stereoCameraModel());
}
else if(!stat.getLastSignatureData().sensorData().laserScanRaw().isEmpty() ||
!stat.getLastSignatureData().sensorData().laserScanCompressed().isEmpty())
{
Transform scanLocalTransform;
if(!stat.getLastSignatureData().sensorData().laserScanRaw().isEmpty())
{
scanLocalTransform = stat.getLastSignatureData().sensorData().laserScanRaw().localTransform();
}
else
{
scanLocalTransform = stat.getLastSignatureData().sensorData().laserScanCompressed().localTransform();
}
//fake frustum
CameraModel model(
2,
2,
2,
1.5,
scanLocalTransform*CameraModel::opticalRotation(),
0,
cv::Size(4,3));
_cloudViewer->updateCameraFrustum(poses.rbegin()->second, model);
}
}
_cloudViewer->updateCameraTargetPosition(poses.rbegin()->second);