CloudViewer: added camera Ortho mode context menu option

guilib/include/rtabmap/gui/CloudViewer.h

@@ -329,6 +329,7 @@ public:
 	bool isCameraTargetFollow() const;
 	bool isCameraFree() const;
 	bool isCameraLockZ() const;
+	bool isCameraOrtho() const;
 	bool isGridShown() const;
 	unsigned int getGridCellCount() const;
 	float getGridCellSize() const;
@@ -341,6 +342,7 @@ public:
 	void setCameraTargetFollow(bool enabled = true);
 	void setCameraFree();
 	void setCameraLockZ(bool enabled = true);
+	void setCameraOrtho(bool enabled = true);
 	void setGridShown(bool shown);
 	void setNormalsShown(bool shown);
 	void setGridCellCount(unsigned int count);
@@ -387,6 +389,7 @@ private:
     QAction * _aFollowCamera;
     QAction * _aResetCamera;
     QAction * _aLockViewZ;
+    QAction * _aCameraOrtho;
     QAction * _aShowTrajectory;
     QAction * _aSetTrajectorySize;
     QAction * _aClearTrajectory;

guilib/include/rtabmap/gui/CloudViewerInteractorStyle.h

@@ -28,6 +28,7 @@ public:
 public:
 	CloudViewerInteractorStyle();
 	virtual void Rotate();
+	void setOrthoMode(bool enabled);
 protected:
 	virtual void OnMouseMove();
 	virtual void OnLeftButtonDown();
@@ -43,6 +44,7 @@ private:
 	int ResetPixelDistance;
 	float PreviousMeasure[3];
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointsHolder_;
+	bool orthoMode_;
 };
 
 } /* namespace rtabmap */

guilib/src/CloudViewer.cpp

@@ -85,6 +85,7 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 		_aFollowCamera(0),
 		_aResetCamera(0),
 		_aLockViewZ(0),
+		_aCameraOrtho(0),
 		_aShowTrajectory(0),
 		_aSetTrajectorySize(0),
 		_aClearTrajectory(0),
@@ -224,6 +225,9 @@ void CloudViewer::createMenu()
 	_aLockViewZ = new QAction("Lock view Z", this);
 	_aLockViewZ->setCheckable(true);
 	_aLockViewZ->setChecked(true);
+	_aCameraOrtho = new QAction("Ortho mode", this);
+	_aCameraOrtho->setCheckable(true);
+	_aCameraOrtho->setChecked(false);
 	_aResetCamera = new QAction("Reset position", this);
 	_aShowTrajectory= new QAction("Show trajectory", this);
 	_aShowTrajectory->setCheckable(true);
@@ -267,6 +271,7 @@ void CloudViewer::createMenu()
 	cameraMenu->addAction(freeCamera);
 	cameraMenu->addSeparator();
 	cameraMenu->addAction(_aLockViewZ);
+	cameraMenu->addAction(_aCameraOrtho);
 	cameraMenu->addAction(_aResetCamera);
 	QActionGroup * group = new QActionGroup(this);
 	group->addAction(_aLockCamera);
@@ -358,6 +363,7 @@ void CloudViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("camera_target_follow", this->isCameraTargetFollow());
 	settings.setValue("camera_free", this->isCameraFree());
 	settings.setValue("camera_lockZ", this->isCameraLockZ());
+	settings.setValue("camera_ortho", this->isCameraOrtho());
 
 	settings.setValue("bg_color", this->getDefaultBackgroundColor());
 	settings.setValue("rendering_rate", this->getRenderingRate());
@@ -404,6 +410,7 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group)
 		this->setCameraFree();
 	}
 	this->setCameraLockZ(settings.value("camera_lockZ", this->isCameraLockZ()).toBool());
+	this->setCameraOrtho(settings.value("camera_ortho", this->isCameraOrtho()).toBool());
 
 	this->setDefaultBackgroundColor(settings.value("bg_color", this->getDefaultBackgroundColor()).value<QColor>());
 	
@@ -2025,7 +2032,14 @@ void CloudViewer::resetCamera()
 	{
 		// reset relative to last current pose
 		cv::Point3f pt = util3d::transformPoint(cv::Point3f(_lastPose.x(), _lastPose.y(), _lastPose.z()), ( _lastPose.rotation()*Transform(-1, 0, 0)).translation());
-		if(_aLockViewZ->isChecked())
+		if(_aCameraOrtho->isChecked())
+		{
+			_visualizer->setCameraPosition(
+					_lastPose.x(), _lastPose.y(), _lastPose.z()+5,
+					_lastPose.x(), _lastPose.y(), _lastPose.z(),
+					1, 0, 0, 1);
+		}
+		else if(_aLockViewZ->isChecked())
 		{
 			_visualizer->setCameraPosition(
 					pt.x, pt.y, pt.z,
@@ -2040,6 +2054,13 @@ void CloudViewer::resetCamera()
 					_lastPose.r31(), _lastPose.r32(), _lastPose.r33(), 1);
 		}
 	}
+	else if(_aCameraOrtho->isChecked())
+	{
+		_visualizer->setCameraPosition(
+				0, 0, 5,
+				0, 0, 0,
+				1, 0, 0, 1);
+	}
 	else
 	{
 		_visualizer->setCameraPosition(
@@ -2371,7 +2392,7 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 			std::vector<pcl::visualization::Camera> cameras;
 			_visualizer->getCameras(cameras);
 
-			if(_aLockCamera->isChecked())
+			if(_aLockCamera->isChecked() || _aCameraOrtho->isChecked())
 			{
 				//update camera position
 				Eigen::Vector3f diff = pos - Eigen::Vector3f(_lastPose.x(), _lastPose.y(), _lastPose.z());
@@ -2599,6 +2620,17 @@ void CloudViewer::setCameraLockZ(bool enabled)
 	_lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
 	_aLockViewZ->setChecked(enabled);
 }
+void CloudViewer::setCameraOrtho(bool enabled)
+{
+	_lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
+	CloudViewerInteractorStyle * interactor = CloudViewerInteractorStyle::SafeDownCast(this->GetInteractor()->GetInteractorStyle());
+	if(interactor)
+	{
+		interactor->setOrthoMode(enabled);
+		this->update();
+	}
+	_aCameraOrtho->setChecked(enabled);
+}
 bool CloudViewer::isCameraTargetLocked() const
 {
 	return _aLockCamera->isChecked();
@@ -2615,6 +2647,10 @@ bool CloudViewer::isCameraLockZ() const
 {
 	return _aLockViewZ->isChecked();
 }
+bool CloudViewer::isCameraOrtho() const
+{
+	return _aCameraOrtho->isChecked();
+}
 double CloudViewer::getRenderingRate() const
 {
 	return _renderingRate;
@@ -2934,7 +2970,7 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 	QVTKWidget::mouseMoveEvent(event);
 
 	// camera view up z locked?
-	if(_aLockViewZ->isChecked())
+	if(_aLockViewZ->isChecked() && !_aCameraOrtho->isChecked())
 	{
 		std::vector<pcl::visualization::Camera> cameras;
 		_visualizer->getCameras(cameras);
@@ -2955,6 +2991,18 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 			_lastCameraOrientation = newCameraOrientation;
 			_lastCameraPose = cv::Vec3d(cameras.front().pos);
 		}
+		else
+		{
+			if(cameras.front().view[2] == 0)
+			{
+				cameras.front().pos[0] -= 0.00001*cameras.front().view[0];
+				cameras.front().pos[1] -= 0.00001*cameras.front().view[1];
+			}
+			else
+			{
+				cameras.front().pos[0] -= 0.00001;
+			}
+		}
 		cameras.front().view[0] = 0;
 		cameras.front().view[1] = 0;
 		cameras.front().view[2] = 1;
@@ -2973,7 +3021,7 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 void CloudViewer::wheelEvent(QWheelEvent * event)
 {
 	QVTKWidget::wheelEvent(event);
-	if(_aLockViewZ->isChecked())
+	if(_aLockViewZ->isChecked() && !_aCameraOrtho->isChecked())
 	{
 		std::vector<pcl::visualization::Camera> cameras;
 		_visualizer->getCameras(cameras);
@@ -3112,6 +3160,10 @@ void CloudViewer::handleAction(QAction * a)
 			this->update();
 		}
 	}
+	else if(a == _aCameraOrtho)
+	{
+		this->setCameraOrtho(_aCameraOrtho->isChecked());
+	}
 	else if(a == _aSetLighting)
 	{
 		this->setLighting(_aSetLighting->isChecked());

guilib/src/CloudViewerInteractorStyle.cpp

@@ -11,6 +11,7 @@
 
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UMath.h"
 
 #include <vtkRenderer.h>
 #include <vtkRenderWindow.h>
@@ -28,7 +29,8 @@ CloudViewerInteractorStyle::CloudViewerInteractorStyle() :
 	viewer_(0),
 	NumberOfClicks(0),
 	ResetPixelDistance(0),
-	pointsHolder_(new pcl::PointCloud<pcl::PointXYZRGB>)
+	pointsHolder_(new pcl::PointCloud<pcl::PointXYZRGB>),
+	orthoMode_(false)
 {
 	PreviousPosition[0] = PreviousPosition[1] = 0;
 	PreviousMeasure[0] = PreviousMeasure[1] =  PreviousMeasure[2] = 0.0f;
@@ -46,7 +48,7 @@ void CloudViewerInteractorStyle::Rotate()
 	vtkRenderWindowInteractor *rwi = this->Interactor;
 
 	int dx = rwi->GetEventPosition()[0] - rwi->GetLastEventPosition()[0];
-	int dy = rwi->GetEventPosition()[1] - rwi->GetLastEventPosition()[1];
+	int dy = orthoMode_?0:rwi->GetEventPosition()[1] - rwi->GetLastEventPosition()[1];
 
 	int *size = this->CurrentRenderer->GetRenderWindow()->GetSize();
 
@@ -58,9 +60,16 @@ void CloudViewerInteractorStyle::Rotate()
 
 	vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
 	UASSERT(camera);
-	camera->Azimuth(rxf);
-	camera->Elevation(ryf);
-	camera->OrthogonalizeViewUp();
+	if(!orthoMode_)
+	{
+		camera->Azimuth(rxf);
+		camera->Elevation(ryf);
+		camera->OrthogonalizeViewUp();
+	}
+	else
+	{
+		camera->Roll(-rxf);
+	}
 
 	if (this->AutoAdjustCameraClippingRange)
 	{
@@ -75,6 +84,27 @@ void CloudViewerInteractorStyle::Rotate()
 	//rwi->Render();
 }
 
+void CloudViewerInteractorStyle::setOrthoMode(bool enabled)
+{
+	if (this->CurrentRenderer == NULL)
+	{
+		return;
+	}
+
+	vtkSmartPointer<vtkCamera> cam = CurrentRenderer->GetActiveCamera ();
+	UASSERT(cam.Get());
+	cam->SetParallelProjection (enabled);
+	if(enabled)
+	{
+		double x,y,z;
+		cam->GetFocalPoint(x, y, z);
+		cam->SetPosition(x, y, z+(cam->GetDistance()<=5?5:cam->GetDistance()));
+		cam->SetViewUp(1, 0, 0);
+	}
+	CurrentRenderer->SetActiveCamera (cam);
+	orthoMode_ = enabled;
+}
+
 void CloudViewerInteractorStyle::OnMouseMove()
 {
 	if(this->CurrentRenderer &&