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,150 @@
+/*
+ * CameraOpenni.cpp
+ *
+ *  Created on: 2013-08-22
+ *      Author: Mathieu
+ */
+
+#include "rtabmap/core/CameraOpenni.h"
+#include "rtabmap/core/CameraEvent.h"
+#include "rtabmap/utilite/ULogger.h"
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <pcl/io/openni_grabber.h>
+
+namespace rtabmap {
+
+CameraOpenni::CameraOpenni(const std::string & deviceId, float inputRate, const Transform & localTransform) :
+		interface_(0),
+		deviceId_(deviceId),
+		rate_(inputRate),
+		frameRateTimer_(new UTimer()),
+		localTransform_(localTransform),
+		seq_(0)
+{
+}
+
+CameraOpenni::~CameraOpenni()
+{
+	UDEBUG("");
+	kill();
+	delete frameRateTimer_;
+	if(interface_)
+	{
+		uSleep(100); // make sure it is stopped
+		delete interface_;
+		interface_ = 0;
+	}
+}
+
+void CameraOpenni::image_cb (
+		const boost::shared_ptr<openni_wrapper::Image>& rgb,
+		const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
+		float constant)
+{
+	if(rate_>0.0f)
+	{
+		if(frameRateTimer_->getElapsedTime() < 1.0f/rate_)
+		{
+			return;
+		}
+	}
+	frameRateTimer_->start();
+
+	UTimer t;
+
+	cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
+	rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
+	cv::Mat bgrFrame;
+	cv::cvtColor(rgbFrame, bgrFrame, CV_RGB2BGR);
+
+	cv::Mat depthFrame(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
+	depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depthFrame.data);
+
+	this->post(new CameraEvent(bgrFrame, depthFrame, constant, localTransform_, ++seq_));
+}
+
+bool CameraOpenni::init()
+{
+	if(interface_ && interface_->isRunning())
+	{
+		UERROR("Already started!!!\n");
+		return false;
+	}
+	else if(interface_)
+	{
+		delete interface_;
+		interface_ = 0;
+	}
+
+	seq_ = 0;
+	try
+	{
+		interface_ = new pcl::OpenNIGrabber(deviceId_);
+	}
+	catch(const pcl::IOException& ex)
+	{
+		UERROR("OpenNI exception: %s", ex.what());
+		if(interface_)
+		{
+			delete interface_;
+			interface_ = 0;
+		}
+		return false;
+	}
+
+	frameRateTimer_->start();
+	return true;
+}
+
+void CameraOpenni::start()
+{
+	if(interface_)
+	{
+		if(!connection_.connected())
+		{
+			boost::function<void (
+					const boost::shared_ptr<openni_wrapper::Image>&,
+					const boost::shared_ptr<openni_wrapper::DepthImage>&,
+					float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
+			connection_ = interface_->registerCallback (f);
+		}
+
+		if(!interface_->isRunning())
+		{
+			interface_->start ();
+		}
+	}
+}
+
+void CameraOpenni::pause()
+{
+	if(connection_.connected())
+	{
+		connection_.disconnect();
+	}
+}
+
+void CameraOpenni::kill()
+{
+	UDEBUG("");
+	if(interface_)
+	{
+		interface_->stop();
+	}
+}
+
+bool CameraOpenni::isRunning()
+{
+	return (interface_ && interface_->isRunning());
+}
+
+void CameraOpenni::setFrameRate(float rate)
+{
+	rate_ = rate;
+}
+
+} /* namespace rtabmap */