CloudViewer: Added local transformation between base frame and camera frame (when they are not the same)

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<std::string> _graphes;
     std::set<std::string> _coordinates;
     std::set<std::string> _texts;
+    std::set<std::string> _arrows;
     std::set<std::string> _frustums;
     pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
     unsigned int _maxTrajectorySize;

guilib/include/rtabmap/gui/MainWindow.h

@@ -229,7 +229,6 @@ private:
 	void update3DMapVisibility(bool cloudsShown, bool scansShown);
 	void updateMapCloud(
 			const std::map<int, Transform> & poses,
-			const Transform & pose,
 			const std::multimap<int, Link> & constraints,
 			const std::map<int, int> & mapIds,
 			const std::map<int, std::string> & labels,

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<std::string> arrows = _arrows;
+	for(std::set<std::string>::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();

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<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();
+					}
+					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<int, Transform> & posesIn,
-		const Transform & currentPose,
 		const std::multimap<int, Link> & constraints,
 		const std::map<int, int> & mapIdsIn,
 		const std::map<int, std::string> & labels,
 		const std::map<int, Transform> & groundTruths, // ground truth should contain only valid transforms
 		bool verboseProgress)
 {
-	UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d labelsIn=%d currentPose=%s",
+	UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d labelsIn=%d",
-			(int)posesIn.size(), (int)constraints.size(), (int)mapIdsIn.size(), (int)labels.size(), currentPose.prettyPrint().c_str());
+			(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<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
 	int i=1;
 	for(std::map<int, Transform>::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<int, Transform> 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<int, Transform>(_currentPosesMap),
-					Transform(),
 					std::multimap<int, Link>(_currentLinksMap),
 					std::map<int, int>(_currentMapIds),
 					std::map<int, std::string>(_currentLabels),
@@ -4289,7 +4309,6 @@ void MainWindow::postProcessing()
 	alignPosesToGroundTruth(optimizedPoses, _currentGTPosesMap);
 	this->updateMapCloud(
 			optimizedPoses,
-			Transform(),
 			std::multimap<int, Link>(_currentLinksMap),
 			std::map<int, int>(_currentMapIds),
 			std::map<int, std::string>(_currentLabels),
@@ -4652,7 +4671,6 @@ void MainWindow::anchorCloudsToGroundTruth()
 {
 	this->updateMapCloud(
 			std::map<int, Transform>(_currentPosesMap),
-			Transform(),
 			std::multimap<int, Link>(_currentLinksMap),
 			std::map<int, int>(_currentMapIds),
 			std::map<int, std::string>(_currentLabels),