Merged pcl_integration branch to trunk

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-09-03 01:50:24 +08:00 · 2013-12-11 00:12:44 +00:00
parent 97c70d394e
commit 8b8511e154
124 changed files with 21692 additions and 4458 deletions
--- a/guilib/src/OdometryViewer.cpp
+++ b/guilib/src/OdometryViewer.cpp
@@ -0,0 +1,174 @@
+/*
+ * OdometryViewer.cpp
+ *
+ *  Created on: 2013-10-15
+ *      Author: Mathieu
+ */
+
+#include "rtabmap/gui/OdometryViewer.h"
+
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <pcl/common/transforms.h>
+#include <pcl/io/pcd_io.h>
+#include <QtGui/QInputDialog>
+#include <QtGui/QAction>
+#include <QtGui/QMenu>
+#include <QtGui/QKeyEvent>
+
+namespace rtabmap {
+
+
+OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, QWidget * parent) :
+		CloudViewer(parent),
+		maxClouds_(maxClouds),
+		voxelSize_(voxelSize),
+		decimation_(decimation),
+		id_(0),
+		_aSetVoxelSize(0),
+		_aSetDecimation(0),
+		_aSetCloudHistorySize(0),
+		_aPause(0)
+{
+
+	//add actions to CloudViewer menu
+	_aSetVoxelSize = new QAction("Set voxel size...", this);
+	_aSetDecimation = new QAction("Set depth image decimation...", this);
+	_aSetCloudHistorySize = new QAction("Set cloud history size...", this);
+	_aPause = new QAction("Pause", this);
+	_aPause->setCheckable(true);
+	menu()->addAction(_aSetVoxelSize);
+	menu()->addAction(_aSetDecimation);
+	menu()->addAction(_aSetCloudHistorySize);
+	menu()->addAction(_aPause);
+}
+
+void OdometryViewer::processData()
+{
+	rtabmap::Image data;
+	dataMutex_.lock();
+	if(buffer_.size())
+	{
+		data = buffer_.back();
+		buffer_.clear();
+	}
+	dataMutex_.unlock();
+
+	if(!data.empty() && this->isVisible())
+	{
+		UINFO("New pose = %s", data.pose().prettyPrint().c_str());
+
+		// visualization: buffering the clouds
+		// Create the new cloud
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+		cloud = util3d::cloudFromDepthRGB(
+				data.image(),
+				data.depth(),
+			   float(data.depth().cols/2),
+			   float(data.depth().rows/2),
+			   1.0f/data.depthConstant(),
+			   1.0f/data.depthConstant(),
+			   decimation_);
+
+		if(voxelSize_ > 0.0f)
+		{
+			cloud = util3d::voxelize(cloud, voxelSize_);
+		}
+
+		cloud = util3d::transformPointCloud(cloud, data.localTransform());
+
+		data.id()?id_=data.id():++id_;
+
+		clouds_.insert(std::make_pair(id_, cloud));
+
+		if((int)clouds_.size() > maxClouds_)
+		{
+			this->removeCloud(uFormat("cloud%d", clouds_.begin()->first));
+			clouds_.erase(clouds_.begin());
+		}
+
+		if(clouds_.size())
+		{
+			this->addCloud(uFormat("cloud%d", clouds_.rbegin()->first), clouds_.rbegin()->second, data.pose());
+		}
+
+		this->updateCameraPosition(data.pose());
+
+		this->setBackgroundColor(Qt::black);
+
+		this->render();
+	}
+}
+
+void OdometryViewer::handleEvent(UEvent * event)
+{
+	if(!_aPause->isChecked())
+	{
+		if(event->getClassName().compare("OdometryEvent") == 0)
+		{
+			rtabmap::OdometryEvent * odomEvent = (rtabmap::OdometryEvent*)event;
+
+			if(odomEvent->isValid())
+			{
+				bool empty = false;
+				dataMutex_.lock();
+				if(buffer_.empty())
+				{
+					buffer_.push_back(odomEvent->data());
+					empty= true;
+				}
+				else
+				{
+					buffer_.back() = odomEvent->data();
+				}
+				dataMutex_.unlock();
+				if(empty)
+				{
+					QMetaObject::invokeMethod(this, "processData");
+				}
+			}
+			else
+			{
+				//UWARN("odom=%fs, Cannot compute odometry!!!", timer_.restart());
+				QMetaObject::invokeMethod(this, "setBackgroundColor", Q_ARG(QColor, Qt::darkRed));
+				QMetaObject::invokeMethod(this, "render");
+			}
+		}
+	}
+}
+
+void OdometryViewer::handleAction(QAction * a)
+{
+	CloudViewer::handleAction(a);
+	if(a == _aSetVoxelSize)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Set voxel size"), tr("Size (0=disabled)"), voxelSize_, 0.0, 0.1, 2, &ok);
+		if(ok)
+		{
+			voxelSize_ = value;
+		}
+	}
+	else if(a == _aSetCloudHistorySize)
+	{
+		bool ok;
+		int value = QInputDialog::getInt(this, tr("Set cloud history size"), tr("Size (0=infinite)"), maxClouds_, 0, 100, 1, &ok);
+		if(ok)
+		{
+			maxClouds_ = value;
+		}
+	}
+	else if(a == _aSetDecimation)
+	{
+		bool ok;
+		int value = QInputDialog::getInt(this, tr("Set depth image decimation"), tr("Decimation (0=infinite)"), decimation_, 1, 8, 1, &ok);
+		if(ok)
+		{
+			decimation_ = value;
+		}
+	}
+}
+
+} /* namespace rtabmap */