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

CloudViewer: adjust frustum fov based on camera model (https://github.com/introlab/rtabmap_ros/issues/419)

Browse Source
This commit is contained in:
matlabbe
2020-05-24 11:13:13 -04:00
parent eb9999d7b1
commit 6acfc62196
6 changed files with 33 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
guilib/include/rtabmap/gui/CloudViewer.h
View File

@@ -256,10 +256,12 @@ public:
void addOrUpdateFrustum(
const std::string & id,
const Transform & transform,
const Transform & pose,
const Transform & localTransform,
double scale,
const QColor & color = QColor());
const QColor & color = QColor(),
float fovX=1.1,
float fovY=0.85);
bool updateFrustumPose(
const std::string & id,
const Transform & pose);

26
guilib/src/CloudViewer.cpp
View File

@@ -1831,18 +1831,20 @@ static const float frustum_vertices[] = {
0.0f, 0.0f, 0.0f,
1.0f, 1.0f, 1.0f,
1.0f, -1.0f, 1.0f,
1.0f, -1.0f, -1.0f,
1.0f, 1.0f, -1.0f};
-1.0f, -1.0f, 1.0f,
-1.0f, 1.0f, 1.0f};
static const int frustum_indices[] = {
1, 2, 3, 4, 1, 0, 2, 0, 3, 0, 4};
void CloudViewer::addOrUpdateFrustum(
const std::string & id,
const Transform & transform,
const Transform & pose,
const Transform & localTransform,
double scale,
const QColor & color)
const QColor & color,
float fovX,
float fovY)
{
if(id.empty())
{
@@ -1854,7 +1856,7 @@ void CloudViewer::addOrUpdateFrustum(
this->removeFrustum(id);
#endif
if(!transform.isNull())
if(!pose.isNull())
{
if(_frustums.find(id)==_frustums.end())
{
@@ -1865,9 +1867,9 @@ void CloudViewer::addOrUpdateFrustum(
frustumSize/=3;
pcl::PointCloud<pcl::PointXYZ> frustumPoints;
frustumPoints.resize(frustumSize);
float scaleX = 0.5f * scale;
float scaleY = 0.4f * scale; //4x3 arbitrary ratio
float scaleZ = 0.3f * scale;
float scaleX = tan((fovX>0?fovX:1.1)/2.0f) * scale;
float scaleY = tan((fovY>0?fovY:0.85)/2.0f) * scale;
float scaleZ = scale;
QColor c = Qt::gray;
if(color.isValid())
{
@@ -1876,9 +1878,9 @@ void CloudViewer::addOrUpdateFrustum(
Transform opticalRotInv(0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0);
#if PCL_VERSION_COMPARE(<, 1, 7, 2)
Eigen::Affine3f t = (transform*localTransform*opticalRotInv).toEigen3f();
Eigen::Affine3f t = (pose*localTransform).toEigen3f();
#else
Eigen::Affine3f t = (localTransform*opticalRotInv).toEigen3f();
Eigen::Affine3f t = (localTransform).toEigen3f();
#endif
for(int i=0; i<frustumSize; ++i)
{
@@ -1902,7 +1904,7 @@ void CloudViewer::addOrUpdateFrustum(
_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
}
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
if(!this->updateFrustumPose(id, transform))
if(!this->updateFrustumPose(id, pose))
{
UERROR("Failed updating pose of frustum %s!?", id.c_str());
}
@@ -2686,7 +2688,7 @@ void CloudViewer::updateCameraFrustums(const Transform & pose, const std::vector
}
std::string id = uFormat("reference_frustum_%d", i);
this->removeFrustum(id);
this->addOrUpdateFrustum(id, pose, baseToCamera, _frustumScale, _frustumColor);
this->addOrUpdateFrustum(id, pose, baseToCamera, _frustumScale, _frustumColor, models[i].fovX(), models[i].fovY());
if(!baseToCamera.isIdentity())
{
this->addOrUpdateLine(uFormat("reference_frustum_line_%d", i), pose, pose * baseToCamera, _frustumColor);

2
guilib/src/DepthCalibrationDialog.cpp
View File

@@ -445,7 +445,7 @@ void DepthCalibrationDialog::calibrate(
for(std::map<int, SensorData>::iterator iter=sequence.begin(); iter!=sequence.end(); ++iter)
{
Transform baseToCamera = iter->second.cameraModels()[0].localTransform();
viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first), baseToCamera, 0.2);
viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first), baseToCamera, 0.2, QColor(), iter->second.cameraModels()[0].fovX(), iter->second.cameraModels()[0].fovY());
}
_progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)... done.").arg(map->size()).arg(sequence.size()));

9
guilib/src/MainWindow.cpp
View File

@@ -1274,7 +1274,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
if(!t.isNull())
{
QColor color = Qt::yellow;
_cloudViewer->addOrUpdateFrustum(frustumId, _odometryCorrection*iter->second, t, _cloudViewer->getFrustumScale(), color);
_cloudViewer->addOrUpdateFrustum(frustumId, _odometryCorrection*iter->second, t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
}
}
}
@@ -2646,17 +2646,18 @@ void MainWindow::updateMapCloud(
// Supporting only one frustum per node
if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModel().isValidForProjection())
{
Transform t = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().localTransform():s.sensorData().cameraModels()[0].localTransform();
const CameraModel & model = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().left():s.sensorData().cameraModels()[0];
Transform t = model.localTransform();
if(!t.isNull())
{
QColor color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
_cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color);
_cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
if(_currentGTPosesMap.find(iter->first)!=_currentGTPosesMap.end())
{
std::string gtFrustumId = uFormat("f_gt_%d", iter->first);
color = Qt::gray;
_cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color);
_cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
}
}
}