diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index 479953ea..48da9100 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -142,7 +142,8 @@ public: void removeOccupancyGridMap(); void updateCameraTargetPosition( - const Transform & pose); + const Transform & pose, + const Transform & localTransform = Transform::getIdentity()); void addOrUpdateCoordinate( const std::string & id, @@ -154,6 +155,14 @@ public: void removeCoordinate(const std::string & id); void removeAllCoordinates(); + void addOrUpdateArrow( + const std::string & id, + const Transform & from, + const Transform & to, + const QColor & color); + void removeArrow(const std::string & id); + void removeAllArrows(); + void addOrUpdateFrustum( const std::string & id, const Transform & transform, @@ -282,6 +291,7 @@ private: std::set _graphes; std::set _coordinates; std::set _texts; + std::set _arrows; std::set _frustums; pcl::PointCloud::Ptr _trajectory; unsigned int _maxTrajectorySize; diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 2c333c16..737176df 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -229,7 +229,6 @@ private: void update3DMapVisibility(bool cloudsShown, bool scansShown); void updateMapCloud( const std::map & poses, - const Transform & pose, const std::multimap & constraints, const std::map & mapIds, const std::map & labels, diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 352438ec..ee4b7536 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -172,6 +172,7 @@ void CloudViewer::clear() this->removeAllClouds(); this->removeAllGraphs(); this->removeAllCoordinates(); + this->removeAllArrows(); this->removeAllFrustums(); this->removeAllTexts(); this->clearTrajectory(); @@ -792,6 +793,62 @@ void CloudViewer::removeAllCoordinates() UASSERT(_coordinates.empty()); } +void CloudViewer::addOrUpdateArrow( + const std::string & id, + const Transform & from, + const Transform & to, + const QColor & color) +{ + if(id.empty()) + { + UERROR("id should not be empty!"); + return; + } + + removeArrow(id); + + if(!from.isNull() && !to.isNull()) + { + _arrows.insert(id); + + QColor c = Qt::gray; + if(color.isValid()) + { + c = color; + } + + pcl::PointXYZ pt1(from.x(), from.y(), from.z()); + pcl::PointXYZ pt2(to.x(), to.y(), to.z()); + + _visualizer->addArrow(pt2, pt1, c.redF(), c.greenF(), c.blueF(), false, id); + } +} + +void CloudViewer::removeArrow(const std::string & id) +{ + if(id.empty()) + { + UERROR("id should not be empty!"); + return; + } + + if(_arrows.find(id) != _arrows.end()) + { + _visualizer->removeShape(id); + _arrows.erase(id); + } +} + +void CloudViewer::removeAllArrows() +{ + std::set arrows = _arrows; + for(std::set::iterator iter = arrows.begin(); iter!=arrows.end(); ++iter) + { + this->removeArrow(*iter); + } + UASSERT(_arrows.empty()); +} + static const float frustum_vertices[] = { 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, @@ -1055,6 +1112,7 @@ void CloudViewer::setFrustumShown(bool shown) if(!shown) { this->removeFrustum("reference_frustum"); + this->removeArrow("reference_frustum_arrow"); this->update(); } _aShowFrustum->setChecked(shown); @@ -1074,8 +1132,12 @@ void CloudViewer::setFrustumColor(QColor value) if(_frustums.find("reference_frustum") != _frustums.end()) { _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, value.redF(), value.greenF(), value.blueF(), "reference_frustum"); - this->update(); } + if(_arrows.find("reference_frustum_arrow") != _arrows.end()) + { + _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, value.redF(), value.greenF(), value.blueF(), "reference_frustum_arrow"); + } + this->update(); _frustumColor = value; } @@ -1189,7 +1251,7 @@ void CloudViewer::setCameraPosition( _visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ); } -void CloudViewer::updateCameraTargetPosition(const Transform & pose) +void CloudViewer::updateCameraTargetPosition(const Transform & pose, const Transform & localTransform) { if(!pose.isNull()) { @@ -1305,7 +1367,17 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose) } else */ if(_aShowFrustum->isChecked()) { - this->addOrUpdateFrustum("reference_frustum", pose, _frustumScale, _frustumColor); + 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->addOrUpdateArrow("reference_frustum_arrow", pose, pose * baseToCamera, _frustumColor); + } } vtkRenderer* renderer = _visualizer->getRendererCollection()->GetFirstRenderer(); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 023966fd..da2fbadc 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -775,6 +775,8 @@ void MainWindow::handleEvent(UEvent* anEvent) { // we receive too many odometry events! just send without data SensorData data(cv::Mat(), odomEvent->data().id(), odomEvent->data().stamp()); + data.setCameraModels(odomEvent->data().cameraModels()); + data.setStereoCameraModel(odomEvent->data().stereoCameraModel()); data.setGroundTruth(odomEvent->data().groundTruth()); OdometryEvent tmp(data, odomEvent->pose(), odomEvent->covariance(), odomEvent->info().copyWithoutData()); emit odometryReceived(tmp); @@ -1036,7 +1038,17 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom) if(!odom.pose().isNull()) { // update camera position - _cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose()); + Transform localTransform; + if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull()) + { + localTransform = odom.data().cameraModels()[0].localTransform(); + } + else if(!odom.data().stereoCameraModel().localTransform().isNull()) + { + localTransform = odom.data().stereoCameraModel().localTransform(); + } + + _cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose(), localTransform); } _cloudViewer->update(); @@ -1527,9 +1539,30 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) Transform groundTruthOffset = alignPosesToGroundTruth(poses, groundTruth); UDEBUG("time= %d ms", time.restart()); + if(!_odometryReceived && poses.size()) + { + 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(); + } + else if(!iter->second.sensorData().stereoCameraModel().localTransform().isNull()) + { + localTransform = iter->second.sensorData().stereoCameraModel().localTransform(); + } + } + _cloudViewer->updateCameraTargetPosition(poses.rbegin()->second, localTransform); + if(_ui->graphicsView_graphView->isVisible()) + { + _ui->graphicsView_graphView->updateReferentialPosition(poses.rbegin()->second); + } + } + updateMapCloud( poses, - _odometryReceived||poses.size()==0?Transform():poses.rbegin()->second, stat.constraints(), mapIds, labels, @@ -1725,15 +1758,14 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) void MainWindow::updateMapCloud( const std::map & posesIn, - const Transform & currentPose, const std::multimap & constraints, const std::map & mapIdsIn, const std::map & labels, const std::map & groundTruths, // ground truth should contain only valid transforms bool verboseProgress) { - UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d labelsIn=%d currentPose=%s", - (int)posesIn.size(), (int)constraints.size(), (int)mapIdsIn.size(), (int)labels.size(), currentPose.prettyPrint().c_str()); + UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d labelsIn=%d", + (int)posesIn.size(), (int)constraints.size(), (int)mapIdsIn.size(), (int)labels.size()); if(posesIn.size()) { _currentPosesMap = posesIn; @@ -1810,7 +1842,7 @@ void MainWindow::updateMapCloud( } // Map updated! regenerate the assembled cloud, last pose is the new one - UDEBUG("Update map with %d locations (currentPose=%s)", poses.size(), currentPose.prettyPrint().c_str()); + UDEBUG("Update map with %d locations", poses.size()); QMap viewerClouds = _cloudViewer->getAddedClouds(); int i=1; for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) @@ -2040,11 +2072,6 @@ void MainWindow::updateMapCloud( if(_ui->graphicsView_graphView->isVisible()) { _ui->graphicsView_graphView->updateGraph(posesIn, constraints, mapIdsIn); - if(!currentPose.isNull()) - { - _ui->graphicsView_graphView->updateReferentialPosition(currentPose); - } - _ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap); } cv::Mat map8U; @@ -2147,12 +2174,6 @@ void MainWindow::updateMapCloud( } } - if(!currentPose.isNull()) - { - UDEBUG(""); - _cloudViewer->updateCameraTargetPosition(currentPose); - } - UDEBUG(""); _cloudViewer->update(); UDEBUG(""); @@ -2813,7 +2834,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve QApplication::processEvents(); std::map poses = event.getPoses(); alignPosesToGroundTruth(poses, groundTruth); - this->updateMapCloud(poses, Transform(), event.getConstraints(), mapIds, labels, groundTruth, true); + this->updateMapCloud(poses, event.getConstraints(), mapIds, labels, groundTruth, true); _initProgressDialog->appendText("Updating the 3D map cloud... done."); } else @@ -2957,7 +2978,6 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags) { this->updateMapCloud( std::map(_currentPosesMap), - Transform(), std::multimap(_currentLinksMap), std::map(_currentMapIds), std::map(_currentLabels), @@ -4289,7 +4309,6 @@ void MainWindow::postProcessing() alignPosesToGroundTruth(optimizedPoses, _currentGTPosesMap); this->updateMapCloud( optimizedPoses, - Transform(), std::multimap(_currentLinksMap), std::map(_currentMapIds), std::map(_currentLabels), @@ -4652,7 +4671,6 @@ void MainWindow::anchorCloudsToGroundTruth() { this->updateMapCloud( std::map(_currentPosesMap), - Transform(), std::multimap(_currentLinksMap), std::map(_currentMapIds), std::map(_currentLabels),