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

CloudViewer: Added updateCameraFrustum() method (multi-cameras supported)

Browse Source
This commit is contained in:
matlabbe
2016-06-19 18:33:36 -04:00
parent ab991c2a7d
commit 62db5370aa
3 changed files with 94 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
guilib/include/rtabmap/gui/CloudViewer.h
+17 -3
View File
@@ -30,13 +30,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
#include "rtabmap/core/Transform.h"
#include "rtabmap/core/StereoCameraModel.h"
#include <QVTKWidget.h>
#include <pcl/pcl_base.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/PolygonMesh.h>
#include <pcl/TextureMesh.h>
#include "rtabmap/core/Transform.h"
#include <QtCore/QMap>
#include <QtCore/QSet>
#include <QtCore/qnamespace.h>
@@ -142,8 +145,19 @@ public:
void removeOccupancyGridMap();
void updateCameraTargetPosition(
const Transform & pose,
const Transform & localTransform = Transform::getIdentity());
const Transform & pose);
void updateCameraFrustum(
const Transform & pose,
const StereoCameraModel & model);
void updateCameraFrustum(
const Transform & pose,
const CameraModel & model);
void updateCameraFrustums(
const Transform & pose,
const std::vector<CameraModel> & models);
void addOrUpdateCoordinate(
const std::string & id,
guilib/src/CloudViewer.cpp
+68 -29
View File
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>
#include <QMenu>
@@ -178,10 +179,6 @@ void CloudViewer::clear()
this->clearTrajectory();
this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
if(_aShowFrustum->isChecked())
{
this->addOrUpdateFrustum("reference_frustum", Transform::getIdentity(), _frustumScale, _frustumColor);
}
}
void CloudViewer::createMenu()
@@ -1119,8 +1116,22 @@ void CloudViewer::setFrustumShown(bool shown)
{
if(!shown)
{
this->removeFrustum("reference_frustum");
this->removeLine("reference_frustum_line");
std::set<std::string> frustumsCopy = _frustums;
for(std::set<std::string>::iterator iter=frustumsCopy.begin(); iter!=frustumsCopy.end(); ++iter)
{
if(uStrContains(*iter, "reference_frustum"))
{
this->removeFrustum(*iter);
}
}
std::set<std::string> linesCopy = _lines;
for(std::set<std::string>::iterator iter=linesCopy.begin(); iter!=linesCopy.end(); ++iter)
{
if(uStrContains(*iter, "reference_frustum_line"))
{
this->removeLine(*iter);
}
}
this->update();
}
_aShowFrustum->setChecked(shown);
@@ -1137,9 +1148,12 @@ void CloudViewer::setFrustumColor(QColor value)
{
value = Qt::gray;
}
if(_frustums.find("reference_frustum") != _frustums.end())
for(std::set<std::string>::iterator iter=_frustums.begin(); iter!=_frustums.end(); ++iter)
{
_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, value.redF(), value.greenF(), value.blueF(), "reference_frustum");
if(uStrContains(*iter, "reference_frustum"))
{
_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, value.redF(), value.greenF(), value.blueF(), *iter);
}
}
this->update();
_frustumColor = value;
@@ -1255,7 +1269,7 @@ void CloudViewer::setCameraPosition(
_visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ);
}
void CloudViewer::updateCameraTargetPosition(const Transform & pose, const Transform & localTransform)
void CloudViewer::updateCameraTargetPosition(const Transform & pose)
{
if(!pose.isNull())
{
@@ -1364,26 +1378,6 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose, const Trans
this->addOrUpdateCoordinate("reference", pose, 0.2);
}
// commented: update pose is crashing...
/*if(_frustums.find("reference_frustum") != _frustums.end())
{
this->updateFrustumPose("reference_frustum", pose);
}
else */ if(_aShowFrustum->isChecked())
{
Transform baseToCamera = Transform::getIdentity();
Transform opticalRot(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
if(!localTransform.isNull() && !localTransform.isIdentity())
{
baseToCamera = localTransform*opticalRot.inverse();
}
this->addOrUpdateFrustum("reference_frustum", pose * baseToCamera, _frustumScale, _frustumColor);
if(!baseToCamera.isIdentity())
{
this->addOrUpdateLine("reference_frustum_line", pose, pose * baseToCamera, _frustumColor);
}
}
vtkRenderer* renderer = _visualizer->getRendererCollection()->GetFirstRenderer();
vtkSmartPointer<vtkCamera> cam = renderer->GetActiveCamera ();
cam->SetPosition (cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2]);
@@ -1396,6 +1390,51 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose, const Trans
_lastPose = pose;
}
void CloudViewer::updateCameraFrustum(const Transform & pose, const StereoCameraModel & model)
{
std::vector<CameraModel> models;
models.push_back(model.left());
updateCameraFrustums(pose, models);
}
void CloudViewer::updateCameraFrustum(const Transform & pose, const CameraModel & model)
{
std::vector<CameraModel> models;
models.push_back(model);
updateCameraFrustums(pose, models);
}
void CloudViewer::updateCameraFrustums(const Transform & pose, const std::vector<CameraModel> & models)
{
if(!pose.isNull())
{
// commented: update pose is crashing...
/*if(_frustums.find("reference_frustum") != _frustums.end())
{
this->updateFrustumPose("reference_frustum", pose);
}
else */ if(_aShowFrustum->isChecked())
{
Transform baseToCamera;
Transform opticalRot(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
for(unsigned int i=0; i<models.size(); ++i)
{
baseToCamera = Transform::getIdentity();
if(!models[i].localTransform().isNull() && !models[i].localTransform().isIdentity())
{
baseToCamera = models[i].localTransform()*opticalRot.inverse();
}
this->addOrUpdateFrustum(uFormat("reference_frustum_%d", i), pose * baseToCamera, _frustumScale, _frustumColor);
if(!baseToCamera.isIdentity())
{
this->addOrUpdateLine(uFormat("reference_frustum_line_%d", i), pose, pose * baseToCamera, _frustumColor);
}
}
}
}
}
const QColor & CloudViewer::getDefaultBackgroundColor() const
{
return _defaultBgColor;
guilib/src/MainWindow.cpp
+9 -7
View File
@@ -1063,17 +1063,17 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
if(!odom.pose().isNull())
{
// update camera position
Transform localTransform;
_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
{
localTransform = odom.data().cameraModels()[0].localTransform();
_cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), odom.data().cameraModels());
}
else if(!odom.data().stereoCameraModel().localTransform().isNull())
{
localTransform = odom.data().stereoCameraModel().localTransform();
_cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), odom.data().stereoCameraModel());
}
_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose(), localTransform);
}
_cloudViewer->update();
@@ -1579,20 +1579,22 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
if(!_odometryReceived && poses.size())
{
_cloudViewer->updateCameraTargetPosition(poses.rbegin()->second);
Transform localTransform = Transform::getIdentity();
std::map<int, Signature>::const_iterator iter = stat.getSignatures().find(poses.rbegin()->first);
if(iter != stat.getSignatures().end())
{
if(iter->second.sensorData().cameraModels().size() && !iter->second.sensorData().cameraModels()[0].localTransform().isNull())
{
localTransform = iter->second.sensorData().cameraModels()[0].localTransform();
_cloudViewer->updateCameraFrustums(poses.rbegin()->second, iter->second.sensorData().cameraModels());
}
else if(!iter->second.sensorData().stereoCameraModel().localTransform().isNull())
{
localTransform = iter->second.sensorData().stereoCameraModel().localTransform();
_cloudViewer->updateCameraFrustum(poses.rbegin()->second, iter->second.sensorData().stereoCameraModel());
}
}
_cloudViewer->updateCameraTargetPosition(poses.rbegin()->second, localTransform);
if(_ui->graphicsView_graphView->isVisible())
{
_ui->graphicsView_graphView->updateReferentialPosition(poses.rbegin()->second);