Merged pcl_integration branch to trunk

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-10-07 02:07:45 +08:00 · 2013-12-11 00:12:44 +00:00
parent 97c70d394e
commit 8b8511e154
124 changed files with 21692 additions and 4458 deletions
@@ -0,0 +1,212 @@
+/*
+ * LoopClosureViewer.cpp
+ *
+ *  Created on: 2013-10-21
+ *      Author: Mathieu
+ */
+
+#include "rtabmap/gui/LoopClosureViewer.h"
+#include "ui_loopClosureViewer.h"
+
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Signature.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UStl.h"
+
+#include <QtCore/QTimer>
+
+namespace rtabmap {
+
+LoopClosureViewer::LoopClosureViewer(QWidget * parent) :
+	QWidget(parent),
+	sA_(0),
+	sB_(0),
+	decimation_(1),
+	maxDepth_(0),
+	samples_(0)
+{
+	ui_ = new Ui_loopClosureViewer();
+	ui_->setupUi(this);
+
+	connect(ui_->checkBox_rawCloud, SIGNAL(clicked()), this, SLOT(updateView()));
+}
+
+LoopClosureViewer::~LoopClosureViewer() {
+	delete ui_;
+	if(sA_)
+	{
+		delete sA_;
+	}
+	if(sB_)
+	{
+		delete sB_;
+	}
+}
+
+void LoopClosureViewer::setData(Signature * sA, Signature * sB)
+{
+	if(sA_)
+	{
+		delete sA_;
+	}
+	if(sB_)
+	{
+		delete sB_;
+	}
+	sA_ = sA;
+	sB_ = sB;
+	if(sA_ && sB_)
+	{
+		ui_->label_idA->setText(QString("[%1-%2]").arg(sA->id()).arg(sB->id()));
+	}
+}
+
+void LoopClosureViewer::updateView(const Transform & transform)
+{
+	if(sA_ && sB_)
+	{
+		int decimation = 1;
+		float maxDepth = 0;
+		int samples = 0;
+
+		if(!ui_->checkBox_rawCloud->isChecked())
+		{␍			decimation = decimation_;
+			maxDepth = maxDepth_;
+			samples = samples_;
+		}
+
+		UDEBUG("decimation = %d", decimation);
+		UDEBUG("maxDepth = %d", maxDepth);
+		UDEBUG("samples = %d", samples);
+
+		Transform t;
+		if(!transform.isNull())
+		{
+			transform_ = transform;
+			t = transform;
+		}
+		else if(!transform_.isNull())
+		{
+			t = transform_;
+		}
+		else
+		{
+			t = sB_->getPose();
+		}
+
+		UDEBUG("t= %s", t.prettyPrint().c_str());
+		ui_->label_transform->setText(QString("(%1)").arg(t.prettyPrint().c_str()));
+		if(!t.isNull())
+		{
+			util3d::CompressionThread ctiA(sA_->getImage(), true);
+			util3d::CompressionThread ctdA(sA_->getDepth(), true);
+			util3d::CompressionThread ctiB(sB_->getImage(), true);
+			util3d::CompressionThread ctdB(sB_->getDepth(), true);
+			util3d::CompressionThread ct2dA(sA_->getDepth2D(), false);
+			util3d::CompressionThread ct2dB(sB_->getDepth2D(), false);
+			ctiA.start();
+			ctdA.start();
+			ctiB.start();
+			ctdB.start();
+			ct2dA.start();
+			ct2dB.start();
+			ctiA.join();
+			ctdA.join();
+			ctiB.join();
+			ctdB.join();
+			ct2dA.join();
+			ct2dB.join();
+			cv::Mat imageA = ctiA.getUncompressedData();
+			cv::Mat depthA = ctdA.getUncompressedData();
+			cv::Mat imageB = ctiB.getUncompressedData();
+			cv::Mat depthB = ctdB.getUncompressedData();
+			cv::Mat depth2dA = ct2dA.getUncompressedData();
+			cv::Mat depth2dB = ct2dB.getUncompressedData();
+
+			//cloud 3d
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
+			cloudA = util3d::cloudFromDepthRGB(
+					imageA,
+					depthA,
+					depthA.cols/2,
+					depthA.rows/2,
+					1.0f/sA_->getDepthConstant(),
+					1.0f/sA_->getDepthConstant(),
+					decimation);
+
+			cloudA = util3d::removeNaNFromPointCloud(cloudA);
+
+			if(maxDepth>0.0)
+			{
+				cloudA = util3d::passThrough(cloudA, "z", 0, maxDepth);
+			}
+			if(samples>0 && (int)cloudA->size() > samples)
+			{
+				cloudA = util3d::sampling(cloudA, samples);
+			}
+			cloudA = util3d::transformPointCloud(cloudA, sA_->getLocalTransform());
+
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
+			cloudB = util3d::cloudFromDepthRGB(
+					imageB,
+					depthB,
+					depthB.cols/2,
+					depthB.rows/2,
+					1.0f/sB_->getDepthConstant(),
+					1.0f/sB_->getDepthConstant(),
+					decimation);
+
+			cloudB = util3d::removeNaNFromPointCloud(cloudB);
+
+			if(maxDepth>0.0)
+			{
+				cloudB = util3d::passThrough(cloudB, "z", 0, maxDepth);
+			}
+			if(samples>0 && (int)cloudB->size() > samples)
+			{
+				cloudB = util3d::sampling(cloudB, samples);
+			}
+			cloudB = util3d::transformPointCloud(cloudB, t*sB_->getLocalTransform());
+
+			//cloud 2d
+			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
+			scanA = util3d::depth2DToPointCloud(depth2dA);
+			scanB = util3d::depth2DToPointCloud(depth2dB);
+			scanB = util3d::transformPointCloud(scanB, t);
+
+			ui_->label_idA->setText(QString("[%1 (%2) -> %3 (%4)]").arg(sB_->id()).arg(cloudB->size()).arg(sA_->id()).arg(cloudA->size()));
+
+			if(cloudA->size())
+			{
+				ui_->cloudViewerTransform->addOrUpdateCloud("cloud0", cloudA);
+			}
+			if(cloudB->size())
+			{
+				ui_->cloudViewerTransform->addOrUpdateCloud("cloud1", cloudB);
+			}
+			if(scanA->size())
+			{
+				ui_->cloudViewerTransform->addOrUpdateCloud("scan0", scanA);
+			}
+			if(scanB->size())
+			{
+				ui_->cloudViewerTransform->addOrUpdateCloud("scan1", scanB);
+			}
+		}
+		else
+		{
+			ui_->cloudViewerTransform->removeAllClouds();
+		}
+		ui_->cloudViewerTransform->render();
+	}
+}
+
+
+void LoopClosureViewer::showEvent(QShowEvent * event)
+{
+	QWidget::showEvent( event );
+	QTimer::singleShot(500, this, SLOT(updateView())); // make sure the QVTKWidget is shown!
+}
+
+} /* namespace rtabmap */