From 5439d0af465d5f8e74665a8b664b5b26e0d0d7a3 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sun, 28 Jun 2015 12:51:40 -0400
Subject: [PATCH] fixed camera flickers when moving the camera over Z-axis

---
 guilib/include/rtabmap/gui/CloudViewer.h |  4 +-
 guilib/src/CloudViewer.cpp               | 48 +++++++++++++++++++-----
 2 files changed, 41 insertions(+), 11 deletions(-)

diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h
index 7922e088..34379fc9 100644
--- a/guilib/include/rtabmap/gui/CloudViewer.h
+++ b/guilib/include/rtabmap/gui/CloudViewer.h
@@ -201,13 +201,13 @@ protected:
 	virtual void keyPressEvent(QKeyEvent * event);
 	virtual void mousePressEvent(QMouseEvent * event);
 	virtual void mouseMoveEvent(QMouseEvent * event);
+	virtual void wheelEvent(QWheelEvent * event);
 	virtual void contextMenuEvent(QContextMenuEvent * event);
 	virtual void handleAction(QAction * event);
 	QMenu * menu() {return _menu;}
 
 private:
 	void createMenu();
-	void mouseEventOccurred (const pcl::visualization::MouseEvent &event, void* viewer_void);
 	void addGrid();
 	void removeGrid();
 
@@ -230,6 +230,8 @@ private:
     unsigned int _maxTrajectorySize;
     unsigned int _gridCellCount;
     float _gridCellSize;
+    cv::Vec3d _lastCameraOrientation;
+    cv::Vec3d _lastCameraPose;
     QMap<std::string, Transform> _addedClouds; // include cloud, scan, meshes
     Transform _lastPose;
     std::list<std::string> _gridLines;
diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp
index 09734748..82aa4072 100644
--- a/guilib/src/CloudViewer.cpp
+++ b/guilib/src/CloudViewer.cpp
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/core/util3d.h>
 #include <pcl/visualization/pcl_visualizer.h>
@@ -47,15 +48,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-void CloudViewer::mouseEventOccurred (const pcl::visualization::MouseEvent &event, void* viewer_void)
-{
-	if (event.getButton () == pcl::visualization::MouseEvent::LeftButton ||
-	  event.getButton () == pcl::visualization::MouseEvent::MiddleButton)
-	{
-		this->update(); // this will apply frustum
-	}
-}
-
 CloudViewer::CloudViewer(QWidget *parent) :
 		QVTKWidget(parent),
 		_visualizer(new pcl::visualization::PCLVisualizer("PCLVisualizer", false)),
@@ -75,6 +67,8 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_maxTrajectorySize(100),
 		_gridCellCount(50),
 		_gridCellSize(1),
+		_lastCameraOrientation(0,0,0),
+		_lastCameraPose(0,0,0),
 		_workingDirectory("."),
 		_defaultBgColor(Qt::black),
 		_currentBgColor(Qt::black)
@@ -88,7 +82,6 @@ CloudViewer::CloudViewer(QWidget *parent) :
 	//_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow());
 	this->GetInteractor()->SetInteractorStyle (_visualizer->getInteractorStyle());
 
-	_visualizer->registerMouseCallback (&CloudViewer::mouseEventOccurred, *this, (void*)_visualizer);
 	_visualizer->setCameraPosition(
 				-1, 0, 0,
 				0, 0, 0,
@@ -681,6 +674,7 @@ void CloudViewer::setCameraPosition(
 		float focalX, float focalY, float focalZ,
 		float upX, float upY, float upZ)
 {
+	_lastCameraOrientation= _lastCameraPose= cv::Vec3f(0,0,0);
 	_visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ);
 }
 
@@ -891,6 +885,7 @@ void CloudViewer::setCameraFree()
 
 void CloudViewer::setCameraLockZ(bool enabled)
 {
+	_lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
 	_aLockViewZ->setChecked(enabled);
 }
 
@@ -1168,12 +1163,31 @@ void CloudViewer::mousePressEvent(QMouseEvent * event)
 void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 {
 	QVTKWidget::mouseMoveEvent(event);
+
 	// camera view up z locked?
 	if(_aLockViewZ->isChecked())
 	{
 		std::vector<pcl::visualization::Camera> cameras;
 		_visualizer->getCameras(cameras);
 
+		cv::Vec3d newCameraOrientation = cv::Vec3d(0,0,1).cross(cv::Vec3d(cameras.front().pos)-cv::Vec3d(cameras.front().focal));
+		double norm = cv::norm(cv::Vec3d(cameras.front().pos)-cv::Vec3d(cameras.front().focal));
+
+		if(	_lastCameraOrientation!=cv::Vec3d(0,0,0) &&
+			_lastCameraPose!=cv::Vec3d(0,0,0) &&
+			(  (uSign(_lastCameraOrientation[0]) != uSign(newCameraOrientation[0]) &&
+			    uSign(_lastCameraOrientation[1]) != uSign(newCameraOrientation[1])   ) ||
+			   (norm && fabs(cameras.front().pos[2]-cameras.front().focal[2])/norm > 0.9999)))
+		{
+			cameras.front().pos[0] = _lastCameraPose[0];
+			cameras.front().pos[1] = _lastCameraPose[1];
+			cameras.front().pos[2] = _lastCameraPose[2];
+		}
+		else if(newCameraOrientation != cv::Vec3d(0,0,0))
+		{
+			_lastCameraOrientation = newCameraOrientation;
+			_lastCameraPose = cv::Vec3d(cameras.front().pos);
+		}
 		cameras.front().view[0] = 0;
 		cameras.front().view[1] = 0;
 		cameras.front().view[2] = 1;
@@ -1184,6 +1198,19 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
 
 	}
+
+	emit configChanged();
+}
+
+void CloudViewer::wheelEvent(QWheelEvent * event)
+{
+	QVTKWidget::wheelEvent(event);
+	if(_aLockViewZ->isChecked())
+	{
+		std::vector<pcl::visualization::Camera> cameras;
+		_visualizer->getCameras(cameras);
+		_lastCameraPose = cv::Vec3d(cameras.front().pos);
+	}
 	emit configChanged();
 }
 
@@ -1214,6 +1241,7 @@ void CloudViewer::handleAction(QAction * a)
 	}
 	else if(a == _aResetCamera)
 	{
+		_lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
 		if((_aFollowCamera->isChecked() || _aLockCamera->isChecked()) && !_lastPose.isNull())
 		{
 			// reset relative to last current pose