saving CloudViewer settings (Camera,Trajectory,Grid) in the config file

2026-09-02 01:20:25 +08:00 · 2015-01-13 18:11:34 -05:00
parent 6bd2a0926b
commit fb0b23ae06
10 changed files with 379 additions and 27 deletions
--- a/guilib/src/CloudViewer.cpp
+++ b/guilib/src/CloudViewer.cpp
@@ -70,10 +70,14 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_aSetTrajectorySize(0),
 		_aClearTrajectory(0),
 		_aShowGrid(0),
+		_aSetGridCellCount(0),
+		_aSetGridCellSize(0),
 		_aSetBackgroundColor(0),
 		_menu(0),
 		_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
 		_maxTrajectorySize(100),
+		_gridCellCount(50),
+		_gridCellSize(1),
 		_workingDirectory("."),
 		_backgroundColor(Qt::black)
 {
@@ -125,6 +129,8 @@ void CloudViewer::createMenu()
 	_aClearTrajectory = new QAction("Clear trajectory", this);
 	_aShowGrid = new QAction("Show grid", this);
 	_aShowGrid->setCheckable(true);
+	_aSetGridCellCount = new QAction("Set cell count...", this);
+	_aSetGridCellSize = new QAction("Set cell size...", this);
 	_aSetBackgroundColor = new QAction("Set background color...", this);

 	QMenu * cameraMenu = new QMenu("Camera", this);
@@ -144,11 +150,16 @@ void CloudViewer::createMenu()
 	trajectoryMenu->addAction(_aSetTrajectorySize);
 	trajectoryMenu->addAction(_aClearTrajectory);

+	QMenu * gridMenu = new QMenu("Grid", this);
+	gridMenu->addAction(_aShowGrid);
+	gridMenu->addAction(_aSetGridCellCount);
+	gridMenu->addAction(_aSetGridCellSize);
+
 	//menus
 	_menu = new QMenu(this);
 	_menu->addMenu(cameraMenu);
 	_menu->addMenu(trajectoryMenu);
-	_menu->addAction(_aShowGrid);
+	_menu->addMenu(gridMenu);
 	_menu->addAction(_aSetBackgroundColor);
 }

@@ -480,12 +491,22 @@ void CloudViewer::removeAllGraphs()
 	_graphes.clear();
 }

+bool CloudViewer::isTrajectoryShown() const
+{
+	return _aShowTrajectory->isChecked();
+}
+
+int CloudViewer::getTrajectorySize() const
+{
+	return _maxTrajectorySize;
+}
+
 void CloudViewer::setTrajectoryShown(bool shown)
 {
 	_aShowTrajectory->setChecked(shown);
 }

-void CloudViewer::setTrajectorySize(int value)
+void CloudViewer::setTrajectorySize(unsigned int value)
 {
 	_maxTrajectorySize = value;
 }
@@ -494,7 +515,6 @@ void CloudViewer::clearTrajectory()
 {
 	_trajectory->clear();
 	_visualizer->removeShape("trajectory");
-	_lastPose.setNull();
 	this->render();
 }

@@ -521,7 +541,42 @@ bool CloudViewer::getPose(const std::string & id, Transform & pose)
 	return false;
 }

-void CloudViewer::updateCameraPosition(const Transform & pose)
+Transform CloudViewer::getTargetPose() const
+{
+	if(_lastPose.isNull())
+	{
+		return Transform::getIdentity();
+	}
+	return _lastPose;
+}
+
+void CloudViewer::getCameraPosition(
+		float & x, float & y, float & z,
+		float & focalX, float & focalY, float & focalZ,
+		float & upX, float & upY, float & upZ) const
+{
+	std::vector<pcl::visualization::Camera> cameras;
+	_visualizer->getCameras(cameras);
+	x = cameras.front().pos[0];
+	y = cameras.front().pos[1];
+	z = cameras.front().pos[2];
+	focalX = cameras.front().focal[0];
+	focalY = cameras.front().focal[1];
+	focalZ = cameras.front().focal[2];
+	upX = cameras.front().view[0];
+	upY = cameras.front().view[1];
+	upZ = cameras.front().view[2];
+}
+
+void CloudViewer::setCameraPosition(
+		float x, float y, float z,
+		float focalX, float focalY, float focalZ,
+		float upX, float upY, float upZ)
+{
+	_visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ);
+}
+
+void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 {
 	if(!pose.isNull())
 	{
@@ -752,12 +807,73 @@ void CloudViewer::setGridShown(bool shown)
 	}
 }

+bool CloudViewer::isCameraTargetLocked() const
+{
+	return _aLockCamera->isChecked();
+}
+bool CloudViewer::isCameraTargetFollow() const
+{
+	return _aFollowCamera->isChecked();
+}
+bool CloudViewer::isCameraFree() const
+{
+	return !_aFollowCamera->isChecked() && !_aLockCamera->isChecked();
+}
+bool CloudViewer::isCameraLockZ() const
+{
+	return _aLockViewZ->isChecked();
+}
+bool CloudViewer::isGridShown() const
+{
+	return _aShowGrid->isChecked();
+}
+unsigned int CloudViewer::getGridCellCount() const
+{
+	return _gridCellCount;
+}
+float CloudViewer::getGridCellSize() const
+{
+	return _gridCellSize;
+}
+
+void CloudViewer::setGridCellCount(unsigned int count)
+{
+	if(count > 0)
+	{
+		_gridCellCount = count;
+		if(_aShowGrid->isChecked())
+		{
+			this->removeGrid();
+			this->addGrid();
+		}
+	}
+	else
+	{
+		UERROR("Cannot set grid cell count < 1, count=%d", count);
+	}
+}
+void CloudViewer::setGridCellSize(float size)
+{
+	if(size > 0)
+	{
+		_gridCellSize = size;
+		if(_aShowGrid->isChecked())
+		{
+			this->removeGrid();
+			this->addGrid();
+		}
+	}
+	else
+	{
+		UERROR("Cannot set grid cell size <= 0, value=%f", size);
+	}
+}
 void CloudViewer::addGrid()
 {
 	if(_gridLines.empty())
 	{
-		float cellSize = 1.0f;
-		int cellCount = 50;
+		float cellSize = _gridCellSize;
+		int cellCount = _gridCellCount;
 		double r=0.5;
 		double g=0.5;
 		double b=0.5;
@@ -935,6 +1051,39 @@ void CloudViewer::keyPressEvent(QKeyEvent * event)
 	}
 }

+void CloudViewer::mousePressEvent(QMouseEvent * event)
+{
+	if(event->button() == Qt::RightButton)
+	{
+		event->accept();
+	}
+	else
+	{
+		QVTKWidget::mousePressEvent(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);
+
+		cameras.front().view[0] = 0;
+		cameras.front().view[1] = 0;
+		cameras.front().view[2] = 1;
+
+		_visualizer->setCameraPosition(
+			cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
+			cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
+			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
+
+	}
+}
+
 void CloudViewer::contextMenuEvent(QContextMenuEvent * event)
 {
 	QAction * a = _menu->exec(event->globalPos());
@@ -961,10 +1110,31 @@ void CloudViewer::handleAction(QAction * a)
 	}
 	else if(a == _aResetCamera)
 	{
-		_visualizer->setCameraPosition(
-				-1, 0, 0,
-				0, 0, 0,
-				0, 0, 1);
+		if((_aFollowCamera->isChecked() || _aLockCamera->isChecked()) && !_lastPose.isNull())
+		{
+			// reset relative to last current pose
+			if(_aLockViewZ->isChecked())
+			{
+				_visualizer->setCameraPosition(
+						_lastPose.x()-1, _lastPose.y(), _lastPose.z(),
+						_lastPose.x(), _lastPose.y(), _lastPose.z(),
+						0, 0, 1);
+			}
+			else
+			{
+				_visualizer->setCameraPosition(
+						_lastPose.x()-1, _lastPose.y(), _lastPose.z(),
+						_lastPose.x(), _lastPose.y(), _lastPose.z(),
+						_lastPose.r31(), _lastPose.r32(), _lastPose.r33());
+			}
+		}
+		else
+		{
+			_visualizer->setCameraPosition(
+					-1, 0, 0,
+					0, 0, 0,
+					0, 0, 1);
+		}
 		this->render();
 	}
 	else if(a == _aShowGrid)
@@ -980,12 +1150,37 @@ void CloudViewer::handleAction(QAction * a)

 		this->render();
 	}
+	else if(a == _aSetGridCellCount)
+	{
+		bool ok;
+		int value = QInputDialog::getInt(this, tr("Set grid cell count"), tr("Count"), _gridCellCount, 1, 10000, 10, &ok);
+		if(ok)
+		{
+			this->setGridCellCount(value);
+		}
+	}
+	else if(a == _aSetGridCellSize)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Set grid cell size"), tr("Size (m)"), _gridCellSize, 0.01, 10, 2, &ok);
+		if(ok)
+		{
+			this->setGridCellSize(value);
+		}
+	}
 	else if(a == _aSetBackgroundColor)
 	{
-		QColor color = Qt::black;
+		QColor color = this->getBackgroundColor();
 		color = QColorDialog::getColor(color, this);
 		this->setBackgroundColor(color);
 	}
+	else if(a == _aLockViewZ)
+	{
+		if(_aLockViewZ->isChecked())
+		{
+			this->render();
+		}
+	}
 }

 } /* namespace rtabmap */
--- a/guilib/src/DatabaseViewer.cpp
+++ b/guilib/src/DatabaseViewer.cpp
@@ -1240,7 +1240,7 @@ void DatabaseViewer::updateStereo(const Signature * data)
 		UINFO("correspondences = %d/%d (%f) (time kpt=%fs flow=%fs)",
 				(int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeFlow);

-		ui_->stereoViewer->updateCameraPosition(Transform::getIdentity());
+		ui_->stereoViewer->updateCameraTargetPosition(Transform::getIdentity());
 		ui_->stereoViewer->addOrUpdateCloud("stereo", cloud);
 		ui_->stereoViewer->render();

@@ -1655,7 +1655,7 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
 		}

 		//update cordinate
-		ui_->constraintsViewer->updateCameraPosition(t);
+		ui_->constraintsViewer->updateCameraTargetPosition(t);
 		ui_->constraintsViewer->clearTrajectory();

 		ui_->constraintsViewer->render();
--- a/guilib/src/ImageView.cpp
+++ b/guilib/src/ImageView.cpp
@@ -77,17 +77,17 @@ void ImageView::resetZoom()
 	this->setDragMode(QGraphicsView::NoDrag);
 }

-bool ImageView::isImageShown()
+bool ImageView::isImageShown() const
 {
 	return _showImage->isChecked();
 }

-bool ImageView::isImageDepthShown()
+bool ImageView::isImageDepthShown() const
 {
 	return _showImageDepth->isChecked();
 }

-bool ImageView::isFeaturesShown()
+bool ImageView::isFeaturesShown() const
 {
 	return _showFeatures->isChecked();
 }
@@ -121,7 +121,7 @@ void ImageView::setImageDepthShown(bool shown)
 	}
 }

-bool ImageView::isLinesShown()
+bool ImageView::isLinesShown() const
 {
 	return _showLines->isChecked();
 }
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -196,6 +196,9 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->imageView_source->setBackgroundBrush(QBrush(Qt::black));
 	_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
 	_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black));
+	_preferencesDialog->loadImageViewState(_ui->imageView_source->objectName(), _ui->imageView_source);
+	_preferencesDialog->loadImageViewState(_ui->imageView_loopClosure->objectName(), _ui->imageView_loopClosure);
+	_preferencesDialog->loadImageViewState(_ui->imageView_odometry->objectName(), _ui->imageView_odometry);

 	_posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
 	_ui->posteriorPlot->addCurve(_posteriorCurve, false);
@@ -370,6 +373,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
 	_ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
 	_ui->widget_cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
+	_preferencesDialog->loadCloudViewerState(_ui->widget_cloudViewer->objectName(), _ui->widget_cloudViewer);

 	if(_ui->statsToolBox->findChildren<StatItem*>().size() == 0)
 	{
@@ -448,6 +452,10 @@ void MainWindow::closeEvent(QCloseEvent* event)

 		//write settings before quit?
 		_preferencesDialog->saveMainWindowState(this);
+		_preferencesDialog->saveCloudViewerState(_ui->widget_cloudViewer->objectName(), _ui->widget_cloudViewer);
+		_preferencesDialog->saveImageViewState(_ui->imageView_source->objectName(), _ui->imageView_source);
+		_preferencesDialog->saveImageViewState(_ui->imageView_loopClosure->objectName(), _ui->imageView_loopClosure);
+		_preferencesDialog->saveImageViewState(_ui->imageView_odometry->objectName(), _ui->imageView_odometry);

 		_ui->dockWidget_imageView->close();
 		_ui->dockWidget_likelihood->close();
@@ -695,7 +703,7 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 			if(!data.pose().isNull())
 			{
 				// update camera position
-				_ui->widget_cloudViewer->updateCameraPosition(_odometryCorrection*data.pose());
+				_ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*data.pose());
 			}
 		}
 		_ui->widget_cloudViewer->render();
@@ -1359,7 +1367,7 @@ void MainWindow::updateMapCloud(

 	if(!currentPose.isNull())
 	{
-		_ui->widget_cloudViewer->updateCameraPosition(currentPose);
+		_ui->widget_cloudViewer->updateCameraTargetPosition(currentPose);
 	}

 	_ui->widget_cloudViewer->render();
--- a/guilib/src/OdometryViewer.cpp
+++ b/guilib/src/OdometryViewer.cpp
@@ -147,7 +147,7 @@ void OdometryViewer::processData()
 				this->addCloud(uFormat("cloud%d", clouds_.rbegin()->first), clouds_.rbegin()->second, data.pose());
 			}

-			this->updateCameraPosition(data.pose());
+			this->updateCameraTargetPosition(data.pose());

 			if(qualityWarningThr_ && quality>=0 && quality < qualityWarningThr_)
 			{
--- a/guilib/src/PreferencesDialog.cpp
+++ b/guilib/src/PreferencesDialog.cpp
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QDir>
 #include <QtCore/QTimer>

+#include <QtGui/QVector3D>
 #include <QtGui/QFileDialog>
 #include <QtGui/QMessageBox>
 #include <QtGui/QStandardItemModel>
@@ -56,6 +57,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include "rtabmap/gui/LoopClosureViewer.h"
 #include "rtabmap/gui/DataRecorder.h"
+#include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/gui/ImageView.h"

 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -1687,6 +1690,120 @@ void PreferencesDialog::loadMainWindowState(QMainWindow * mainWindow)
 	loadWindowGeometry("MainWindow", mainWindow);
 }

+void PreferencesDialog::saveCloudViewerState(const QString & name, const CloudViewer * viewer)
+{
+	QSettings settings(getIniFilePath(), QSettings::IniFormat);
+	settings.beginGroup("Gui");
+	settings.beginGroup(name);
+
+	float poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ;
+	viewer->getCameraPosition(poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ);
+	QVector3D pose(poseX, poseY, poseZ);
+	QVector3D focal(focalX, focalY, focalZ);
+	if(!viewer->isCameraFree())
+	{
+		// make camera position relative to target
+		Transform T = viewer->getTargetPose();
+		if(viewer->isCameraTargetLocked())
+		{
+			T = Transform(T.x(), T.y(), T.z(), 0,0,0);
+		}
+		Transform F(focalX, focalY, focalZ, 0,0,0);
+		Transform P(poseX, poseY, poseZ, 0,0,0);
+		Transform newFocal = T.inverse() * F;
+		Transform newPose = newFocal * F.inverse() * P;
+		pose = QVector3D(newPose.x(), newPose.y(), newPose.z());
+		focal = QVector3D(newFocal.x(), newFocal.y(), newFocal.z());
+	}
+	settings.setValue("camera_pose", pose);
+	settings.setValue("camera_focal", focal);
+	settings.setValue("camera_up", QVector3D(upX, upY, upZ));
+
+	settings.setValue("grid", viewer->isGridShown());
+	settings.setValue("grid_cell_count", viewer->getGridCellCount());
+	settings.setValue("grid_cell_size", viewer->getGridCellSize());
+
+	settings.setValue("trajectory_shown", viewer->isTrajectoryShown());
+	settings.setValue("trajectory_size", viewer->getTrajectorySize());
+
+	settings.setValue("camera_target_locked", viewer->isCameraTargetLocked());
+	settings.setValue("camera_target_follow", viewer->isCameraTargetFollow());
+	settings.setValue("camera_free", viewer->isCameraFree());
+	settings.setValue("camera_lockZ", viewer->isCameraLockZ());
+
+	settings.setValue("bg_color", viewer->getBackgroundColor());
+
+	settings.endGroup(); // "name"
+	settings.endGroup(); // Gui
+}
+
+void PreferencesDialog::loadCloudViewerState(const QString & name, CloudViewer * viewer)
+{
+	QByteArray bytes;
+	QSettings settings(getIniFilePath(), QSettings::IniFormat);
+	settings.beginGroup("Gui");
+	settings.beginGroup(name);
+
+	float poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ;
+	viewer->getCameraPosition(poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ);
+	QVector3D pose(poseX, poseY, poseZ), focal(focalX, focalY, focalZ), up(upX, upY, upZ);
+	pose = settings.value("camera_pose", pose).value<QVector3D>();
+	focal = settings.value("camera_focal", focal).value<QVector3D>();
+	up = settings.value("camera_up", up).value<QVector3D>();
+	viewer->setCameraPosition(pose.x(),pose.y(),pose.z(), focal.x(),focal.y(),focal.z(), up.x(),up.y(),up.z());
+
+	viewer->setGridShown(settings.value("grid", viewer->isGridShown()).toBool());
+	viewer->setGridCellCount(settings.value("grid_cell_count", viewer->getGridCellCount()).toInt());
+	viewer->setGridCellSize(settings.value("grid_cell_size", viewer->getGridCellSize()).toFloat());
+
+	viewer->setTrajectoryShown(settings.value("trajectory_shown", viewer->isTrajectoryShown()).toBool());
+	viewer->setTrajectorySize(settings.value("trajectory_size", viewer->getTrajectorySize()).toInt());
+
+	viewer->setCameraTargetLocked(settings.value("camera_target_locked", viewer->isCameraTargetLocked()).toBool());
+	viewer->setCameraTargetFollow(settings.value("camera_target_follow", viewer->isCameraTargetFollow()).toBool());
+	if(settings.value("camera_free", viewer->isCameraFree()).toBool())
+	{
+		viewer->setCameraFree();
+	}
+	viewer->setCameraLockZ(settings.value("camera_lockZ", viewer->isCameraLockZ()).toBool());
+
+	viewer->setBackgroundColor(settings.value("bg_color", viewer->getBackgroundColor()).value<QColor>());
+
+	settings.endGroup(); //"name"
+	settings.endGroup(); // Gui
+}
+
+void PreferencesDialog::saveImageViewState(const QString & name, const ImageView * view)
+{
+	QSettings settings(getIniFilePath(), QSettings::IniFormat);
+	settings.beginGroup("Gui");
+	settings.beginGroup(name);
+
+	settings.setValue("image_shown", view->isImageShown());
+	settings.setValue("depth_shown", view->isImageDepthShown());
+	settings.setValue("features_shown", view->isFeaturesShown());
+	settings.setValue("lines_shown", view->isLinesShown());
+
+	settings.endGroup(); // "name"
+	settings.endGroup(); // Gui
+}
+
+void PreferencesDialog::loadImageViewState(const QString & name, ImageView * view)
+{
+	QByteArray bytes;
+	QSettings settings(getIniFilePath(), QSettings::IniFormat);
+	settings.beginGroup("Gui");
+	settings.beginGroup(name);
+
+	view->setImageShown(settings.value("image_shown", view->isImageShown()).toBool());
+	view->setImageDepthShown(settings.value("depth_shown", view->isImageDepthShown()).toBool());
+	view->setFeaturesShown(settings.value("features_shown", view->isFeaturesShown()).toBool());
+	view->setLinesShown(settings.value("lines_shown", view->isLinesShown()).toBool());
+
+	settings.endGroup(); //"name"
+	settings.endGroup(); // Gui
+}
+
 void PreferencesDialog::saveCustomConfig(const QString & section, const QString & key, const QString & value)
 {
 	QSettings settings(getIniFilePath(), QSettings::IniFormat);