mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
Refactoring CameraRGBD class (now including OpenNI, OpenNI2, OpenNI from OpenCV and Freenect)
Added tool to test RGB-D camera: rtabmap-rgbd_camera Fixed Freenect corrupted depth image (after some time) git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1366 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
@@ -31,12 +31,6 @@
|
||||
class UDirectory;
|
||||
class UTimer;
|
||||
|
||||
namespace openni
|
||||
{
|
||||
class Device;
|
||||
class VideoStream;
|
||||
}
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
@@ -48,20 +42,16 @@ class RTABMAP_EXP Camera
|
||||
{
|
||||
public:
|
||||
virtual ~Camera();
|
||||
cv::Mat takeImage(); // backward compatibility
|
||||
void takeImage(cv::Mat & rgb);
|
||||
void takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
|
||||
cv::Mat takeImage();
|
||||
virtual bool init() = 0;
|
||||
|
||||
//getters
|
||||
void getImageSize(unsigned int & width, unsigned int & height);
|
||||
float getImageRate() const {return _imageRate;}
|
||||
const Transform & getLocalTransform() const {return _localTransform;}
|
||||
|
||||
//setters
|
||||
void setImageRate(float imageRate) {_imageRate = imageRate;}
|
||||
void setImageSize(unsigned int width, unsigned int height);
|
||||
void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
|
||||
|
||||
protected:
|
||||
/**
|
||||
@@ -71,17 +61,14 @@ protected:
|
||||
*/
|
||||
Camera(float imageRate = 0,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0,
|
||||
unsigned int framesDropped = 0);
|
||||
unsigned int imageHeight = 0);
|
||||
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant) = 0;
|
||||
virtual cv::Mat captureImage() = 0;
|
||||
|
||||
private:
|
||||
float _imageRate;
|
||||
unsigned int _imageWidth;
|
||||
unsigned int _imageHeight;
|
||||
unsigned int _framesDropped;
|
||||
Transform _localTransform;
|
||||
UTimer * _frameRateTimer;
|
||||
};
|
||||
|
||||
@@ -98,15 +85,14 @@ public:
|
||||
bool refreshDir = false,
|
||||
float imageRate = 0,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0,
|
||||
unsigned int framesDropped = 0);
|
||||
unsigned int imageHeight = 0);
|
||||
virtual ~CameraImages();
|
||||
|
||||
virtual bool init();
|
||||
std::string getPath() const {return _path;}
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
|
||||
virtual cv::Mat captureImage();
|
||||
|
||||
private:
|
||||
std::string _path;
|
||||
@@ -135,13 +121,11 @@ public:
|
||||
CameraVideo(int usbDevice = 0,
|
||||
float imageRate = 0,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0,
|
||||
unsigned int framesDropped = 0);
|
||||
unsigned int imageHeight = 0);
|
||||
CameraVideo(const std::string & filePath,
|
||||
float imageRate = 0,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0,
|
||||
unsigned int framesDropped = 0);
|
||||
unsigned int imageHeight = 0);
|
||||
virtual ~CameraVideo();
|
||||
|
||||
virtual bool init();
|
||||
@@ -149,7 +133,7 @@ public:
|
||||
const std::string & getFilePath() const {return _filePath;}
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
|
||||
virtual cv::Mat captureImage();
|
||||
|
||||
private:
|
||||
// File type
|
||||
@@ -163,58 +147,4 @@ private:
|
||||
};
|
||||
|
||||
|
||||
|
||||
/////////////////////////
|
||||
// CameraRGBD
|
||||
/////////////////////////
|
||||
class RTABMAP_EXP CameraRGBD :
|
||||
public Camera
|
||||
{
|
||||
|
||||
public:
|
||||
static bool available();
|
||||
|
||||
public:
|
||||
CameraRGBD(float imageRate = 0,
|
||||
bool asus = false);
|
||||
virtual ~CameraRGBD();
|
||||
|
||||
virtual bool init();
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
|
||||
|
||||
private:
|
||||
bool _asus;
|
||||
cv::VideoCapture _capture;
|
||||
float _depthFocal;
|
||||
};
|
||||
|
||||
/////////////////////////
|
||||
// CameraOpenNI2
|
||||
/////////////////////////
|
||||
class RTABMAP_EXP CameraOpenNI2 :
|
||||
public Camera
|
||||
{
|
||||
|
||||
public:
|
||||
static bool available();
|
||||
|
||||
public:
|
||||
CameraOpenNI2(float imageRate = 0);
|
||||
virtual ~CameraOpenNI2();
|
||||
|
||||
virtual bool init();
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
|
||||
|
||||
private:
|
||||
openni::Device * _device;
|
||||
openni::VideoStream * _color;
|
||||
openni::VideoStream * _depth;
|
||||
float _depthFocal;
|
||||
};
|
||||
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
@@ -1,98 +0,0 @@
|
||||
/*
|
||||
* CameraFreenect.h
|
||||
*
|
||||
* Created on: 2014-06-02
|
||||
* Author: Mathieu
|
||||
*/
|
||||
|
||||
#ifndef CAMERAFREENECT_H_
|
||||
#define CAMERAFREENECT_H_
|
||||
|
||||
#include <rtabmap/core/RtabmapExp.h>
|
||||
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <rtabmap/utilite/UEventsSender.h>
|
||||
#include <rtabmap/utilite/UThread.h>
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <stdint.h>
|
||||
|
||||
class UTimer;
|
||||
typedef struct _freenect_context freenect_context;
|
||||
typedef struct _freenect_device freenect_device;
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class RTABMAP_EXP FreenectDevice {
|
||||
public:
|
||||
FreenectDevice(freenect_context *ctx, int index);
|
||||
virtual ~FreenectDevice();
|
||||
void startVideo();
|
||||
void stopVideo();
|
||||
void startDepth();
|
||||
void stopDepth();
|
||||
|
||||
bool init();
|
||||
|
||||
cv::Mat getRgb();
|
||||
cv::Mat getDepth();
|
||||
float getDepthFocal() const {return depthFocal_;}
|
||||
|
||||
// Do not call directly even in child
|
||||
void VideoCallback(void *video, uint32_t timestamp);
|
||||
// Do not call directly even in child
|
||||
void DepthCallback(void *depth, uint32_t timestamp);
|
||||
private:
|
||||
static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp);
|
||||
static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp);
|
||||
|
||||
//noncopyable
|
||||
FreenectDevice( const FreenectDevice& );
|
||||
const FreenectDevice& operator=( const FreenectDevice& );
|
||||
|
||||
private:
|
||||
int index_;
|
||||
freenect_context * ctx_;
|
||||
freenect_device * device_;
|
||||
cv::Mat depthMat_;
|
||||
cv::Mat rgbMat_;
|
||||
UMutex depthMutex_;
|
||||
UMutex rgbMutex_;
|
||||
bool depthReady_;
|
||||
bool rgbReady_;
|
||||
float depthFocal_;
|
||||
};
|
||||
|
||||
class RTABMAP_EXP CameraFreenect : public UEventsSender, public UThread
|
||||
{
|
||||
public:
|
||||
static bool available();
|
||||
|
||||
public:
|
||||
// default local transform z in, x right, y down));
|
||||
CameraFreenect(int deviceId= 0,
|
||||
float rate=0,
|
||||
const Transform & localTransform = Transform::getIdentity());
|
||||
virtual ~CameraFreenect();
|
||||
|
||||
bool init();
|
||||
|
||||
void setFrameRate(float rate);
|
||||
|
||||
private:
|
||||
virtual void mainLoopBegin();
|
||||
virtual void mainLoop();
|
||||
virtual void mainLoopEnd();
|
||||
|
||||
private:
|
||||
int deviceId_;
|
||||
float rate_;
|
||||
UTimer * frameRateTimer_;
|
||||
Transform localTransform_; // transform from camera_optical_link to base_link
|
||||
int seq_;
|
||||
freenect_context * ctx_;
|
||||
FreenectDevice * freenectDevice_;
|
||||
};
|
||||
|
||||
} /* namespace rtabmap */
|
||||
#endif /* CAMERAFREENECT_H_ */
|
||||
@@ -1,63 +0,0 @@
|
||||
/*
|
||||
* CameraOpenni.h
|
||||
*
|
||||
* Created on: 2013-08-22
|
||||
* Author: Mathieu
|
||||
*/
|
||||
|
||||
#ifndef CAMERAOPENNI_H_
|
||||
#define CAMERAOPENNI_H_
|
||||
|
||||
#include <rtabmap/core/RtabmapExp.h>
|
||||
|
||||
#include <pcl/io/openni_camera/openni_depth_image.h>
|
||||
#include <pcl/io/openni_camera/openni_image.h>
|
||||
|
||||
#include <boost/signals2/connection.hpp>
|
||||
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <rtabmap/utilite/UEventsSender.h>
|
||||
|
||||
class UTimer;
|
||||
|
||||
namespace pcl
|
||||
{
|
||||
class Grabber;
|
||||
}
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class RTABMAP_EXP CameraOpenni : public UEventsSender
|
||||
{
|
||||
public:
|
||||
// default local transform z in, x right, y down));
|
||||
CameraOpenni(const std::string & deviceId="",
|
||||
float rate=0,
|
||||
const Transform & localTRansform = Transform::getIdentity());
|
||||
virtual ~CameraOpenni();
|
||||
|
||||
void image_cb (
|
||||
const boost::shared_ptr<openni_wrapper::Image>& rgb,
|
||||
const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
|
||||
float constant);
|
||||
|
||||
bool init();
|
||||
void start();
|
||||
void pause();
|
||||
void kill();
|
||||
bool isRunning();
|
||||
|
||||
void setFrameRate(float rate);
|
||||
|
||||
private:
|
||||
pcl::Grabber* interface_;
|
||||
std::string deviceId_;
|
||||
float rate_;
|
||||
UTimer * frameRateTimer_;
|
||||
Transform localTransform_; // transform from camera_optical_link to base_link
|
||||
int seq_;
|
||||
boost::signals2::connection connection_;
|
||||
};
|
||||
|
||||
} /* namespace rtabmap */
|
||||
#endif /* CAMERAOPENNI_H_ */
|
||||
215
corelib/include/rtabmap/core/CameraRGBD.h
Normal file
215
corelib/include/rtabmap/core/CameraRGBD.h
Normal file
@@ -0,0 +1,215 @@
|
||||
/*
|
||||
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
|
||||
*
|
||||
* This file is part of RTAB-Map.
|
||||
*
|
||||
* RTAB-Map is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* RTAB-Map is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include "rtabmap/core/Image.h"
|
||||
#include "rtabmap/utilite/UMutex.h"
|
||||
#include "rtabmap/utilite/USemaphore.h"
|
||||
#include <set>
|
||||
#include <stack>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
|
||||
#include <pcl/io/openni_camera/openni_depth_image.h>
|
||||
#include <pcl/io/openni_camera/openni_image.h>
|
||||
|
||||
#include <boost/signals2/connection.hpp>
|
||||
|
||||
class UDirectory;
|
||||
class UTimer;
|
||||
|
||||
namespace openni
|
||||
{
|
||||
class Device;
|
||||
class VideoStream;
|
||||
}
|
||||
|
||||
namespace pcl
|
||||
{
|
||||
class Grabber;
|
||||
}
|
||||
|
||||
typedef struct _freenect_context freenect_context;
|
||||
typedef struct _freenect_device freenect_device;
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
/**
|
||||
* Class CameraRGBD
|
||||
*
|
||||
*/
|
||||
class RTABMAP_EXP CameraRGBD
|
||||
{
|
||||
public:
|
||||
virtual ~CameraRGBD();
|
||||
void takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
|
||||
virtual bool init() = 0;
|
||||
|
||||
//getters
|
||||
float getImageRate() const {return _imageRate;}
|
||||
const Transform & getLocalTransform() const {return _localTransform;}
|
||||
|
||||
//setters
|
||||
void setImageRate(float imageRate) {_imageRate = imageRate;}
|
||||
void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* Constructor
|
||||
*
|
||||
* @param imageRate : image/second , 0 for fast as the camera can
|
||||
*/
|
||||
CameraRGBD(float imageRate = 0,
|
||||
const Transform & localTransform = Transform::getIdentity());
|
||||
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant) = 0;
|
||||
|
||||
private:
|
||||
float _imageRate;
|
||||
Transform _localTransform;
|
||||
UTimer * _frameRateTimer;
|
||||
};
|
||||
|
||||
/////////////////////////
|
||||
// CameraOpenNIPCL
|
||||
/////////////////////////
|
||||
class RTABMAP_EXP CameraOpenni :
|
||||
public CameraRGBD
|
||||
{
|
||||
public:
|
||||
static bool available() {return true;}
|
||||
|
||||
public:
|
||||
// default local transform z in, x right, y down));
|
||||
CameraOpenni(const std::string & deviceId="",
|
||||
float imageRate = 0,
|
||||
const Transform & localTransform = Transform::getIdentity());
|
||||
virtual ~CameraOpenni();
|
||||
|
||||
void image_cb (
|
||||
const boost::shared_ptr<openni_wrapper::Image>& rgb,
|
||||
const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
|
||||
float constant);
|
||||
|
||||
bool init();
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
|
||||
|
||||
private:
|
||||
pcl::Grabber* interface_;
|
||||
std::string deviceId_;
|
||||
boost::signals2::connection connection_;
|
||||
cv::Mat depth_;
|
||||
cv::Mat rgb_;
|
||||
float depthConstant_;
|
||||
UMutex dataMutex_;
|
||||
USemaphore dataReady_;
|
||||
};
|
||||
|
||||
/////////////////////////
|
||||
// CameraOpenNICV
|
||||
/////////////////////////
|
||||
class RTABMAP_EXP CameraOpenNICV :
|
||||
public CameraRGBD
|
||||
{
|
||||
|
||||
public:
|
||||
static bool available();
|
||||
|
||||
public:
|
||||
CameraOpenNICV(bool asus = false,
|
||||
float imageRate = 0,
|
||||
const Transform & localTransform = Transform::getIdentity());
|
||||
virtual ~CameraOpenNICV();
|
||||
|
||||
virtual bool init();
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
|
||||
|
||||
private:
|
||||
bool _asus;
|
||||
cv::VideoCapture _capture;
|
||||
float _depthFocal;
|
||||
};
|
||||
|
||||
/////////////////////////
|
||||
// CameraOpenNI2
|
||||
/////////////////////////
|
||||
class RTABMAP_EXP CameraOpenNI2 :
|
||||
public CameraRGBD
|
||||
{
|
||||
|
||||
public:
|
||||
static bool available();
|
||||
|
||||
public:
|
||||
CameraOpenNI2(float imageRate = 0,
|
||||
const Transform & localTransform = Transform::getIdentity());
|
||||
virtual ~CameraOpenNI2();
|
||||
|
||||
virtual bool init();
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
|
||||
|
||||
private:
|
||||
openni::Device * _device;
|
||||
openni::VideoStream * _color;
|
||||
openni::VideoStream * _depth;
|
||||
float _depthFocal;
|
||||
};
|
||||
|
||||
|
||||
/////////////////////////
|
||||
// CameraFreenect
|
||||
/////////////////////////
|
||||
class FreenectDevice;
|
||||
|
||||
class RTABMAP_EXP CameraFreenect :
|
||||
public CameraRGBD
|
||||
{
|
||||
public:
|
||||
static bool available();
|
||||
|
||||
public:
|
||||
// default local transform z in, x right, y down));
|
||||
CameraFreenect(int deviceId= 0,
|
||||
float imageRate=0.0f,
|
||||
const Transform & localTransform = Transform::getIdentity());
|
||||
virtual ~CameraFreenect();
|
||||
|
||||
bool init();
|
||||
|
||||
protected:
|
||||
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
|
||||
|
||||
private:
|
||||
int deviceId_;
|
||||
freenect_context * ctx_;
|
||||
FreenectDevice * freenectDevice_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap
|
||||
@@ -21,29 +21,27 @@
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <rtabmap/utilite/UThreadNode.h>
|
||||
#include <rtabmap/utilite/UEventsHandler.h>
|
||||
|
||||
#include "rtabmap/core/Parameters.h"
|
||||
|
||||
#include <stack>
|
||||
#include <rtabmap/utilite/UThread.h>
|
||||
#include <rtabmap/utilite/UEventsSender.h>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
class Camera;
|
||||
class CameraRGBD;
|
||||
|
||||
/**
|
||||
* Class CameraThread
|
||||
*
|
||||
*/
|
||||
class RTABMAP_EXP CameraThread :
|
||||
public UThreadNode,
|
||||
public UThread,
|
||||
public UEventsSender
|
||||
{
|
||||
public:
|
||||
// ownership transferred
|
||||
CameraThread(Camera * camera, bool autoRestart = false);
|
||||
CameraThread(Camera * camera);
|
||||
CameraThread(CameraRGBD * camera);
|
||||
virtual ~CameraThread();
|
||||
|
||||
bool init(); // call camera->init()
|
||||
@@ -51,15 +49,14 @@ public:
|
||||
//getters
|
||||
bool isPaused() const {return !this->isRunning();}
|
||||
bool isCapturing() const {return this->isRunning();}
|
||||
void setAutoRestart(bool autoRestart) {_autoRestart = autoRestart;}
|
||||
Camera * getCamera() {return _camera;}
|
||||
void setImageRate(float imageRate);
|
||||
|
||||
private:
|
||||
virtual void mainLoop();
|
||||
|
||||
private:
|
||||
Camera * _camera;
|
||||
bool _autoRestart;
|
||||
CameraRGBD * _cameraRGBD;
|
||||
int _seq;
|
||||
};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user