diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index 6dc093dd..f8c13913 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -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 #include #include #include #include #include -#include "rtabmap/core/Transform.h" + #include #include #include @@ -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 & models); void addOrUpdateCoordinate( const std::string & id, diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 1ca95c66..7652dc6a 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -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 frustumsCopy = _frustums; + for(std::set::iterator iter=frustumsCopy.begin(); iter!=frustumsCopy.end(); ++iter) + { + if(uStrContains(*iter, "reference_frustum")) + { + this->removeFrustum(*iter); + } + } + std::set linesCopy = _lines; + for(std::set::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::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 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 models; + models.push_back(model.left()); + updateCameraFrustums(pose, models); +} + +void CloudViewer::updateCameraFrustum(const Transform & pose, const CameraModel & model) +{ + std::vector models; + models.push_back(model); + updateCameraFrustums(pose, models); +} + +void CloudViewer::updateCameraFrustums(const Transform & pose, const std::vector & 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; iaddOrUpdateFrustum(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; diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 96d558a2..528ae2ff 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -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::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);