i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Camera refactoring (#315)

Browse Source 
* Camera Refactoring part 1 (Mac OS X)

* fixed camera*** -> Camera***

* Fixed build for cameras Zed/RealSense/RealSense2

* increased version to 0.17.7

* fixed build for cameras K4W2 and FlyCapture2
This commit is contained in:
matlabbe
2018-10-01 19:33:56 -04:00
committed by GitHub
parent eb38b9cfab
commit 0059a4bc1b
116 changed files with 7716 additions and 6799 deletions
Download Patch File Download Diff File Expand all files Collapse all files

212
corelib/include/rtabmap/core/CameraRGB.h
View File

@@ -27,213 +27,5 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/Camera.h"
#include "rtabmap/utilite/UTimer.h"
#include <set>
#include <stack>
#include <list>
#include <vector>
class UDirectory;
namespace rtabmap
{
/////////////////////////
// CameraImages
/////////////////////////
class RTABMAP_EXP CameraImages :
public Camera
{
public:
CameraImages();
CameraImages(
const std::string & path,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraImages();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual bool odomProvided() const { return odometry_.size() > 0; }
std::string getPath() const {return _path;}
unsigned int imagesCount() const;
std::vector<std::string> filenames() const;
bool isImagesRectified() const {return _rectifyImages;}
int getBayerMode() const {return _bayerMode;}
const CameraModel & cameraModel() const {return _model;}
void setPath(const std::string & dir) {_path=dir;}
virtual void setStartIndex(int index) {_startAt = index;} // negative means last
void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
void setBayerMode(int mode) {_bayerMode = mode;} // -1=disabled (default) 0=BayerBG, 1=BayerGB, 2=BayerRG, 3=BayerGR
void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "", bool syncImageRateWithStamps=true)
{
_filenamesAreTimestamps = fileNamesAreStamps;
_timestampsPath=filePath;
_syncImageRateWithStamps = syncImageRateWithStamps;
}
void setScanPath(
const std::string & dir,
int maxScanPts = 0,
int downsampleStep = 1,
float voxelSize = 0.0f,
int normalsK = 0, // compute normals if > 0
float normalsRadius = 0, // compute normals if > 0
const Transform & localTransform=Transform::getIdentity(),
bool forceGroundNormalsUp = false)
{
_scanPath = dir;
_scanLocalTransform = localTransform;
_scanMaxPts = maxScanPts;
_scanDownsampleStep = downsampleStep;
_scanNormalsK = normalsK;
_scanNormalsRadius = normalsRadius;
_scanVoxelSize = voxelSize;
_scanForceGroundNormalsUp = forceGroundNormalsUp;
}
void setDepthFromScan(bool enabled, int fillHoles = 1, bool fillHolesFromBorder = false)
{
_depthFromScan = enabled;
_depthFromScanFillHoles = fillHoles;
_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
}
// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
void setOdometryPath(const std::string & filePath, int format = 0)
{
_odometryPath = filePath;
_odometryFormat = format;
}
// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
void setGroundTruthPath(const std::string & filePath, int format = 0)
{
_groundTruthPath = filePath;
_groundTruthFormat = format;
}
void setMaxPoseTimeDiff(double diff) {_maxPoseTimeDiff = diff;}
double getMaxPoseTimeDiff() const {return _maxPoseTimeDiff;}
void setDepth(bool isDepth, float depthScaleFactor = 1.0f)
{
_isDepth = isDepth;
_depthScaleFactor=depthScaleFactor;
}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
bool readPoses(
std::list<Transform> & outputPoses,
std::list<double> & stamps,
const std::string & filePath,
int format,
double maxTimeDiff) const;
private:
std::string _path;
int _startAt;
// If the list of files in the directory is refreshed
// on each call of takeImage()
bool _refreshDir;
bool _rectifyImages;
int _bayerMode;
bool _isDepth;
float _depthScaleFactor;
int _count;
UDirectory * _dir;
std::string _lastFileName;
int _countScan;
UDirectory * _scanDir;
std::string _lastScanFileName;
std::string _scanPath;
Transform _scanLocalTransform;
int _scanMaxPts;
int _scanDownsampleStep;
float _scanVoxelSize;
int _scanNormalsK;
float _scanNormalsRadius;
bool _scanForceGroundNormalsUp;
bool _depthFromScan;
int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical
bool _depthFromScanFillHolesFromBorder;
bool _filenamesAreTimestamps;
std::string _timestampsPath;
bool _syncImageRateWithStamps;
std::string _odometryPath;
int _odometryFormat;
std::string _groundTruthPath;
int _groundTruthFormat;
double _maxPoseTimeDiff;
std::list<double> _stamps;
std::list<Transform> odometry_;
std::list<Transform> groundTruth_;
CameraModel _model;
UTimer _captureTimer;
double _captureDelay;
};
/////////////////////////
// CameraVideo
/////////////////////////
class RTABMAP_EXP CameraVideo :
public Camera
{
public:
enum Source{kVideoFile, kUsbDevice};
public:
CameraVideo(int usbDevice = 0,
bool rectifyImages = false,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
CameraVideo(const std::string & filePath,
bool rectifyImages = false,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraVideo();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
int getUsbDevice() const {return _usbDevice;}
const std::string & getFilePath() const {return _filePath;}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
// File type
std::string _filePath;
bool _rectifyImages;
cv::VideoCapture _capture;
Source _src;
// Usb camera
int _usbDevice;
std::string _guid;
CameraModel _model;
};
} // namespace rtabmap
#include <rtabmap/core/camera/CameraImages.h>
#include <rtabmap/core/camera/CameraVideo.h>

488
corelib/include/rtabmap/core/CameraRGBD.h
View File

@@ -27,483 +27,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <rtabmap/core/camera/CameraFreenect.h>
#include <rtabmap/core/camera/CameraFreenect2.h>
#include <rtabmap/core/camera/CameraK4W2.h>
#include <rtabmap/core/camera/CameraOpenni.h>
#include <rtabmap/core/camera/CameraOpenNI2.h>
#include <rtabmap/core/camera/CameraOpenNICV.h>
#include <rtabmap/core/camera/CameraRealSense.h>
#include <rtabmap/core/camera/CameraRealSense2.h>
#include <rtabmap/core/camera/CameraRGBDImages.h>
#include "rtabmap/utilite/UMutex.h"
#include "rtabmap/utilite/USemaphore.h"
#include "rtabmap/core/CameraModel.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraRGB.h"
#include "rtabmap/core/Version.h"
#include <pcl/pcl_config.h>
#ifdef HAVE_OPENNI
#if __linux__ && __i386__ && __cplusplus >= 201103L
#warning "Openni driver is not available on i386 when building with c++11 support"
#else
#define RTABMAP_OPENNI
#include <pcl/io/openni_camera/openni_depth_image.h>
#include <pcl/io/openni_camera/openni_image.h>
#endif
#endif
#include <boost/signals2/connection.hpp>
namespace openni
{
class Device;
class VideoStream;
}
namespace pcl
{
class Grabber;
}
namespace libfreenect2
{
class Freenect2;
class Freenect2Device;
class SyncMultiFrameListener;
class Registration;
class PacketPipeline;
}
namespace rs
{
class context;
class device;
namespace slam {
class slam;
}
}
namespace rs2
{
class context;
class device;
class syncer;
}
struct rs2_intrinsics;
struct rs2_extrinsics;
typedef struct _freenect_context freenect_context;
typedef struct _freenect_device freenect_device;
typedef struct IKinectSensor IKinectSensor;
typedef struct ICoordinateMapper ICoordinateMapper;
typedef struct _DepthSpacePoint DepthSpacePoint;
typedef struct _ColorSpacePoint ColorSpacePoint;
typedef struct tagRGBQUAD RGBQUAD;
typedef struct IMultiSourceFrameReader IMultiSourceFrameReader;
namespace rtabmap
{
/////////////////////////
// CameraOpenNIPCL
/////////////////////////
class RTABMAP_EXP CameraOpenni :
public Camera
{
public:
static bool available();
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();
#ifdef RTABMAP_OPENNI
void image_cb (
const boost::shared_ptr<openni_wrapper::Image>& rgb,
const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
float constant);
#endif
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
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 Camera
{
public:
static bool available();
public:
CameraOpenNICV(bool asus = false,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraOpenNICV();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const {return "";} // unknown with OpenCV
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
bool _asus;
cv::VideoCapture _capture;
float _depthFocal;
};
/////////////////////////
// CameraOpenNI2
/////////////////////////
class RTABMAP_EXP CameraOpenNI2 :
public Camera
{
public:
static bool available();
static bool exposureGainAvailable();
enum Type {kTypeColorDepth, kTypeIRDepth, kTypeIR};
public:
CameraOpenNI2(const std::string & deviceId = "",
Type type = kTypeColorDepth,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraOpenNI2();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
bool setAutoWhiteBalance(bool enabled);
bool setAutoExposure(bool enabled);
bool setExposure(int value);
bool setGain(int value);
bool setMirroring(bool enabled);
void setOpenNI2StampsAndIDsUsed(bool used);
void setIRDepthShift(int horizontal, int vertical);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_OPENNI2
Type _type;
openni::Device * _device;
openni::VideoStream * _color;
openni::VideoStream * _depth;
float _depthFx;
float _depthFy;
std::string _deviceId;
bool _openNI2StampsAndIDsUsed;
StereoCameraModel _stereoModel;
int _depthHShift;
int _depthVShift;
#endif
};
/////////////////////////
// CameraFreenect
/////////////////////////
class FreenectDevice;
class RTABMAP_EXP CameraFreenect :
public Camera
{
public:
static bool available();
enum Type {kTypeColorDepth, kTypeIRDepth};
public:
// default local transform z in, x right, y down));
CameraFreenect(int deviceId= 0,
Type type = kTypeColorDepth,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraFreenect();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_FREENECT
int deviceId_;
Type type_;
freenect_context * ctx_;
FreenectDevice * freenectDevice_;
StereoCameraModel stereoModel_;
#endif
};
/////////////////////////
// CameraFreenect2
/////////////////////////
class RTABMAP_EXP CameraFreenect2 :
public Camera
{
public:
static bool available();
enum Type{
kTypeColor2DepthSD,
kTypeDepth2ColorSD,
kTypeDepth2ColorHD,
kTypeDepth2ColorHD2,
kTypeIRDepth,
kTypeColorIR
};
public:
// default local transform z in, x right, y down));
CameraFreenect2(int deviceId= 0,
Type type = kTypeDepth2ColorSD,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
float minDepth = 0.3f,
float maxDepth = 12.0f,
bool bilateralFiltering = true,
bool edgeAwareFiltering = true,
bool noiseFiltering = true,
const std::string & pipelineName = "");
virtual ~CameraFreenect2();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_FREENECT2
int deviceId_;
Type type_;
StereoCameraModel stereoModel_;
libfreenect2::Freenect2 * freenect2_;
libfreenect2::Freenect2Device *dev_;
libfreenect2::SyncMultiFrameListener * listener_;
libfreenect2::Registration * reg_;
float minKinect2Depth_;
float maxKinect2Depth_;
bool bilateralFiltering_;
bool edgeAwareFiltering_;
bool noiseFiltering_;
std::string pipelineName_;
#endif
};
/////////////////////////
// CameraK4W2
/////////////////////////
class RTABMAP_EXP CameraK4W2 :
public Camera
{
public:
static bool available();
enum Type {
kTypeColor2DepthSD,
kTypeDepth2ColorSD,
kTypeDepth2ColorHD
};
public:
static const int cDepthWidth = 512;
static const int cDepthHeight = 424;
static const int cColorWidth = 1920;
static const int cColorHeight = 1080;
public:
// default local transform z in, x right, y down));
CameraK4W2(int deviceId = 0, // not used
Type type = kTypeDepth2ColorSD,
float imageRate = 0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraK4W2();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
void close();
private:
#ifdef RTABMAP_K4W2
Type type_;
IKinectSensor* pKinectSensor_;
ICoordinateMapper* pCoordinateMapper_;
DepthSpacePoint* pDepthCoordinates_;
ColorSpacePoint* pColorCoordinates_;
IMultiSourceFrameReader* pMultiSourceFrameReader_;
RGBQUAD * pColorRGBX_;
INT_PTR hMSEvent;
CameraModel colorCameraModel_;
#endif
};
/////////////////////////
// CameraRealSense
/////////////////////////
class slam_event_handler;
class RTABMAP_EXP CameraRealSense :
public Camera
{
public:
static bool available();
enum RGBSource {kColor, kInfrared, kFishEye};
public:
// default local transform z in, x right, y down));
CameraRealSense(
int deviceId = 0,
int presetRGB = 0, // 0=best quality, 1=largest image, 2=highest framerate
int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate
bool computeOdometry = false,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraRealSense();
void setDepthScaledToRGBSize(bool enabled);
void setRGBSource(RGBSource source);
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual bool odomProvided() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_REALSENSE
rs::context * ctx_;
rs::device * dev_;
int deviceId_;
int presetRGB_;
int presetDepth_;
bool computeOdometry_;
bool depthScaledToRGBSize_;
RGBSource rgbSource_;
CameraModel cameraModel_;
std::vector<int> rsRectificationTable_;
int motionSeq_[2];
rs::slam::slam * slam_;
UMutex slamLock_;
std::map<double, std::pair<cv::Mat, cv::Mat> > bufferedFrames_;
std::pair<cv::Mat, cv::Mat> lastSyncFrames_;
UMutex dataMutex_;
USemaphore dataReady_;
#endif
};
/////////////////////////
// CameraRealSense2
/////////////////////////
class slam_event_handler;
class RTABMAP_EXP CameraRealSense2 :
public Camera
{
public:
static bool available();
public:
// default local transform z in, x right, y down));
CameraRealSense2(
const std::string & deviceId = "",
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraRealSense2();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
// parameters are set during initialization
void setEmitterEnabled(bool enabled);
void setIRDepthFormat(bool enabled);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_REALSENSE2
rs2::context * ctx_;
rs2::device * dev_;
std::string deviceId_;
rs2::syncer * syncer_;
float depth_scale_meters_;
rs2_intrinsics * depthIntrinsics_;
rs2_intrinsics * rgbIntrinsics_;
rs2_extrinsics * depthToRGBExtrinsics_;
cv::Mat depthBuffer_;
cv::Mat rgbBuffer_;
CameraModel model_;
bool emitterEnabled_;
bool irDepth_;
#endif
};
/////////////////////////
// CameraRGBDImages
/////////////////////////
class CameraImages;
class RTABMAP_EXP CameraRGBDImages :
public CameraImages
{
public:
static bool available();
public:
CameraRGBDImages(
const std::string & pathRGBImages,
const std::string & pathDepthImages,
float depthScaleFactor = 1.0f,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraRGBDImages();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);cameraDepth_.setStartIndex(index);} // negative means last
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
CameraImages cameraDepth_;
};
} // namespace rtabmap

233
corelib/include/rtabmap/core/CameraStereo.h
View File

@@ -27,231 +27,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/CameraModel.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraRGB.h"
#include "rtabmap/core/Version.h"
#include <list>
namespace FlyCapture2
{
class Camera;
}
namespace sl
{
class Camera;
}
namespace rtabmap
{
/////////////////////////
// CameraStereoDC1394
/////////////////////////
class DC1394Device;
class RTABMAP_EXP CameraStereoDC1394 :
public Camera
{
public:
static bool available();
public:
CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoDC1394();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_DC1394
DC1394Device *device_;
StereoCameraModel stereoModel_;
#endif
};
/////////////////////////
// CameraStereoFlyCapture2
/////////////////////////
class RTABMAP_EXP CameraStereoFlyCapture2 :
public Camera
{
public:
static bool available();
public:
CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoFlyCapture2();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_FLYCAPTURE2
FlyCapture2::Camera * camera_;
void * triclopsCtx_; // TriclopsContext
#endif
};
/////////////////////////
// CameraStereoZED
/////////////////////////
class RTABMAP_EXP CameraStereoZed :
public Camera
{
public:
static bool available();
public:
CameraStereoZed(
int deviceId,
int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY
int sensingMode = 0,// 0=STANDARD, 1=FILL
int confidenceThr = 100,
bool computeOdometry = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
bool selfCalibration = true);
CameraStereoZed(
const std::string & svoFilePath,
int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY
int sensingMode = 0,// 0=STANDARD, 1=FILL
int confidenceThr = 100,
bool computeOdometry = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
bool selfCalibration = true);
virtual ~CameraStereoZed();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual bool odomProvided() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_ZED
sl::Camera * zed_;
StereoCameraModel stereoModel_;
CameraVideo::Source src_;
int usbDevice_;
std::string svoFilePath_;
int resolution_;
int quality_;
bool selfCalibration_;
int sensingMode_;
int confidenceThr_;
bool computeOdometry_;
bool lost_;
#endif
};
/////////////////////////
// CameraStereoImages
/////////////////////////
class CameraImages;
class RTABMAP_EXP CameraStereoImages :
public CameraImages
{
public:
static bool available();
public:
CameraStereoImages(
const std::string & pathLeftImages,
const std::string & pathRightImages,
bool rectifyImages = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
CameraStereoImages(
const std::string & pathLeftRightImages,
bool rectifyImages = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoImages();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);camera2_->setStartIndex(index);} // negative means last
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
CameraImages * camera2_;
StereoCameraModel stereoModel_;
};
/////////////////////////
// CameraStereoVideo
/////////////////////////
class CameraImages;
class RTABMAP_EXP CameraStereoVideo :
public Camera
{
public:
static bool available();
public:
CameraStereoVideo(
const std::string & pathSideBySide,
bool rectifyImages = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
CameraStereoVideo(
const std::string & pathLeft,
const std::string & pathRight,
bool rectifyImages = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
CameraStereoVideo(
int device,
bool rectifyImages = false,
float imageRate = 0.0f,
const Transform & localTransform = Transform::getIdentity());
CameraStereoVideo(
int deviceLeft,
int deviceRight,
bool rectifyImages = false,
float imageRate = 0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoVideo();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
cv::VideoCapture capture_;
cv::VideoCapture capture2_;
std::string path_;
std::string path2_;
bool rectifyImages_;
StereoCameraModel stereoModel_;
std::string cameraName_;
CameraVideo::Source src_;
int usbDevice_;
int usbDevice2_;
};
} // namespace rtabmap
#include <rtabmap/core/camera/CameraStereoDC1394.h>
#include <rtabmap/core/camera/CameraStereoFlyCapture2.h>
#include <rtabmap/core/camera/CameraStereoImages.h>
#include <rtabmap/core/camera/CameraStereoVideo.h>
#include <rtabmap/core/camera/CameraStereoZed.h>

2
corelib/include/rtabmap/core/Odometry.h
View File

@@ -56,7 +56,7 @@ public:
};
public:
static Odometry * create(const ParametersMap & parameters);
static Odometry * create(const ParametersMap & parameters = ParametersMap());
static Odometry * create(Type & type, const ParametersMap & parameters = ParametersMap());
public:

77
corelib/include/rtabmap/core/camera/CameraFreenect.h Normal file
View File

@@ -0,0 +1,77 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/StereoCameraModel.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Version.h"
typedef struct _freenect_context freenect_context;
typedef struct _freenect_device freenect_device;
namespace rtabmap
{
class FreenectDevice;
class RTABMAP_EXP CameraFreenect :
public Camera
{
public:
static bool available();
enum Type {kTypeColorDepth, kTypeIRDepth};
public:
// default local transform z in, x right, y down));
CameraFreenect(int deviceId= 0,
Type type = kTypeColorDepth,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraFreenect();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_FREENECT
int deviceId_;
Type type_;
freenect_context * ctx_;
FreenectDevice * freenectDevice_;
StereoCameraModel stereoModel_;
#endif
};
} // namespace rtabmap

103
corelib/include/rtabmap/core/camera/CameraFreenect2.h Normal file
View File

@@ -0,0 +1,103 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/StereoCameraModel.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Version.h"
namespace libfreenect2
{
class Freenect2;
class Freenect2Device;
class SyncMultiFrameListener;
class Registration;
class PacketPipeline;
}
namespace rtabmap
{
class RTABMAP_EXP CameraFreenect2 :
public Camera
{
public:
static bool available();
enum Type{
kTypeColor2DepthSD,
kTypeDepth2ColorSD,
kTypeDepth2ColorHD,
kTypeDepth2ColorHD2,
kTypeIRDepth,
kTypeColorIR
};
public:
// default local transform z in, x right, y down));
CameraFreenect2(int deviceId= 0,
Type type = kTypeDepth2ColorSD,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
float minDepth = 0.3f,
float maxDepth = 12.0f,
bool bilateralFiltering = true,
bool edgeAwareFiltering = true,
bool noiseFiltering = true,
const std::string & pipelineName = "");
virtual ~CameraFreenect2();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_FREENECT2
int deviceId_;
Type type_;
StereoCameraModel stereoModel_;
libfreenect2::Freenect2 * freenect2_;
libfreenect2::Freenect2Device *dev_;
libfreenect2::SyncMultiFrameListener * listener_;
libfreenect2::Registration * reg_;
float minKinect2Depth_;
float maxKinect2Depth_;
bool bilateralFiltering_;
bool edgeAwareFiltering_;
bool noiseFiltering_;
std::string pipelineName_;
#endif
};
} // namespace rtabmap

185
corelib/include/rtabmap/core/camera/CameraImages.h Normal file
View File

@@ -0,0 +1,185 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/Camera.h"
#include "rtabmap/utilite/UTimer.h"
#include <list>
class UDirectory;
namespace rtabmap
{
class RTABMAP_EXP CameraImages :
public Camera
{
public:
CameraImages();
CameraImages(
const std::string & path,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraImages();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual bool odomProvided() const { return odometry_.size() > 0; }
std::string getPath() const {return _path;}
unsigned int imagesCount() const;
std::vector<std::string> filenames() const;
bool isImagesRectified() const {return _rectifyImages;}
int getBayerMode() const {return _bayerMode;}
const CameraModel & cameraModel() const {return _model;}
void setPath(const std::string & dir) {_path=dir;}
virtual void setStartIndex(int index) {_startAt = index;} // negative means last
void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
void setBayerMode(int mode) {_bayerMode = mode;} // -1=disabled (default) 0=BayerBG, 1=BayerGB, 2=BayerRG, 3=BayerGR
void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "", bool syncImageRateWithStamps=true)
{
_filenamesAreTimestamps = fileNamesAreStamps;
_timestampsPath=filePath;
_syncImageRateWithStamps = syncImageRateWithStamps;
}
void setScanPath(
const std::string & dir,
int maxScanPts = 0,
int downsampleStep = 1,
float voxelSize = 0.0f,
int normalsK = 0, // compute normals if > 0
float normalsRadius = 0, // compute normals if > 0
const Transform & localTransform=Transform::getIdentity(),
bool forceGroundNormalsUp = false)
{
_scanPath = dir;
_scanLocalTransform = localTransform;
_scanMaxPts = maxScanPts;
_scanDownsampleStep = downsampleStep;
_scanNormalsK = normalsK;
_scanNormalsRadius = normalsRadius;
_scanVoxelSize = voxelSize;
_scanForceGroundNormalsUp = forceGroundNormalsUp;
}
void setDepthFromScan(bool enabled, int fillHoles = 1, bool fillHolesFromBorder = false)
{
_depthFromScan = enabled;
_depthFromScanFillHoles = fillHoles;
_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
}
// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
void setOdometryPath(const std::string & filePath, int format = 0)
{
_odometryPath = filePath;
_odometryFormat = format;
}
// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
void setGroundTruthPath(const std::string & filePath, int format = 0)
{
_groundTruthPath = filePath;
_groundTruthFormat = format;
}
void setMaxPoseTimeDiff(double diff) {_maxPoseTimeDiff = diff;}
double getMaxPoseTimeDiff() const {return _maxPoseTimeDiff;}
void setDepth(bool isDepth, float depthScaleFactor = 1.0f)
{
_isDepth = isDepth;
_depthScaleFactor=depthScaleFactor;
}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
bool readPoses(
std::list<Transform> & outputPoses,
std::list<double> & stamps,
const std::string & filePath,
int format,
double maxTimeDiff) const;
private:
std::string _path;
int _startAt;
// If the list of files in the directory is refreshed
// on each call of takeImage()
bool _refreshDir;
bool _rectifyImages;
int _bayerMode;
bool _isDepth;
float _depthScaleFactor;
int _count;
UDirectory * _dir;
std::string _lastFileName;
int _countScan;
UDirectory * _scanDir;
std::string _lastScanFileName;
std::string _scanPath;
Transform _scanLocalTransform;
int _scanMaxPts;
int _scanDownsampleStep;
float _scanVoxelSize;
int _scanNormalsK;
float _scanNormalsRadius;
bool _scanForceGroundNormalsUp;
bool _depthFromScan;
int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical
bool _depthFromScanFillHolesFromBorder;
bool _filenamesAreTimestamps;
std::string _timestampsPath;
bool _syncImageRateWithStamps;
std::string _odometryPath;
int _odometryFormat;
std::string _groundTruthPath;
int _groundTruthFormat;
double _maxPoseTimeDiff;
std::list<double> _stamps;
std::list<Transform> odometry_;
std::list<Transform> groundTruth_;
CameraModel _model;
UTimer _captureTimer;
double _captureDelay;
};
} // namespace rtabmap

97
corelib/include/rtabmap/core/camera/CameraK4W2.h Normal file
View File

@@ -0,0 +1,97 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/CameraModel.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Version.h"
typedef struct IKinectSensor IKinectSensor;
typedef struct ICoordinateMapper ICoordinateMapper;
typedef struct _DepthSpacePoint DepthSpacePoint;
typedef struct _ColorSpacePoint ColorSpacePoint;
typedef struct tagRGBQUAD RGBQUAD;
typedef struct IMultiSourceFrameReader IMultiSourceFrameReader;
namespace rtabmap
{
class RTABMAP_EXP CameraK4W2 :
public Camera
{
public:
static bool available();
enum Type {
kTypeColor2DepthSD,
kTypeDepth2ColorSD,
kTypeDepth2ColorHD
};
public:
static const int cDepthWidth = 512;
static const int cDepthHeight = 424;
static const int cColorWidth = 1920;
static const int cColorHeight = 1080;
public:
// default local transform z in, x right, y down));
CameraK4W2(int deviceId = 0, // not used
Type type = kTypeDepth2ColorSD,
float imageRate = 0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraK4W2();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
void close();
private:
#ifdef RTABMAP_K4W2
Type type_;
IKinectSensor* pKinectSensor_;
ICoordinateMapper* pCoordinateMapper_;
DepthSpacePoint* pDepthCoordinates_;
ColorSpacePoint* pColorCoordinates_;
IMultiSourceFrameReader* pMultiSourceFrameReader_;
RGBQUAD * pColorRGBX_;
INT_PTR hMSEvent;
CameraModel colorCameraModel_;
#endif
};
} // namespace rtabmap

93
corelib/include/rtabmap/core/camera/CameraOpenNI2.h Normal file
View File

@@ -0,0 +1,93 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/StereoCameraModel.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Version.h"
namespace openni
{
class Device;
class VideoStream;
}
namespace rtabmap
{
class RTABMAP_EXP CameraOpenNI2 :
public Camera
{
public:
static bool available();
static bool exposureGainAvailable();
enum Type {kTypeColorDepth, kTypeIRDepth, kTypeIR};
public:
CameraOpenNI2(const std::string & deviceId = "",
Type type = kTypeColorDepth,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraOpenNI2();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
bool setAutoWhiteBalance(bool enabled);
bool setAutoExposure(bool enabled);
bool setExposure(int value);
bool setGain(int value);
bool setMirroring(bool enabled);
void setOpenNI2StampsAndIDsUsed(bool used);
void setIRDepthShift(int horizontal, int vertical);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_OPENNI2
Type _type;
openni::Device * _device;
openni::VideoStream * _color;
openni::VideoStream * _depth;
float _depthFx;
float _depthFy;
std::string _deviceId;
bool _openNI2StampsAndIDsUsed;
StereoCameraModel _stereoModel;
int _depthHShift;
int _depthVShift;
#endif
};
} // namespace rtabmap

64
corelib/include/rtabmap/core/camera/CameraOpenNICV.h Normal file
View File

@@ -0,0 +1,64 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Version.h"
namespace rtabmap
{
class RTABMAP_EXP CameraOpenNICV :
public Camera
{
public:
static bool available();
public:
CameraOpenNICV(bool asus = false,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraOpenNICV();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const {return "";} // unknown with OpenCV
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
bool _asus;
cv::VideoCapture _capture;
float _depthFocal;
};
} // namespace rtabmap

96
corelib/include/rtabmap/core/camera/CameraOpenni.h Normal file
View File

@@ -0,0 +1,96 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/utilite/UMutex.h"
#include "rtabmap/utilite/USemaphore.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Version.h"
#include <pcl/pcl_config.h>
#ifdef HAVE_OPENNI
#if __linux__ && __i386__ && __cplusplus >= 201103L
#warning "Openni driver is not available on i386 when building with c++11 support"
#else
#define RTABMAP_OPENNI
#include <pcl/io/openni_camera/openni_depth_image.h>
#include <pcl/io/openni_camera/openni_image.h>
#endif
#endif
#include <boost/signals2/connection.hpp>
namespace pcl
{
class Grabber;
}
namespace rtabmap
{
class RTABMAP_EXP CameraOpenni :
public Camera
{
public:
static bool available();
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();
#ifdef RTABMAP_OPENNI
void image_cb (
const boost::shared_ptr<openni_wrapper::Image>& rgb,
const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
float constant);
#endif
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
pcl::Grabber* interface_;
std::string deviceId_;
boost::signals2::connection connection_;
cv::Mat depth_;
cv::Mat rgb_;
float depthConstant_;
UMutex dataMutex_;
USemaphore dataReady_;
};
} // namespace rtabmap

63
corelib/include/rtabmap/core/camera/CameraRGBDImages.h Normal file
View File

@@ -0,0 +1,63 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <rtabmap/core/camera/CameraImages.h>
namespace rtabmap
{
class RTABMAP_EXP CameraRGBDImages :
public CameraImages
{
public:
static bool available();
public:
CameraRGBDImages(
const std::string & pathRGBImages,
const std::string & pathDepthImages,
float depthScaleFactor = 1.0f,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraRGBDImages();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);cameraDepth_.setStartIndex(index);} // negative means last
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
CameraImages cameraDepth_;
};
} // namespace rtabmap

104
corelib/include/rtabmap/core/camera/CameraRealSense.h Normal file
View File

@@ -0,0 +1,104 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/utilite/UMutex.h"
#include "rtabmap/utilite/USemaphore.h"
#include "rtabmap/core/CameraModel.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Version.h"
namespace rs
{
class context;
class device;
namespace slam {
class slam;
}
}
namespace rtabmap
{
class slam_event_handler;
class RTABMAP_EXP CameraRealSense :
public Camera
{
public:
static bool available();
enum RGBSource {kColor, kInfrared, kFishEye};
public:
// default local transform z in, x right, y down));
CameraRealSense(
int deviceId = 0,
int presetRGB = 0, // 0=best quality, 1=largest image, 2=highest framerate
int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate
bool computeOdometry = false,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraRealSense();
void setDepthScaledToRGBSize(bool enabled);
void setRGBSource(RGBSource source);
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual bool odomProvided() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_REALSENSE
rs::context * ctx_;
rs::device * dev_;
int deviceId_;
int presetRGB_;
int presetDepth_;
bool computeOdometry_;
bool depthScaledToRGBSize_;
RGBSource rgbSource_;
CameraModel cameraModel_;
std::vector<int> rsRectificationTable_;
int motionSeq_[2];
rs::slam::slam * slam_;
UMutex slamLock_;
std::map<double, std::pair<cv::Mat, cv::Mat> > bufferedFrames_;
std::pair<cv::Mat, cv::Mat> lastSyncFrames_;
UMutex dataMutex_;
USemaphore dataReady_;
#endif
};
} // namespace rtabmap

96
corelib/include/rtabmap/core/camera/CameraRealSense2.h Normal file
View File

@@ -0,0 +1,96 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/CameraModel.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Version.h"
#include <pcl/pcl_config.h>
namespace rs2
{
class context;
class device;
class syncer;
}
struct rs2_intrinsics;
struct rs2_extrinsics;
namespace rtabmap
{
class RTABMAP_EXP CameraRealSense2 :
public Camera
{
public:
static bool available();
public:
// default local transform z in, x right, y down));
CameraRealSense2(
const std::string & deviceId = "",
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraRealSense2();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
// parameters are set during initialization
void setEmitterEnabled(bool enabled);
void setIRDepthFormat(bool enabled);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_REALSENSE2
rs2::context * ctx_;
rs2::device * dev_;
std::string deviceId_;
rs2::syncer * syncer_;
float depth_scale_meters_;
rs2_intrinsics * depthIntrinsics_;
rs2_intrinsics * rgbIntrinsics_;
rs2_extrinsics * depthToRGBExtrinsics_;
cv::Mat depthBuffer_;
cv::Mat rgbBuffer_;
CameraModel model_;
bool emitterEnabled_;
bool irDepth_;
#endif
};
} // namespace rtabmap

66
corelib/include/rtabmap/core/camera/CameraStereoDC1394.h Normal file
View File

@@ -0,0 +1,66 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/StereoCameraModel.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Version.h"
namespace rtabmap
{
class DC1394Device;
class RTABMAP_EXP CameraStereoDC1394 :
public Camera
{
public:
static bool available();
public:
CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoDC1394();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_DC1394
DC1394Device *device_;
StereoCameraModel stereoModel_;
#endif
};
} // namespace rtabmap

68
corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h Normal file
View File

@@ -0,0 +1,68 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Version.h"
namespace FlyCapture2
{
class Camera;
}
namespace rtabmap
{
class RTABMAP_EXP CameraStereoFlyCapture2 :
public Camera
{
public:
static bool available();
public:
CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoFlyCapture2();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_FLYCAPTURE2
FlyCapture2::Camera * camera_;
void * triclopsCtx_; // TriclopsContext
#endif
};
} // namespace rtabmap

75
corelib/include/rtabmap/core/camera/CameraStereoImages.h Normal file
View File

@@ -0,0 +1,75 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <rtabmap/core/camera/CameraImages.h>
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/StereoCameraModel.h"
#include "rtabmap/core/Version.h"
namespace rtabmap
{
class CameraImages;
class RTABMAP_EXP CameraStereoImages :
public CameraImages
{
public:
static bool available();
public:
CameraStereoImages(
const std::string & pathLeftImages,
const std::string & pathRightImages,
bool rectifyImages = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
CameraStereoImages(
const std::string & pathLeftRightImages,
bool rectifyImages = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoImages();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);camera2_->setStartIndex(index);} // negative means last
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
CameraImages * camera2_;
StereoCameraModel stereoModel_;
};
} // namespace rtabmap

89
corelib/include/rtabmap/core/camera/CameraStereoVideo.h Normal file
View File

@@ -0,0 +1,89 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/StereoCameraModel.h"
#include "rtabmap/core/camera/CameraVideo.h"
namespace rtabmap
{
class RTABMAP_EXP CameraStereoVideo :
public Camera
{
public:
static bool available();
public:
CameraStereoVideo(
const std::string & pathSideBySide,
bool rectifyImages = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
CameraStereoVideo(
const std::string & pathLeft,
const std::string & pathRight,
bool rectifyImages = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
CameraStereoVideo(
int device,
bool rectifyImages = false,
float imageRate = 0.0f,
const Transform & localTransform = Transform::getIdentity());
CameraStereoVideo(
int deviceLeft,
int deviceRight,
bool rectifyImages = false,
float imageRate = 0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoVideo();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
cv::VideoCapture capture_;
cv::VideoCapture capture2_;
std::string path_;
std::string path2_;
bool rectifyImages_;
StereoCameraModel stereoModel_;
std::string cameraName_;
CameraVideo::Source src_;
int usbDevice_;
int usbDevice2_;
};
} // namespace rtabmap

98
corelib/include/rtabmap/core/camera/CameraStereoZed.h Normal file
View File

@@ -0,0 +1,98 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/StereoCameraModel.h"
#include "rtabmap/core/camera/CameraVideo.h"
#include "rtabmap/core/Version.h"
namespace sl
{
class Camera;
}
namespace rtabmap
{
class RTABMAP_EXP CameraStereoZed :
public Camera
{
public:
static bool available();
public:
CameraStereoZed(
int deviceId,
int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY
int sensingMode = 0,// 0=STANDARD, 1=FILL
int confidenceThr = 100,
bool computeOdometry = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
bool selfCalibration = true);
CameraStereoZed(
const std::string & svoFilePath,
int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY
int sensingMode = 0,// 0=STANDARD, 1=FILL
int confidenceThr = 100,
bool computeOdometry = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
bool selfCalibration = true);
virtual ~CameraStereoZed();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual bool odomProvided() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
#ifdef RTABMAP_ZED
sl::Camera * zed_;
StereoCameraModel stereoModel_;
CameraVideo::Source src_;
int usbDevice_;
std::string svoFilePath_;
int resolution_;
int quality_;
bool selfCalibration_;
int sensingMode_;
int confidenceThr_;
bool computeOdometry_;
bool lost_;
#endif
};
} // namespace rtabmap

80
corelib/include/rtabmap/core/camera/CameraVideo.h Normal file
View File

@@ -0,0 +1,80 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/Camera.h"
namespace rtabmap
{
class RTABMAP_EXP CameraVideo :
public Camera
{
public:
enum Source{kVideoFile, kUsbDevice};
public:
CameraVideo(int usbDevice = 0,
bool rectifyImages = false,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
CameraVideo(const std::string & filePath,
bool rectifyImages = false,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraVideo();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
int getUsbDevice() const {return _usbDevice;}
const std::string & getFilePath() const {return _filePath;}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
private:
// File type
std::string _filePath;
bool _rectifyImages;
cv::VideoCapture _capture;
Source _src;
// Usb camera
int _usbDevice;
std::string _guid;
CameraModel _model;
};
} // namespace rtabmap

0
corelib/include/rtabmap/core/OdometryDVO.h → corelib/include/rtabmap/core/odometry/OdometryDVO.h
View File

0
corelib/include/rtabmap/core/OdometryF2F.h → corelib/include/rtabmap/core/odometry/OdometryF2F.h
View File

0
corelib/include/rtabmap/core/OdometryF2M.h → corelib/include/rtabmap/core/odometry/OdometryF2M.h
View File

0
corelib/include/rtabmap/core/OdometryFovis.h → corelib/include/rtabmap/core/odometry/OdometryFovis.h
View File

0
corelib/include/rtabmap/core/OdometryLOAM.h → corelib/include/rtabmap/core/odometry/OdometryLOAM.h
View File

0
corelib/include/rtabmap/core/OdometryMSCKF.h → corelib/include/rtabmap/core/odometry/OdometryMSCKF.h
View File

0
corelib/include/rtabmap/core/OdometryMono.h → corelib/include/rtabmap/core/odometry/OdometryMono.h
View File

0
corelib/include/rtabmap/core/OdometryORBSLAM2.h → corelib/include/rtabmap/core/odometry/OdometryORBSLAM2.h
View File

0
corelib/include/rtabmap/core/OdometryOkvis.h → corelib/include/rtabmap/core/odometry/OdometryOkvis.h
View File

0
corelib/include/rtabmap/core/OdometryViso2.h → corelib/include/rtabmap/core/odometry/OdometryViso2.h
View File

0
corelib/include/rtabmap/core/OptimizerCVSBA.h → corelib/include/rtabmap/core/optimizer/OptimizerCVSBA.h
View File

0
corelib/include/rtabmap/core/OptimizerG2O.h → corelib/include/rtabmap/core/optimizer/OptimizerG2O.h
View File

0
corelib/include/rtabmap/core/OptimizerGTSAM.h → corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h
View File

0
corelib/include/rtabmap/core/OptimizerTORO.h → corelib/include/rtabmap/core/optimizer/OptimizerTORO.h
View File

68
corelib/src/CMakeLists.txt
View File

@@ -15,11 +15,25 @@ SET(SRC_FILES
Camera.cpp
CameraThread.cpp
CameraRGB.cpp
CameraRGBD.cpp
CameraStereo.cpp
CameraModel.cpp
camera/CameraFreenect.cpp
camera/CameraFreenect2.cpp
camera/CameraImages.cpp
camera/CameraK4W2.cpp
camera/CameraOpenni.cpp
camera/CameraOpenNI2.cpp
camera/CameraOpenNICV.cpp
camera/CameraRealSense.cpp
camera/CameraRealSense2.cpp
camera/CameraRGBDImages.cpp
camera/CameraStereoDC1394.cpp
camera/CameraStereoFlyCapture2.cpp
camera/CameraStereoImages.cpp
camera/CameraStereoVideo.cpp
camera/CameraStereoZed.cpp
camera/CameraVideo.cpp
EpipolarGeometry.cpp
VisualWord.cpp
VWDictionary.cpp
@@ -49,10 +63,10 @@ SET(SRC_FILES
LaserScan.cpp
Optimizer.cpp
OptimizerTORO.cpp
OptimizerG2O.cpp
OptimizerGTSAM.cpp
OptimizerCVSBA.cpp
optimizer/OptimizerTORO.cpp
optimizer/OptimizerG2O.cpp
optimizer/OptimizerGTSAM.cpp
optimizer/OptimizerCVSBA.cpp
Registration.cpp
RegistrationIcp.cpp
@@ -60,16 +74,16 @@ SET(SRC_FILES
Odometry.cpp
OdometryThread.cpp
OdometryF2M.cpp
OdometryMono.cpp
OdometryF2F.cpp
OdometryFovis.cpp
OdometryViso2.cpp
OdometryDVO.cpp
OdometryOkvis.cpp
OdometryORBSLAM2.cpp
OdometryLOAM.cpp
OdometryMSCKF.cpp
odometry/OdometryF2M.cpp
odometry/OdometryMono.cpp
odometry/OdometryF2F.cpp
odometry/OdometryFovis.cpp
odometry/OdometryViso2.cpp
odometry/OdometryDVO.cpp
odometry/OdometryOkvis.cpp
odometry/OdometryORBSLAM2.cpp
odometry/OdometryLOAM.cpp
odometry/OdometryMSCKF.cpp
IMUThread.cpp
@@ -248,12 +262,12 @@ ENDIF(FlyCapture2_FOUND)
IF(WITH_TORO)
SET(SRC_FILES
${SRC_FILES}
toro3d/posegraph3.cpp
toro3d/treeoptimizer3_iteration.cpp
toro3d/treeoptimizer3.cpp
optimizer/toro3d/posegraph3.cpp
optimizer/toro3d/treeoptimizer3_iteration.cpp
optimizer/toro3d/treeoptimizer3.cpp
toro3d/posegraph2.cpp
toro3d/treeoptimizer2.cpp
optimizer/toro3d/posegraph2.cpp
optimizer/toro3d/treeoptimizer2.cpp
)
ENDIF(WITH_TORO)
@@ -270,11 +284,11 @@ IF(G2O_FOUND)
IF(WITH_VERTIGO)
SET(SRC_FILES
${SRC_FILES}
vertigo/g2o/edge_se2Switchable.cpp
vertigo/g2o/edge_se3Switchable.cpp
vertigo/g2o/edge_switchPrior.cpp
vertigo/g2o/types_g2o_robust.cpp
vertigo/g2o/vertex_switchLinear.cpp
optimizer/vertigo/g2o/edge_se2Switchable.cpp
optimizer/vertigo/g2o/edge_se3Switchable.cpp
optimizer/vertigo/g2o/edge_switchPrior.cpp
optimizer/vertigo/g2o/types_g2o_robust.cpp
optimizer/vertigo/g2o/vertex_switchLinear.cpp
)
ENDIF(WITH_VERTIGO)
ENDIF(G2O_FOUND)

4109
corelib/src/CameraRGBD.cpp
View File

File diff suppressed because it is too large Load Diff

1514
corelib/src/CameraStereo.cpp
View File

File diff suppressed because it is too large Load Diff

4
corelib/src/Graph.cpp
View File

@@ -43,8 +43,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <queue>
#include <fstream>
#include <rtabmap/core/OptimizerTORO.h>
#include <rtabmap/core/OptimizerG2O.h>
#include <rtabmap/core/optimizer/OptimizerTORO.h>
#include <rtabmap/core/optimizer/OptimizerG2O.h>
namespace rtabmap {

2
corelib/src/Memory.cpp
View File

@@ -57,7 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Compression.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/Stereo.h"
#include "rtabmap/core/OptimizerG2O.h"
#include "rtabmap/core/optimizer/OptimizerG2O.h"
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <rtabmap/core/OccupancyGrid.h>

18
corelib/src/Odometry.cpp
View File

@@ -25,16 +25,16 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/OdometryF2M.h>
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/OdometryF2F.h"
#include "rtabmap/core/OdometryFovis.h"
#include "rtabmap/core/OdometryViso2.h"
#include "rtabmap/core/OdometryDVO.h"
#include "rtabmap/core/OdometryOkvis.h"
#include "rtabmap/core/OdometryORBSLAM2.h"
#include "rtabmap/core/OdometryLOAM.h"
#include "rtabmap/core/OdometryMSCKF.h"
#include <rtabmap/core/odometry/OdometryF2M.h>
#include "rtabmap/core/odometry/OdometryF2F.h"
#include "rtabmap/core/odometry/OdometryFovis.h"
#include "rtabmap/core/odometry/OdometryViso2.h"
#include "rtabmap/core/odometry/OdometryDVO.h"
#include "rtabmap/core/odometry/OdometryOkvis.h"
#include "rtabmap/core/odometry/OdometryORBSLAM2.h"
#include "rtabmap/core/odometry/OdometryLOAM.h"
#include "rtabmap/core/odometry/OdometryMSCKF.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_mapping.h"

3
corelib/src/OdometryThread.cpp
View File

@@ -25,10 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/OdometryF2M.h>
#include "rtabmap/core/OdometryThread.h"
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/OdometryMono.h"
#include "rtabmap/core/odometry/OdometryMono.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/OdometryEvent.h"

8
corelib/src/Optimizer.cpp
View File

@@ -36,10 +36,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <set>
#include <queue>
#include <rtabmap/core/OptimizerTORO.h>
#include <rtabmap/core/OptimizerG2O.h>
#include <rtabmap/core/OptimizerGTSAM.h>
#include <rtabmap/core/OptimizerCVSBA.h>
#include <rtabmap/core/optimizer/OptimizerTORO.h>
#include <rtabmap/core/optimizer/OptimizerG2O.h>
#include <rtabmap/core/optimizer/OptimizerGTSAM.h>
#include <rtabmap/core/optimizer/OptimizerCVSBA.h>
namespace rtabmap {

477
corelib/src/camera/CameraFreenect.cpp Normal file
View File

@@ -0,0 +1,477 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraFreenect.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/core/util2d.h>
#ifdef RTABMAP_FREENECT
#include <libfreenect.h>
#ifdef FREENECT_DASH_INCLUDES
#include <libfreenect-registration.h>
#else
#include <libfreenect_registration.h>
#endif
#endif
namespace rtabmap
{
#ifdef RTABMAP_FREENECT
class FreenectDevice : public UThread {
public:
FreenectDevice(freenect_context * ctx, int index, bool color = true, bool registered = true) :
index_(index),
color_(color),
registered_(registered),
ctx_(ctx),
device_(0),
depthFocal_(0.0f)
{
UASSERT(ctx_ != 0);
}
virtual ~FreenectDevice()
{
this->join(true);
if(device_ && freenect_close_device(device_) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion");
}
const std::string & getSerial() const {return serial_;}
bool init()
{
if(device_)
{
this->join(true);
freenect_close_device(device_);
device_ = 0;
}
serial_.clear();
std::vector<std::string> deviceSerials;
freenect_device_attributes* attr_list;
freenect_device_attributes* item;
freenect_list_device_attributes(ctx_, &attr_list);
for (item = attr_list; item != NULL; item = item->next) {
deviceSerials.push_back(std::string(item->camera_serial));
}
freenect_free_device_attributes(attr_list);
if(freenect_open_device(ctx_, &device_, index_) < 0)
{
UERROR("FreenectDevice: Cannot open Kinect");
return false;
}
if(index_ >= 0 && index_ < (int)deviceSerials.size())
{
serial_ = deviceSerials[index_];
}
else
{
UERROR("Could not get serial for index %d", index_);
}
UINFO("color=%d registered=%d", color_?1:0, registered_?1:0);
freenect_set_user(device_, this);
freenect_frame_mode videoMode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, color_?FREENECT_VIDEO_RGB:FREENECT_VIDEO_IR_8BIT);
freenect_frame_mode depthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, color_ && registered_?FREENECT_DEPTH_REGISTERED:FREENECT_DEPTH_MM);
if(!videoMode.is_valid)
{
UERROR("Freenect: video mode selected not valid!");
return false;
}
if(!depthMode.is_valid)
{
UERROR("Freenect: depth mode selected not valid!");
return false;
}
UASSERT(videoMode.data_bits_per_pixel == 8 || videoMode.data_bits_per_pixel == 24);
UASSERT(depthMode.data_bits_per_pixel == 16);
freenect_set_video_mode(device_, videoMode);
freenect_set_depth_mode(device_, depthMode);
rgbIrBuffer_ = cv::Mat(cv::Size(videoMode.width,videoMode.height), color_?CV_8UC3:CV_8UC1);
depthBuffer_ = cv::Mat(cv::Size(depthMode.width,depthMode.height), CV_16UC1);
freenect_set_depth_buffer(device_, depthBuffer_.data);
freenect_set_video_buffer(device_, rgbIrBuffer_.data);
freenect_set_depth_callback(device_, freenect_depth_callback);
freenect_set_video_callback(device_, freenect_video_callback);
float rgb_focal_length_sxga = 1050.0f;
float width_sxga = 1280.0f;
float width = freenect_get_current_depth_mode(device_).width;
float scale = width / width_sxga;
if(color_ && registered_)
{
depthFocal_ = rgb_focal_length_sxga * scale;
}
else
{
freenect_registration reg = freenect_copy_registration(device_);
float depth_focal_length_sxga = reg.zero_plane_info.reference_distance / reg.zero_plane_info.reference_pixel_size;
freenect_destroy_registration(&reg);
depthFocal_ = depth_focal_length_sxga * scale;
}
UINFO("FreenectDevice: Depth focal = %f", depthFocal_);
return true;
}
float getDepthFocal() const {return depthFocal_;}
void getData(cv::Mat & rgb, cv::Mat & depth)
{
if(this->isRunning())
{
if(!dataReady_.acquire(1, 5000))
{
UERROR("Not received any frames since 5 seconds, try to restart the camera again.");
}
else
{
UScopeMutex s(dataMutex_);
rgb = rgbIrLastFrame_;
depth = depthLastFrame_;
rgbIrLastFrame_ = cv::Mat();
depthLastFrame_= cv::Mat();
}
}
}
private:
// Do not call directly even in child
void VideoCallback(void* rgb)
{
UASSERT(rgbIrBuffer_.data == rgb);
UScopeMutex s(dataMutex_);
bool notify = rgbIrLastFrame_.empty();
if(color_)
{
cv::cvtColor(rgbIrBuffer_, rgbIrLastFrame_, CV_RGB2BGR);
}
else // IrDepth
{
rgbIrLastFrame_ = rgbIrBuffer_.clone();
}
if(!depthLastFrame_.empty() && notify)
{
dataReady_.release();
}
}
// Do not call directly even in child
void DepthCallback(void* depth)
{
UASSERT(depthBuffer_.data == depth);
UScopeMutex s(dataMutex_);
bool notify = depthLastFrame_.empty();
depthLastFrame_ = depthBuffer_.clone();
if(!rgbIrLastFrame_.empty() && notify)
{
dataReady_.release();
}
}
void startVideo() {
if(device_ && freenect_start_video(device_) < 0) UERROR("Cannot start RGB callback");
}
void stopVideo() {
if(device_ && freenect_stop_video(device_) < 0) UERROR("Cannot stop RGB callback");
}
void startDepth() {
if(device_ && freenect_start_depth(device_) < 0) UERROR("Cannot start depth callback");
}
void stopDepth() {
if(device_ && freenect_stop_depth(device_) < 0) UERROR("Cannot stop depth callback");
}
virtual void mainLoopBegin()
{
this->startDepth();
this->startVideo();
}
virtual void mainLoop()
{
timeval t;
t.tv_sec = 0;
t.tv_usec = 10000;
if(freenect_process_events_timeout(ctx_, &t) < 0)
{
UERROR("FreenectDevice: Cannot process freenect events");
this->kill();
}
}
virtual void mainLoopEnd()
{
this->stopDepth();
this->stopVideo();
dataReady_.release();
}
static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {
FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
device->DepthCallback(depth);
}
static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {
FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
device->VideoCallback(video);
}
//noncopyable
FreenectDevice( const FreenectDevice& );
const FreenectDevice& operator=( const FreenectDevice& );
private:
int index_;
bool color_;
bool registered_;
std::string serial_;
freenect_context * ctx_;
freenect_device * device_;
cv::Mat depthBuffer_;
cv::Mat rgbIrBuffer_;
UMutex dataMutex_;
cv::Mat depthLastFrame_;
cv::Mat rgbIrLastFrame_;
float depthFocal_;
USemaphore dataReady_;
};
#endif
//
// CameraFreenect
//
bool CameraFreenect::available()
{
#ifdef RTABMAP_FREENECT
return true;
#else
return false;
#endif
}
CameraFreenect::CameraFreenect(int deviceId, Type type, float imageRate, const Transform & localTransform) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_FREENECT
,
deviceId_(deviceId),
type_(type),
ctx_(0),
freenectDevice_(0)
#endif
{
#ifdef RTABMAP_FREENECT
if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
// claim camera
freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_CAMERA));
#endif
}
CameraFreenect::~CameraFreenect()
{
#ifdef RTABMAP_FREENECT
if(freenectDevice_)
{
freenectDevice_->join(true);
delete freenectDevice_;
freenectDevice_ = 0;
}
if(ctx_)
{
if(freenect_shutdown(ctx_) < 0){} //FN_WARNING("Freenect did not shutdown in a clean fashion");
}
#endif
}
bool CameraFreenect::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef RTABMAP_FREENECT
if(freenectDevice_)
{
freenectDevice_->join(true);
delete freenectDevice_;
freenectDevice_ = 0;
}
if(ctx_ && freenect_num_devices(ctx_) > 0)
{
// look for calibration files
bool hardwareRegistration = true;
stereoModel_ = StereoCameraModel();
if(!calibrationFolder.empty())
{
// we need the serial, HACK: init a temp device to get it
FreenectDevice dev(ctx_, deviceId_);
if(!dev.init())
{
UERROR("CameraFreenect: Init failed!");
}
std::string calibrationName = dev.getSerial();
if(!cameraName.empty())
{
calibrationName = cameraName;
}
stereoModel_.setName(calibrationName, "depth", "rgb");
hardwareRegistration = !stereoModel_.load(calibrationFolder, calibrationName, false);
if(type_ == kTypeIRDepth)
{
hardwareRegistration = false;
}
if((type_ == kTypeIRDepth && !stereoModel_.left().isValidForRectification()) ||
(type_ == kTypeColorDepth && !stereoModel_.right().isValidForRectification()))
{
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
calibrationName.c_str(), calibrationFolder.c_str());
}
else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && hardwareRegistration)
{
UWARN("Missing extrinsic calibration file for camera \"%s\" in \"%s\" folder, default registration is used even if rgb is rectified!",
calibrationName.c_str(), calibrationFolder.c_str());
}
else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && !hardwareRegistration)
{
UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
"factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
}
}
freenectDevice_ = new FreenectDevice(ctx_, deviceId_, type_==kTypeColorDepth, hardwareRegistration);
if(freenectDevice_->init())
{
freenectDevice_->start();
uSleep(3000);
return true;
}
else
{
UERROR("CameraFreenect: Init failed!");
}
delete freenectDevice_;
freenectDevice_ = 0;
}
else
{
UERROR("CameraFreenect: No devices connected!");
}
#else
UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
#endif
return false;
}
bool CameraFreenect::isCalibrated() const
{
return true;
}
std::string CameraFreenect::getSerial() const
{
#ifdef RTABMAP_FREENECT
if(freenectDevice_)
{
return freenectDevice_->getSerial();
}
#endif
return "";
}
SensorData CameraFreenect::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_FREENECT
if(ctx_ && freenectDevice_)
{
if(freenectDevice_->isRunning())
{
cv::Mat depth,rgb;
freenectDevice_->getData(rgb, depth);
if(!rgb.empty() && !depth.empty())
{
UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
// default calibration
CameraModel model(
freenectDevice_->getDepthFocal(), //fx
freenectDevice_->getDepthFocal(), //fy
float(rgb.cols/2) - 0.5f, //cx
float(rgb.rows/2) - 0.5f, //cy
this->getLocalTransform(),
0,
rgb.size());
if(type_==kTypeIRDepth)
{
if(stereoModel_.left().isValidForRectification())
{
rgb = stereoModel_.left().rectifyImage(rgb);
depth = stereoModel_.left().rectifyImage(depth, 0);
model = stereoModel_.left();
}
}
else
{
if(stereoModel_.right().isValidForRectification())
{
rgb = stereoModel_.right().rectifyImage(rgb);
model = stereoModel_.right();
if(stereoModel_.left().isValidForRectification() && !stereoModel_.stereoTransform().isNull())
{
depth = stereoModel_.left().rectifyImage(depth, 0);
depth = util2d::registerDepth(depth, stereoModel_.left().K(), rgb.size(), stereoModel_.right().K(), stereoModel_.stereoTransform());
}
}
}
model.setLocalTransform(this->getLocalTransform());
data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
}
}
else
{
UERROR("CameraFreenect: Re-initialization needed!");
delete freenectDevice_;
freenectDevice_ = 0;
}
}
#else
UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
#endif
return data;
}
} // namespace rtabmap

730
corelib/src/camera/CameraFreenect2.cpp Normal file
View File

@@ -0,0 +1,730 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraFreenect2.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/core/util2d.h>
#ifdef RTABMAP_FREENECT2
#include <libfreenect2/libfreenect2.hpp>
#include <libfreenect2/frame_listener_impl.h>
#include <libfreenect2/registration.h>
#include <libfreenect2/packet_pipeline.h>
#include <libfreenect2/config.h>
#endif
namespace rtabmap
{
bool CameraFreenect2::available()
{
#ifdef RTABMAP_FREENECT2
return true;
#else
return false;
#endif
}
CameraFreenect2::CameraFreenect2(
int deviceId,
Type type,
float imageRate,
const Transform & localTransform,
float minDepth,
float maxDepth,
bool bilateralFiltering,
bool edgeAwareFiltering,
bool noiseFiltering,
const std::string & pipelineName) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_FREENECT2
,
deviceId_(deviceId),
type_(type),
freenect2_(0),
dev_(0),
listener_(0),
reg_(0),
minKinect2Depth_(minDepth),
maxKinect2Depth_(maxDepth),
bilateralFiltering_(bilateralFiltering),
edgeAwareFiltering_(edgeAwareFiltering),
noiseFiltering_(noiseFiltering),
pipelineName_(pipelineName)
#endif
{
#ifdef RTABMAP_FREENECT2
UASSERT(minKinect2Depth_ < maxKinect2Depth_ && minKinect2Depth_>0 && maxKinect2Depth_>0 && maxKinect2Depth_<=65.535f);
freenect2_ = new libfreenect2::Freenect2();
switch(type_)
{
case kTypeColorIR:
listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
break;
case kTypeIRDepth:
listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
break;
case kTypeColor2DepthSD:
case kTypeDepth2ColorHD:
case kTypeDepth2ColorSD:
default:
listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
break;
}
#endif
}
CameraFreenect2::~CameraFreenect2()
{
#ifdef RTABMAP_FREENECT2
UDEBUG("");
if(dev_)
{
dev_->stop();
dev_->close();
//deleted in freenect2_ destructor (Freeenect2Impl::clearDevices())
}
delete listener_;
delete reg_;
delete freenect2_;
UDEBUG("");
#endif
}
#ifdef RTABMAP_FREENECT2
libfreenect2::PacketPipeline *createPacketPipelineByName(const std::string & name)
{
std::string availablePipelines;
#if defined(LIBFREENECT2_WITH_OPENGL_SUPPORT)
availablePipelines += "gl ";
if (name == "gl")
{
UINFO("Using 'gl' pipeline.");
return new libfreenect2::OpenGLPacketPipeline();
}
#endif
#if defined(LIBFREENECT2_WITH_CUDA_SUPPORT)
availablePipelines += "cuda cudakde ";
if (name == "cuda")
{
UINFO("Using 'cuda' pipeline.");
return new libfreenect2::CudaPacketPipeline();
}
if (name == "cudakde")
{
UINFO("Using 'cudakde' pipeline.");
return new libfreenect2::CudaKdePacketPipeline();
}
#endif
#if defined(LIBFREENECT2_WITH_OPENCL_SUPPORT)
availablePipelines += "cl clkde ";
if (name == "cl")
{
UINFO("Using 'cl' pipeline.");
return new libfreenect2::OpenCLPacketPipeline();
}
if (name == "clkde")
{
UINFO("Using 'clkde' pipeline.");
return new libfreenect2::OpenCLKdePacketPipeline();
}
#endif
availablePipelines += "cpu";
if (name == "cpu")
{
UINFO("Using 'cpu' pipeline.");
return new libfreenect2::CpuPacketPipeline();
}
if (!name.empty())
{
UERROR("'%s' pipeline is not available. Available pipelines are: \"%s\". Default one is used instead (first one in the list).",
name.c_str(), availablePipelines.c_str());
}
// create default pipeline
#if defined(LIBFREENECT2_WITH_OPENGL_SUPPORT)
UINFO("Using 'gl' pipeline.");
return new libfreenect2::OpenGLPacketPipeline();
#elif defined(LIBFREENECT2_WITH_CUDA_SUPPORT)
UINFO("Using 'cuda' pipeline.");
return new libfreenect2::CudaPacketPipeline();
#elif defined(LIBFREENECT2_WITH_OPENCL_SUPPORT)
UINFO("Using 'cl' pipeline.");
return new libfreenect2::OpenCLPacketPipeline();
#else
UINFO("Using 'cpu' pipeline.");
return new libfreenect2::CpuPacketPipeline();
#endif
}
#endif
bool CameraFreenect2::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef RTABMAP_FREENECT2
if(dev_)
{
dev_->stop();
dev_->close();
dev_ = 0; //deleted in freenect2_ destructor (Freeenect2Impl::clearDevices())
}
if(reg_)
{
delete reg_;
reg_ = 0;
}
libfreenect2::PacketPipeline * pipeline = createPacketPipelineByName(pipelineName_);
if(deviceId_ <= 0)
{
UDEBUG("Opening default device...");
dev_ = freenect2_->openDefaultDevice(pipeline);
pipeline = 0;// pipeline deleted in dev_ (Freenect2DeviceImpl::~Freenect2DeviceImpl())
}
else
{
UDEBUG("Opening device ID=%d...", deviceId_);
dev_ = freenect2_->openDevice(deviceId_, pipeline);
pipeline = 0;// pipeline deleted in dev_ (Freenect2DeviceImpl::~Freenect2DeviceImpl())
}
if(dev_)
{
//default
//MinDepth(0.5f),
//MaxDepth(4.5f),
//EnableBilateralFilter(true),
//EnableEdgeAwareFilter(true)
libfreenect2::Freenect2Device::Config config;
config.EnableBilateralFilter = bilateralFiltering_;
config.EnableEdgeAwareFilter = edgeAwareFiltering_;
config.MinDepth = minKinect2Depth_;
config.MaxDepth = maxKinect2Depth_;
dev_->setConfiguration(config);
dev_->setColorFrameListener(listener_);
dev_->setIrAndDepthFrameListener(listener_);
dev_->start();
UINFO("CameraFreenect2: device serial: %s", dev_->getSerialNumber().c_str());
UINFO("CameraFreenect2: device firmware: %s", dev_->getFirmwareVersion().c_str());
//default registration params
libfreenect2::Freenect2Device::IrCameraParams depthParams = dev_->getIrCameraParams();
libfreenect2::Freenect2Device::ColorCameraParams colorParams = dev_->getColorCameraParams();
reg_ = new libfreenect2::Registration(depthParams, colorParams);
// look for calibration files
stereoModel_ = StereoCameraModel();
if(!calibrationFolder.empty())
{
std::string calibrationName = dev_->getSerialNumber();
if(!cameraName.empty())
{
calibrationName = cameraName;
}
stereoModel_.setName(calibrationName, "depth", "rgb");
if(!stereoModel_.load(calibrationFolder, calibrationName, false))
{
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration "
"is used. Note that from version 0.11.10, calibration suffixes for Freenect2 driver have "
"changed from \"_left\"->\"_depth\" and \"_right\"->\"_rgb\". You can safely rename "
"the calibration files to avoid recalibrating.",
calibrationName.c_str(), calibrationFolder.c_str());
}
else
{
UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
"factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
if(type_==kTypeColor2DepthSD)
{
UWARN("Freenect2: When using custom calibration file, type "
"kTypeColor2DepthSD is not supported. kTypeDepth2ColorSD is used instead...");
type_ = kTypeDepth2ColorSD;
}
// downscale color image by 2
cv::Mat colorP = stereoModel_.right().P();
cv::Size colorSize = stereoModel_.right().imageSize();
if(type_ == kTypeDepth2ColorSD)
{
colorP.at<double>(0,0)/=2.0f; //fx
colorP.at<double>(1,1)/=2.0f; //fy
colorP.at<double>(0,2)/=2.0f; //cx
colorP.at<double>(1,2)/=2.0f; //cy
colorSize.width/=2;
colorSize.height/=2;
}
cv::Mat depthP = stereoModel_.left().P();
cv::Size depthSize = stereoModel_.left().imageSize();
float ratioY = float(colorSize.height)/float(depthSize.height);
float ratioX = float(colorSize.width)/float(depthSize.width);
depthP.at<double>(0,0)*=ratioX; //fx
depthP.at<double>(1,1)*=ratioY; //fy
depthP.at<double>(0,2)*=ratioX; //cx
depthP.at<double>(1,2)*=ratioY; //cy
depthSize.width*=ratioX;
depthSize.height*=ratioY;
const CameraModel & l = stereoModel_.left();
const CameraModel & r = stereoModel_.right();
stereoModel_ = StereoCameraModel(stereoModel_.name(),
depthSize, l.K_raw(), l.D_raw(), l.R(), depthP,
colorSize, r.K_raw(), r.D_raw(), r.R(), colorP,
stereoModel_.R(), stereoModel_.T(), stereoModel_.E(), stereoModel_.F());
stereoModel_.initRectificationMap();
}
}
return true;
}
else
{
UERROR("CameraFreenect2: no device connected or failure opening the default one! Note that rtabmap should link on libusb of libfreenect2. "
"Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
}
#else
UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
#endif
return false;
}
bool CameraFreenect2::isCalibrated() const
{
return true;
}
std::string CameraFreenect2::getSerial() const
{
#ifdef RTABMAP_FREENECT2
if(dev_)
{
return dev_->getSerialNumber();
}
#endif
return "";
}
SensorData CameraFreenect2::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_FREENECT2
if(dev_ && listener_)
{
libfreenect2::FrameMap frames;
#ifndef LIBFREENECT2_THREADING_STDLIB
UDEBUG("Waiting for new frames... If it is stalled here, rtabmap should link on libusb of libfreenect2. "
"Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
listener_->waitForNewFrame(frames);
#else
if(!listener_->waitForNewFrame(frames, 1000))
{
UWARN("CameraFreenect2: Failed to get frames! rtabmap should link on libusb of libfreenect2. "
"Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
}
else
#endif
{
double stamp = UTimer::now();
libfreenect2::Frame *rgbFrame = 0;
libfreenect2::Frame *irFrame = 0;
libfreenect2::Frame *depthFrame = 0;
switch(type_)
{
case kTypeColorIR: //used for calibration
rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
break;
case kTypeIRDepth:
irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
break;
case kTypeColor2DepthSD:
case kTypeDepth2ColorSD:
case kTypeDepth2ColorHD:
case kTypeDepth2ColorHD2:
default:
rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
break;
}
cv::Mat rgb, depth;
float fx=0,fy=0,cx=0,cy=0;
if(irFrame && depthFrame)
{
cv::Mat irMat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data);
//convert to gray scaled
float maxIr_ = 0x7FFF;
float minIr_ = 0x0;
const float factor = 255.0f / float((maxIr_ - minIr_));
rgb = cv::Mat(irMat.rows, irMat.cols, CV_8UC1);
for(int i=0; i<irMat.rows; ++i)
{
for(int j=0; j<irMat.cols; ++j)
{
rgb.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(irMat.at<float>(i,j) - minIr_, 0.0f)) * factor, 255.0f);
}
}
cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(rgb, rgb, 1);
cv::flip(depth, depth, 1);
if(stereoModel_.isValidForRectification())
{
//rectify
rgb = stereoModel_.left().rectifyImage(rgb);
depth = stereoModel_.left().rectifyDepth(depth);
fx = stereoModel_.left().fx();
fy = stereoModel_.left().fy();
cx = stereoModel_.left().cx();
cy = stereoModel_.left().cy();
}
else
{
libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
}
}
else
{
//rgb + ir or rgb + depth
if(stereoModel_.isValidForRectification())
{
cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
cv::Mat rgbMat; // rtabmap uses 3 channels RGB
#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
cv::cvtColor(rgbMatC4, rgbMat, CV_RGBA2BGR);
#else
cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
#endif
cv::flip(rgbMat, rgb, 1);
//rectify color
rgb = stereoModel_.right().rectifyImage(rgb);
if(irFrame)
{
//rectify IR
cv::Mat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(depth, depth, 1);
depth = stereoModel_.left().rectifyImage(depth);
}
else
{
//rectify depth
cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(depth, depth, 1);
//depth = stereoModel_.left().rectifyImage(depth, 0); // ~0.5/4 ms but is more noisy
depth = stereoModel_.left().rectifyDepth(depth); // ~16/25 ms
bool registered = true;
if(registered)
{
depth = util2d::registerDepth(
depth,
stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K
depth.size(),
stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K
stereoModel_.stereoTransform());
util2d::fillRegisteredDepthHoles(depth, true, false);
fx = stereoModel_.right().fx();
fy = stereoModel_.right().fy();
cx = stereoModel_.right().cx();
cy = stereoModel_.right().cy();
}
else
{
fx = stereoModel_.left().fx();
fy = stereoModel_.left().fy();
cx = stereoModel_.left().cx();
cy = stereoModel_.left().cy();
}
}
}
else
{
//use data from libfreenect2
if(irFrame)
{
cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
cv::Mat rgbMat; // rtabmap uses 3 channels RGB
#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
cv::cvtColor(rgbMatC4, rgbMat, CV_RGB2BGR);
#else
cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
#endif
cv::flip(rgbMat, rgb, 1);
cv::Mat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(depth, depth, 1);
}
else
{
//registration of the depth
UASSERT(reg_!=0);
float maxDepth = maxKinect2Depth_*1000.0f;
float minDepth = minKinect2Depth_*1000.0f;
if(type_ == kTypeColor2DepthSD || type_ == kTypeDepth2ColorHD)
{
cv::Mat rgbMatBGRA;
libfreenect2::Frame depthUndistorted(512, 424, 4);
libfreenect2::Frame rgbRegistered(512, 424, 4);
// do it before registration
if(noiseFiltering_)
{
cv::Mat depthMat = cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data);
for(int dx=0; dx<depthMat.cols; ++dx)
{
bool onEdgeX = dx==depthMat.cols-1;
for(int dy=0; dy<depthMat.rows; ++dy)
{
bool onEdge = onEdgeX || dy==depthMat.rows-1;
float z = 0.0f;
float & dz = depthMat.at<float>(dy,dx);
if(dz>=minDepth && dz <= maxDepth)
{
z = dz;
if(noiseFiltering_ && !onEdge)
{
z=0;
const float & dz1 = depthMat.at<float>(dy,dx+1);
const float & dz2 = depthMat.at<float>(dy+1,dx);
const float & dz3 = depthMat.at<float>(dy+1,dx+1);
if( dz1>=minDepth && dz1 <= maxDepth &&
dz2>=minDepth && dz2 <= maxDepth &&
dz3>=minDepth && dz3 <= maxDepth)
{
float avg = (dz + dz1 + dz2 + dz3) / 4.0f;
float thres = 0.01f*avg;
if( fabs(dz-avg) < thres &&
fabs(dz1-avg) < thres &&
fabs(dz2-avg) < thres &&
fabs(dz3-avg) < thres)
{
z = dz;
}
}
}
}
dz = z;
}
}
}
libfreenect2::Frame bidDepth(1920, 1082, 4); // HD
reg_->apply(rgbFrame, depthFrame, &depthUndistorted, &rgbRegistered, true, &bidDepth);
cv::Mat depthMat;
if(type_ == kTypeColor2DepthSD)
{
rgbMatBGRA = cv::Mat((int)rgbRegistered.height, (int)rgbRegistered.width, CV_8UC4, rgbRegistered.data);
depthMat = cv::Mat((int)depthUndistorted.height, (int)depthUndistorted.width, CV_32FC1, depthUndistorted.data);
//use IR params
libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
}
else
{
rgbMatBGRA = cv::Mat((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
depthMat = cv::Mat((int)bidDepth.height, (int)bidDepth.width, CV_32FC1, bidDepth.data);
depthMat = depthMat(cv::Range(1, 1081), cv::Range::all());
//use color params
libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
}
//filter max depth and flip
depth = cv::Mat(depthMat.size(), CV_16UC1);
for(int dx=0; dx<depthMat.cols; ++dx)
{
for(int dy=0; dy<depthMat.rows; ++dy)
{
unsigned short z = 0;
const float & dz = depthMat.at<float>(dy,dx);
if(dz>=minDepth && dz <= maxDepth)
{
z = (unsigned short)dz;
}
depth.at<unsigned short>(dy,(depthMat.cols-1)-dx) = z; //flip
}
}
// rtabmap uses 3 channels RGB
#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR);
#else
cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
#endif
cv::flip(rgb, rgb, 1);
}
else //register depth to color (OLD WAY)
{
UASSERT(type_ == kTypeDepth2ColorSD || type_ == kTypeDepth2ColorHD2);
cv::Mat rgbMatBGRA = cv::Mat((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
if(type_ == kTypeDepth2ColorSD)
{
cv::Mat tmp;
cv::resize(rgbMatBGRA, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
rgbMatBGRA = tmp;
}
// rtabmap uses 3 channels RGB
#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR);
#else
cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
#endif
cv::flip(rgb, rgb, 1);
cv::Mat depthFrameMat = cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data);
depth = cv::Mat::zeros(rgbMatBGRA.rows, rgbMatBGRA.cols, CV_16U);
for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
{
for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
{
float dz = depthFrameMat.at<float>(dy,dx);
if(dz>=minDepth && dz<=maxDepth)
{
bool goodDepth = true;
if(noiseFiltering_)
{
goodDepth = false;
float dz1 = depthFrameMat.at<float>(dy,dx+1);
float dz2 = depthFrameMat.at<float>(dy+1,dx);
float dz3 = depthFrameMat.at<float>(dy+1,dx+1);
if(dz1>=minDepth && dz1 <= maxDepth &&
dz2>=minDepth && dz2 <= maxDepth &&
dz3>=minDepth && dz3 <= maxDepth)
{
float avg = (dz + dz1 + dz2 + dz3) / 4.0f;
float thres = 0.01 * avg;
if( fabs(dz-avg) < thres &&
fabs(dz1-avg) < thres &&
fabs(dz2-avg) < thres &&
fabs(dz3-avg) < thres)
{
goodDepth = true;
}
}
}
if(goodDepth)
{
float cx=-1,cy=-1;
reg_->apply(dx, dy, dz, cx, cy);
if(type_ == kTypeDepth2ColorSD)
{
cx/=2.0f;
cy/=2.0f;
}
int rcx = cvRound(cx);
int rcy = cvRound(cy);
if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows))
{
unsigned short & zReg = depth.at<unsigned short>(rcy, rcx);
if(zReg == 0 || zReg > (unsigned short)dz)
{
zReg = (unsigned short)dz;
}
}
}
}
}
}
util2d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeDepth2ColorHD2);
util2d::fillRegisteredDepthHoles(depth, type_==kTypeDepth2ColorSD, type_==kTypeDepth2ColorHD2);//second pass
cv::flip(depth, depth, 1);
libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
fx = params.fx*(type_==kTypeDepth2ColorSD?0.5:1.0f);
fy = params.fy*(type_==kTypeDepth2ColorSD?0.5:1.0f);
cx = params.cx*(type_==kTypeDepth2ColorSD?0.5:1.0f);
cy = params.cy*(type_==kTypeDepth2ColorSD?0.5:1.0f);
}
}
}
}
CameraModel model;
if(fx && fy)
{
model=CameraModel(
fx, //fx
fy, //fy
cx, //cx
cy, // cy
this->getLocalTransform(),
0,
rgb.size());
}
data = SensorData(rgb, depth, model, this->getNextSeqID(), stamp);
listener_->release(frames);
}
}
#else
UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
#endif
return data;
}
} // namespace rtabmap

175
corelib/src/CameraRGB.cpp → corelib/src/camera/CameraImages.cpp
View File

@@ -25,39 +25,19 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/CameraRGB.h"
#include "rtabmap/core/Graph.h"
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/core/camera/CameraImages.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UTimer.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>
#if CV_MAJOR_VERSION >= 3
#include <opencv2/videoio/videoio_c.h>
#endif
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UThreadC.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_surface.h>
#include <pcl/common/io.h>
#include <iostream>
#include <rtabmap/core/Graph.h>
#include <fstream>
#include <cmath>
namespace rtabmap
{
/////////////////////////
// CameraImages
/////////////////////////
CameraImages::CameraImages() :
_startAt(0),
_refreshDir(false),
@@ -776,151 +756,4 @@ SensorData CameraImages::captureImage(CameraInfo * info)
return data;
}
/////////////////////////
// CameraVideo
/////////////////////////
CameraVideo::CameraVideo(
int usbDevice,
bool rectifyImages,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
_rectifyImages(rectifyImages),
_src(kUsbDevice),
_usbDevice(usbDevice)
{
}
CameraVideo::CameraVideo(
const std::string & filePath,
bool rectifyImages,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
_filePath(filePath),
_rectifyImages(rectifyImages),
_src(kVideoFile),
_usbDevice(0)
{
}
CameraVideo::~CameraVideo()
{
_capture.release();
}
bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
{
_guid = cameraName;
if(_capture.isOpened())
{
_capture.release();
}
if(_src == kUsbDevice)
{
ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d", _usbDevice);
_capture.open(_usbDevice);
}
else if(_src == kVideoFile)
{
ULOGGER_DEBUG("Camera: filename=\"%s\"", _filePath.c_str());
_capture.open(_filePath.c_str());
}
else
{
ULOGGER_ERROR("Camera: Unknown source...");
}
if(!_capture.isOpened())
{
ULOGGER_ERROR("Camera: Failed to create a capture object!");
_capture.release();
return false;
}
else
{
if (_guid.empty())
{
unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
if (guid != 0 && guid != 0xffffffff)
{
_guid = uFormat("%08x", guid);
}
}
// look for calibration files
if(!calibrationFolder.empty() && !_guid.empty())
{
if(!_model.load(calibrationFolder, _guid))
{
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
_guid.c_str(), calibrationFolder.c_str());
}
else
{
UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
_model.fx(),
_model.fy(),
_model.cx(),
_model.cy());
}
}
_model.setLocalTransform(this->getLocalTransform());
if(_rectifyImages && !_model.isValidForRectification())
{
UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");
return false;
}
}
return true;
}
bool CameraVideo::isCalibrated() const
{
return _model.isValidForProjection();
}
std::string CameraVideo::getSerial() const
{
return _guid;
}
SensorData CameraVideo::captureImage(CameraInfo * info)
{
cv::Mat img;
if(_capture.isOpened())
{
if(_capture.read(img))
{
if(_model.imageHeight() == 0 || _model.imageWidth() == 0)
{
_model.setImageSize(img.size());
}
if(_model.isValidForRectification() && _rectifyImages)
{
img = _model.rectifyImage(img);
}
else
{
// clone required
img = img.clone();
}
}
else if(_usbDevice)
{
UERROR("Camera has been disconnected!");
}
}
else
{
ULOGGER_WARN("The camera must be initialized before requesting an image.");
}
return SensorData(img, _model, this->getNextSeqID(), UTimer::now());
}
} // namespace rtabmap

579
corelib/src/camera/CameraK4W2.cpp Normal file
View File

@@ -0,0 +1,579 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraK4W2.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UThreadC.h>
#include <rtabmap/core/util2d.h>
#ifdef RTABMAP_K4W2
#include <Kinect.h>
#endif
namespace rtabmap
{
#ifdef RTABMAP_K4W2
// Safe release for interfaces
template<class Interface>
inline void SafeRelease(Interface *& pInterfaceToRelease)
{
if (pInterfaceToRelease != NULL)
{
pInterfaceToRelease->Release();
pInterfaceToRelease = NULL;
}
}
#endif
bool CameraK4W2::available()
{
#ifdef RTABMAP_K4W2
return true;
#else
return false;
#endif
}
CameraK4W2::CameraK4W2(
int deviceId,
Type type,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_K4W2
,
type_(type),
pKinectSensor_(NULL),
pCoordinateMapper_(NULL),
pDepthCoordinates_(new DepthSpacePoint[cColorWidth * cColorHeight]),
pColorCoordinates_(new ColorSpacePoint[cDepthWidth * cDepthHeight]),
pMultiSourceFrameReader_(NULL),
pColorRGBX_(new RGBQUAD[cColorWidth * cColorHeight]),
hMSEvent(NULL)
#endif
{
}
CameraK4W2::~CameraK4W2()
{
#ifdef RTABMAP_K4W2
if (pDepthCoordinates_)
{
delete[] pDepthCoordinates_;
pDepthCoordinates_ = NULL;
}
if (pColorCoordinates_)
{
delete[] pColorCoordinates_;
pColorCoordinates_ = NULL;
}
if (pColorRGBX_)
{
delete[] pColorRGBX_;
pColorRGBX_ = NULL;
}
close();
#endif
}
void CameraK4W2::close()
{
#ifdef RTABMAP_K4W2
if (pMultiSourceFrameReader_)
{
pMultiSourceFrameReader_->UnsubscribeMultiSourceFrameArrived(hMSEvent);
CloseHandle((HANDLE)hMSEvent);
hMSEvent = NULL;
}
// done with frame reader
SafeRelease(pMultiSourceFrameReader_);
// done with coordinate mapper
SafeRelease(pCoordinateMapper_);
// close the Kinect Sensor
if (pKinectSensor_)
{
pKinectSensor_->Close();
}
SafeRelease(pKinectSensor_);
colorCameraModel_ = CameraModel();
#endif
}
bool CameraK4W2::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef RTABMAP_K4W2
HRESULT hr;
close();
hr = GetDefaultKinectSensor(&pKinectSensor_);
if (FAILED(hr))
{
return false;
}
if (pKinectSensor_)
{
// Initialize the Kinect and get coordinate mapper and the frame reader
hr = pKinectSensor_->Open();
if (SUCCEEDED(hr))
{
hr = pKinectSensor_->get_CoordinateMapper(&pCoordinateMapper_);
if (SUCCEEDED(hr))
{
hr = pKinectSensor_->OpenMultiSourceFrameReader(
FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color,
&pMultiSourceFrameReader_);
if (SUCCEEDED(hr))
{
hr = pMultiSourceFrameReader_->SubscribeMultiSourceFrameArrived(&hMSEvent);
}
}
}
}
if (!pKinectSensor_ || FAILED(hr))
{
UERROR("No ready Kinect found!");
close();
return false;
}
// to query camera parameters, we should wait a little
uSleep(3000);
// initialize color calibration if not set yet
CameraIntrinsics intrinsics;
hr = pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
if (SUCCEEDED(hr) && intrinsics.FocalLengthX > 0.0f)
{
// guess color intrinsics by comparing two reprojections
CameraModel depthModel(
intrinsics.FocalLengthX,
intrinsics.FocalLengthY,
intrinsics.PrincipalPointX,
intrinsics.PrincipalPointY);
cv::Mat fakeDepth = cv::Mat::ones(cDepthHeight, cDepthWidth, CV_16UC1) * 1000;
hr = pCoordinateMapper_->MapDepthFrameToColorSpace(cDepthWidth * cDepthHeight, (UINT16*)fakeDepth.data, cDepthWidth * cDepthHeight, pColorCoordinates_);
if (SUCCEEDED(hr))
{
int firstIndex = -1;
int lastIndex = -1;
for (int depthIndex = 0; depthIndex < (cDepthWidth*cDepthHeight); ++depthIndex)
{
ColorSpacePoint p = pColorCoordinates_[depthIndex];
// Values that are negative infinity means it is an invalid color to depth mapping so we
// skip processing for this pixel
if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
{
if (firstIndex == -1)
{
firstIndex = depthIndex;
}
lastIndex = depthIndex;
}
}
UASSERT(firstIndex >= 0 && lastIndex >= 0);
float fx, fy, cx, cy;
float x1, y1, z1, x2, y2, z2;
depthModel.project(firstIndex - (firstIndex / cDepthWidth)*cDepthWidth, firstIndex / cDepthWidth, 1.0f, x1, y1, z1);
depthModel.project(lastIndex - (lastIndex / cDepthWidth)*cDepthWidth, lastIndex / cDepthWidth, 1.0f, x2, y2, z2);
ColorSpacePoint uv1 = pColorCoordinates_[firstIndex];
ColorSpacePoint uv2 = pColorCoordinates_[lastIndex];
fx = ((uv1.X - uv2.X)*z1*z2) / (x1*z2 - x2*z1);
cx = uv1.X - (x1 / z1) * fx;
fy = ((uv1.Y - uv2.Y)*z1*z2) / (y1*z2 - y2*z1);
cy = uv1.Y - (y1 / z1) * fy;
colorCameraModel_ = CameraModel(
fx,
fy,
float(cColorWidth) - cx,
cy,
this->getLocalTransform(),
0,
cv::Size(cColorWidth, cColorHeight));
}
}
if (!colorCameraModel_.isValidForProjection())
{
UERROR("Failed to get camera parameters! Is the camera connected? Try restarting the camera again or use kTypeColor2DepthSD.");
close();
return false;
}
std::string serial = getSerial();
if (!serial.empty())
{
UINFO("Running kinect device \"%s\"", serial.c_str());
}
return true;
#else
UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
return false;
#endif
}
bool CameraK4W2::isCalibrated() const
{
return true;
}
std::string CameraK4W2::getSerial() const
{
#ifdef RTABMAP_K4W2
if (pKinectSensor_)
{
wchar_t uid[255] = { 0 };
// It seems to fail every time!?
HRESULT hr = pKinectSensor_->get_UniqueKinectId(255, uid);
if (SUCCEEDED(hr))
{
std::wstring ws(uid);
return std::string(ws.begin(), ws.end());
}
}
#endif
return "";
}
SensorData CameraK4W2::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_K4W2
if (!pMultiSourceFrameReader_)
{
return data;
}
HRESULT hr;
//now check for frame events
HANDLE handles[] = { reinterpret_cast<HANDLE>(hMSEvent) };
double t = UTimer::now();
while((UTimer::now()-t < 5.0) && WaitForMultipleObjects(_countof(handles), handles, false, 5000) == WAIT_OBJECT_0)
{
IMultiSourceFrameArrivedEventArgs* pArgs = NULL;
hr = pMultiSourceFrameReader_->GetMultiSourceFrameArrivedEventData(hMSEvent, &pArgs);
if (SUCCEEDED(hr))
{
IMultiSourceFrameReference * pFrameRef = NULL;
hr = pArgs->get_FrameReference(&pFrameRef);
if (SUCCEEDED(hr))
{
IMultiSourceFrame* pMultiSourceFrame = NULL;
IDepthFrame* pDepthFrame = NULL;
IColorFrame* pColorFrame = NULL;
hr = pFrameRef->AcquireFrame(&pMultiSourceFrame);
if (FAILED(hr))
{
UERROR("Failed getting latest frame.");
}
IDepthFrameReference* pDepthFrameReference = NULL;
hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
if (SUCCEEDED(hr))
{
hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
}
SafeRelease(pDepthFrameReference);
IColorFrameReference* pColorFrameReference = NULL;
hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
if (SUCCEEDED(hr))
{
hr = pColorFrameReference->AcquireFrame(&pColorFrame);
}
SafeRelease(pColorFrameReference);
if (pDepthFrame && pColorFrame)
{
IFrameDescription* pDepthFrameDescription = NULL;
int nDepthWidth = 0;
int nDepthHeight = 0;
UINT nDepthBufferSize = 0;
UINT16 *pDepthBuffer = NULL;
IFrameDescription* pColorFrameDescription = NULL;
int nColorWidth = 0;
int nColorHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
UINT nColorBufferSize = 0;
RGBQUAD *pColorBuffer = NULL;
// get depth frame data
if (SUCCEEDED(hr))
hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription);
if (SUCCEEDED(hr))
hr = pDepthFrameDescription->get_Width(&nDepthWidth);
if (SUCCEEDED(hr))
hr = pDepthFrameDescription->get_Height(&nDepthHeight);
if (SUCCEEDED(hr))
hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
// get color frame data
if (SUCCEEDED(hr))
hr = pColorFrame->get_FrameDescription(&pColorFrameDescription);
if (SUCCEEDED(hr))
hr = pColorFrameDescription->get_Width(&nColorWidth);
if (SUCCEEDED(hr))
hr = pColorFrameDescription->get_Height(&nColorHeight);
if (SUCCEEDED(hr))
hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
if (SUCCEEDED(hr))
{
if (imageFormat == ColorImageFormat_Bgra)
{
hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
}
else if (pColorRGBX_)
{
pColorBuffer = pColorRGBX_;
nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
}
else
{
hr = E_FAIL;
}
}
if(SUCCEEDED(hr))
{
//ProcessFrame(nDepthTime, pDepthBuffer, nDepthWidth, nDepthHeight,
// pColorBuffer, nColorWidth, nColorHeight,
// pBodyIndexBuffer, nBodyIndexWidth, nBodyIndexHeight);
// Make sure we've received valid data
if (pCoordinateMapper_ &&
pDepthBuffer && (nDepthWidth == cDepthWidth) && (nDepthHeight == cDepthHeight) &&
pColorBuffer && (nColorWidth == cColorWidth) && (nColorHeight == cColorHeight))
{
if (type_ == kTypeColor2DepthSD)
{
HRESULT hr = pCoordinateMapper_->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nColorWidth * nColorHeight, pDepthCoordinates_);
if (SUCCEEDED(hr))
{
cv::Mat depth = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_16UC1);
cv::Mat imageColorRegistered = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_8UC3);
// loop over output pixels
for (int colorIndex = 0; colorIndex < (nColorWidth*nColorHeight); ++colorIndex)
{
DepthSpacePoint p = pDepthCoordinates_[colorIndex];
// Values that are negative infinity means it is an invalid color to depth mapping so we
// skip processing for this pixel
if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
{
// To avoid black lines caused by rounding pixel values, we should set 4 pixels
// At the same do mirror
int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
pixel_x_l = nDepthWidth - static_cast<int>(p.X);
pixel_y_l = static_cast<int>(p.Y);
pixel_x_h = pixel_x_l - 1;
pixel_y_h = pixel_y_l + 1;
const RGBQUAD* pSrc = pColorBuffer + colorIndex;
if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
{
unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_l);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(pixel_y_l, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_l*nDepthWidth);
}
if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
{
unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_l);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(pixel_y_h, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_h*nDepthWidth);
}
if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
{
unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_h);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(pixel_y_l, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_l*nDepthWidth);
}
if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
{
unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_h);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(pixel_y_h, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_h*nDepthWidth);
}
}
}
CameraIntrinsics intrinsics;
pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
CameraModel model(
intrinsics.FocalLengthX,
intrinsics.FocalLengthY,
intrinsics.PrincipalPointX,
intrinsics.PrincipalPointY,
this->getLocalTransform(),
0,
depth.size());
data = SensorData(imageColorRegistered, depth, model, this->getNextSeqID(), UTimer::now());
}
else
{
UERROR("Failed color to depth registration!");
}
}
else //depthToColor
{
HRESULT hr = pCoordinateMapper_->MapDepthFrameToColorSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nDepthWidth * nDepthHeight, pColorCoordinates_);
if (SUCCEEDED(hr))
{
cv::Mat depthSource(nDepthHeight, nDepthWidth, CV_16UC1, pDepthBuffer);
cv::Mat depthRegistered = cv::Mat::zeros(
type_ == kTypeDepth2ColorSD ? nColorHeight/2 : nColorHeight,
type_ == kTypeDepth2ColorSD ? nColorWidth/2 : nColorWidth,
CV_16UC1);
cv::Mat imageColor;
if(type_ == kTypeDepth2ColorSD)
{
cv::Mat tmp;
cv::resize(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
cv::cvtColor(tmp, imageColor, CV_BGRA2BGR);
}
else
{
cv::cvtColor(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), imageColor, CV_BGRA2BGR);
}
// loop over output pixels
for (int depthIndex = 0; depthIndex < (nDepthWidth*nDepthHeight); ++depthIndex)
{
ColorSpacePoint p = pColorCoordinates_[depthIndex];
// Values that are negative infinity means it is an invalid color to depth mapping so we
// skip processing for this pixel
if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
{
if (type_ == kTypeDepth2ColorSD)
{
p.X /= 2.0f;
p.Y /= 2.0f;
}
const unsigned short & depth_value = depthSource.at<unsigned short>(0, depthIndex);
int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
// get the coordinate on image plane.
pixel_x_l = depthRegistered.cols - p.X; // flip depth
pixel_y_l = p.Y;
pixel_x_h = pixel_x_l - 1;
pixel_y_h = pixel_y_l + 1;
if (pixel_x_l >= 0 && pixel_x_l < depthRegistered.cols &&
pixel_y_l>0 && pixel_y_l < depthRegistered.rows && // ignore first line
depth_value)
{
unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_l, pixel_x_l);
if (depthPixel == 0 || depthPixel > depth_value)
{
depthPixel = depth_value;
}
}
if (pixel_x_h >= 0 && pixel_x_h < depthRegistered.cols &&
pixel_y_h>0 && pixel_y_h < depthRegistered.rows && // ignore first line
depth_value)
{
unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_h, pixel_x_h);
if (depthPixel == 0 || depthPixel > depth_value)
{
depthPixel = depth_value;
}
}
}
}
CameraModel model = colorCameraModel_;
if (type_ == kTypeDepth2ColorSD)
{
model = model.scaled(0.5);
}
util2d::fillRegisteredDepthHoles(depthRegistered, true, true, type_ == kTypeDepth2ColorHD);
depthRegistered = rtabmap::util2d::fillDepthHoles(depthRegistered, 1);
cv::flip(imageColor, imageColor, 1);
data = SensorData(imageColor, depthRegistered, model, this->getNextSeqID(), UTimer::now());
}
else
{
UERROR("Failed depth to color registration!");
}
}
}
}
SafeRelease(pDepthFrameDescription);
SafeRelease(pColorFrameDescription);
}
pFrameRef->Release();
SafeRelease(pDepthFrame);
SafeRelease(pColorFrame);
SafeRelease(pMultiSourceFrame);
}
pArgs->Release();
}
if (!data.imageRaw().empty())
{
break;
}
}
#else
UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
#endif
return data;
}
} // namespace rtabmap

584
corelib/src/camera/CameraOpenNI2.cpp Normal file
View File

@@ -0,0 +1,584 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraOpenNI2.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UThreadC.h>
#include <rtabmap/core/util2d.h>
#ifdef RTABMAP_OPENNI2
#include <OniVersion.h>
#include <OpenNI.h>
#endif
namespace rtabmap
{
bool CameraOpenNI2::available()
{
#ifdef RTABMAP_OPENNI2
return true;
#else
return false;
#endif
}
bool CameraOpenNI2::exposureGainAvailable()
{
#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
return true;
#else
return false;
#endif
}
CameraOpenNI2::CameraOpenNI2(
const std::string & deviceId,
Type type,
float imageRate,
const rtabmap::Transform & localTransform) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_OPENNI2
,
_type(type),
_device(new openni::Device()),
_color(new openni::VideoStream()),
_depth(new openni::VideoStream()),
_depthFx(0.0f),
_depthFy(0.0f),
_deviceId(deviceId),
_openNI2StampsAndIDsUsed(false),
_depthHShift(0),
_depthVShift(0)
#endif
{
}
CameraOpenNI2::~CameraOpenNI2()
{
#ifdef RTABMAP_OPENNI2
_color->stop();
_color->destroy();
_depth->stop();
_depth->destroy();
_device->close();
openni::OpenNI::shutdown();
delete _device;
delete _color;
delete _depth;
#endif
}
bool CameraOpenNI2::setAutoWhiteBalance(bool enabled)
{
#ifdef RTABMAP_OPENNI2
if(_color && _color->getCameraSettings())
{
return _color->getCameraSettings()->setAutoWhiteBalanceEnabled(enabled) == openni::STATUS_OK;
}
#else
UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
#endif
return false;
}
bool CameraOpenNI2::setAutoExposure(bool enabled)
{
#ifdef RTABMAP_OPENNI2
if(_color && _color->getCameraSettings())
{
return _color->getCameraSettings()->setAutoExposureEnabled(enabled) == openni::STATUS_OK;
}
#else
UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
#endif
return false;
}
bool CameraOpenNI2::setExposure(int value)
{
#ifdef RTABMAP_OPENNI2
#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
if(_color && _color->getCameraSettings())
{
return _color->getCameraSettings()->setExposure(value) == openni::STATUS_OK;
}
#else
UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
#endif
#else
UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
#endif
return false;
}
bool CameraOpenNI2::setGain(int value)
{
#ifdef RTABMAP_OPENNI2
#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
if(_color && _color->getCameraSettings())
{
return _color->getCameraSettings()->setGain(value) == openni::STATUS_OK;
}
#else
UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
#endif
#else
UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
#endif
return false;
}
bool CameraOpenNI2::setMirroring(bool enabled)
{
#ifdef RTABMAP_OPENNI2
if(_color->isValid() && _depth->isValid())
{
return _depth->setMirroringEnabled(enabled) == openni::STATUS_OK &&
_color->setMirroringEnabled(enabled) == openni::STATUS_OK;
}
#endif
return false;
}
void CameraOpenNI2::setOpenNI2StampsAndIDsUsed(bool used)
{
#ifdef RTABMAP_OPENNI2
_openNI2StampsAndIDsUsed = used;
#endif
}
void CameraOpenNI2::setIRDepthShift(int horizontal, int vertical)
{
#ifdef RTABMAP_OPENNI2
UASSERT(horizontal >= 0);
UASSERT(vertical >= 0);
_depthHShift = horizontal;
_depthVShift = vertical;
#endif
}
bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef RTABMAP_OPENNI2
openni::OpenNI::initialize();
openni::Array<openni::DeviceInfo> devices;
openni::OpenNI::enumerateDevices(&devices);
for(int i=0; i<devices.getSize(); ++i)
{
UINFO("Device %d: Name=%s URI=%s Vendor=%s",
i,
devices[i].getName(),
devices[i].getUri(),
devices[i].getVendor());
}
if(_deviceId.empty() && devices.getSize() == 0)
{
UERROR("CameraOpenNI2: No device detected!");
return false;
}
openni::Status error = _device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str());
if(error != openni::STATUS_OK)
{
if(!_deviceId.empty())
{
UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d).", _deviceId.c_str(), error);
}
else
{
#ifdef _WIN32
UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d).", devices[0].getName(), error);
#else
UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d). Verify if \"%s\" is in udev rules: \"/lib/udev/rules.d/40-libopenni2-0.rules\". If not, add it and reboot.", devices[0].getName(), error, devices[0].getUri());
#endif
}
_device->close();
openni::OpenNI::shutdown();
return false;
}
// look for calibration files
_stereoModel = StereoCameraModel();
bool hardwareRegistration = true;
if(!calibrationFolder.empty())
{
// we need the serial
std::string calibrationName = _device->getDeviceInfo().getName();
if(!cameraName.empty())
{
calibrationName = cameraName;
}
_stereoModel.setName(calibrationName, "depth", "rgb");
hardwareRegistration = !_stereoModel.load(calibrationFolder, calibrationName, false);
if(_type != kTypeColorDepth)
{
hardwareRegistration = false;
}
if((_type != kTypeColorDepth && !_stereoModel.left().isValidForRectification()) ||
(_type == kTypeColorDepth && !_stereoModel.right().isValidForRectification()))
{
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
calibrationName.c_str(), calibrationFolder.c_str());
}
else if(_type == kTypeColorDepth && _stereoModel.right().isValidForRectification() && hardwareRegistration)
{
UWARN("Missing extrinsic calibration file for camera \"%s\" in \"%s\" folder, default registration is used even if rgb is rectified!",
calibrationName.c_str(), calibrationFolder.c_str());
}
else if(_type == kTypeColorDepth && _stereoModel.right().isValidForRectification() && !hardwareRegistration)
{
UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
"factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
}
}
if(UFile::getExtension(_deviceId).compare("oni")==0)
{
if(_device->getPlaybackControl() &&
_device->getPlaybackControl()->setRepeatEnabled(false) != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Cannot set repeat mode to false.");
_device->close();
openni::OpenNI::shutdown();
return false;
}
}
else if(_type==kTypeColorDepth && hardwareRegistration &&
!_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
{
UERROR("CameraOpenNI2: Device doesn't support depth/color registration.");
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_device->getSensorInfo(openni::SENSOR_DEPTH) == NULL ||
_device->getSensorInfo(_type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) == NULL)
{
UERROR("CameraOpenNI2: Cannot get sensor info for depth and %s.", _type==kTypeColorDepth?"color":"ir");
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_depth->create(*_device, openni::SENSOR_DEPTH) != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Cannot create depth stream.");
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_color->create(*_device, _type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Cannot create %s stream.", _type==kTypeColorDepth?"color":"ir");
_depth->destroy();
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_type==kTypeColorDepth && hardwareRegistration &&
_device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Failed to set depth/color registration.");
}
if (_device->setDepthColorSyncEnabled(true) != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Failed to set depth color sync");
}
_depth->setMirroringEnabled(false);
_color->setMirroringEnabled(false);
const openni::Array<openni::VideoMode>& depthVideoModes = _depth->getSensorInfo().getSupportedVideoModes();
for(int i=0; i<depthVideoModes.getSize(); ++i)
{
UINFO("CameraOpenNI2: Depth video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
i,
depthVideoModes[i].getFps(),
depthVideoModes[i].getPixelFormat(),
depthVideoModes[i].getResolutionX(),
depthVideoModes[i].getResolutionY());
}
const openni::Array<openni::VideoMode>& colorVideoModes = _color->getSensorInfo().getSupportedVideoModes();
for(int i=0; i<colorVideoModes.getSize(); ++i)
{
UINFO("CameraOpenNI2: %s video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
_type==kTypeColorDepth?"color":"ir",
i,
colorVideoModes[i].getFps(),
colorVideoModes[i].getPixelFormat(),
colorVideoModes[i].getResolutionX(),
colorVideoModes[i].getResolutionY());
}
openni::VideoMode mMode;
mMode.setFps(30);
mMode.setResolution(640,480);
mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
_depth->setVideoMode(mMode);
openni::VideoMode mModeColor;
mModeColor.setFps(30);
mModeColor.setResolution(640,480);
mModeColor.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
_color->setVideoMode(mModeColor);
UINFO("CameraOpenNI2: Using depth video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
_depth->getVideoMode().getFps(),
_depth->getVideoMode().getPixelFormat(),
_depth->getVideoMode().getResolutionX(),
_depth->getVideoMode().getResolutionY(),
_depth->getHorizontalFieldOfView(),
_depth->getVerticalFieldOfView());
UINFO("CameraOpenNI2: Using %s video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
_type==kTypeColorDepth?"color":"ir",
_color->getVideoMode().getFps(),
_color->getVideoMode().getPixelFormat(),
_color->getVideoMode().getResolutionX(),
_color->getVideoMode().getResolutionY(),
_color->getHorizontalFieldOfView(),
_color->getVerticalFieldOfView());
if(_depth->getVideoMode().getResolutionX() != 640 ||
_depth->getVideoMode().getResolutionY() != 480 ||
_depth->getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_DEPTH_1_MM)
{
UERROR("Could not set depth format to 640x480 pixel=%d(mm)!",
openni::PIXEL_FORMAT_DEPTH_1_MM);
_depth->destroy();
_color->destroy();
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_color->getVideoMode().getResolutionX() != 640 ||
_color->getVideoMode().getResolutionY() != 480 ||
_color->getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_RGB888)
{
UERROR("Could not set %s format to 640x480 pixel=%d!",
_type==kTypeColorDepth?"color":"ir",
openni::PIXEL_FORMAT_RGB888);
_depth->destroy();
_color->destroy();
_device->close();
openni::OpenNI::shutdown();
return false;
}
if(_color->getCameraSettings())
{
UINFO("CameraOpenNI2: AutoWhiteBalanceEnabled = %d", _color->getCameraSettings()->getAutoWhiteBalanceEnabled()?1:0);
UINFO("CameraOpenNI2: AutoExposureEnabled = %d", _color->getCameraSettings()->getAutoExposureEnabled()?1:0);
#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
UINFO("CameraOpenNI2: Exposure = %d", _color->getCameraSettings()->getExposure());
UINFO("CameraOpenNI2: GAIN = %d", _color->getCameraSettings()->getGain());
#endif
}
if(_type==kTypeColorDepth && hardwareRegistration)
{
_depthFx = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
_depthFy = float(_color->getVideoMode().getResolutionY()/2) / std::tan(_color->getVerticalFieldOfView()/2.0f);
}
else
{
_depthFx = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
_depthFy = float(_depth->getVideoMode().getResolutionY()/2) / std::tan(_depth->getVerticalFieldOfView()/2.0f);
}
UINFO("depth fx=%f fy=%f", _depthFx, _depthFy);
if(_type == kTypeIR)
{
UWARN("With type IR-only, depth stream will not be started");
}
if((_type != kTypeIR && _depth->start() != openni::STATUS_OK) ||
_color->start() != openni::STATUS_OK)
{
UERROR("CameraOpenNI2: Cannot start depth and/or color streams.");
_depth->stop();
_color->stop();
_depth->destroy();
_color->destroy();
_device->close();
openni::OpenNI::shutdown();
return false;
}
uSleep(3000); // just to make sure the sensor is correctly initialized and exposure is set
return true;
#else
UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
return false;
#endif
}
bool CameraOpenNI2::isCalibrated() const
{
return true;
}
std::string CameraOpenNI2::getSerial() const
{
#ifdef RTABMAP_OPENNI2
if(_device)
{
return _device->getDeviceInfo().getName();
}
#endif
return "";
}
SensorData CameraOpenNI2::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_OPENNI2
int readyStream = -1;
if(_device->isValid() &&
_depth->isValid() &&
_color->isValid() &&
_device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
_device->getSensorInfo(_type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) != NULL)
{
openni::VideoStream* depthStream[] = {_depth};
openni::VideoStream* colorStream[] = {_color};
if((_type != kTypeIR && openni::OpenNI::waitForAnyStream(depthStream, 1, &readyStream, 5000) != openni::STATUS_OK) ||
openni::OpenNI::waitForAnyStream(colorStream, 1, &readyStream, 5000) != openni::STATUS_OK)
{
UWARN("No frames received since the last 5 seconds, end of stream is reached!");
}
else
{
openni::VideoFrameRef depthFrame, colorFrame;
if(_type != kTypeIR)
{
_depth->readFrame(&depthFrame);
}
_color->readFrame(&colorFrame);
cv::Mat depth, rgb;
if((_type == kTypeIR || depthFrame.isValid()) && colorFrame.isValid())
{
int h,w;
if(_type != kTypeIR)
{
h=depthFrame.getHeight();
w=depthFrame.getWidth();
depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
}
h=colorFrame.getHeight();
w=colorFrame.getWidth();
cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
if(_type==kTypeColorDepth)
{
cv::cvtColor(tmp, rgb, CV_RGB2BGR);
}
else // IR
{
rgb = tmp.clone();
}
}
UASSERT(_depthFx != 0.0f && _depthFy != 0.0f);
if(!rgb.empty() && (_type == kTypeIR || !depth.empty()))
{
// default calibration
CameraModel model(
_depthFx, //fx
_depthFy, //fy
float(rgb.cols/2) - 0.5f, //cx
float(rgb.rows/2) - 0.5f, //cy
this->getLocalTransform(),
0,
rgb.size());
if(_type==kTypeColorDepth)
{
if(_stereoModel.right().isValidForRectification())
{
rgb = _stereoModel.right().rectifyImage(rgb);
model = _stereoModel.right();
if(_stereoModel.left().isValidForRectification() && !_stereoModel.stereoTransform().isNull())
{
if (_depthHShift > 0 || _depthVShift > 0)
{
cv::Mat out = cv::Mat::zeros(depth.size(), depth.type());
depth(cv::Rect(_depthHShift, _depthVShift, depth.cols - _depthHShift, depth.rows - _depthVShift)).copyTo(out(cv::Rect(0, 0, depth.cols - _depthHShift, depth.rows - _depthVShift)));
depth = out;
}
depth = _stereoModel.left().rectifyImage(depth, 0);
depth = util2d::registerDepth(depth, _stereoModel.left().K(), rgb.size(), _stereoModel.right().K(), _stereoModel.stereoTransform());
}
}
}
else // IR
{
if(_stereoModel.left().isValidForRectification())
{
rgb = _stereoModel.left().rectifyImage(rgb);
if(_type!=kTypeIR)
{
depth = _stereoModel.left().rectifyImage(depth, 0);
}
model = _stereoModel.left();
}
}
model.setLocalTransform(this->getLocalTransform());
if(_openNI2StampsAndIDsUsed)
{
data = SensorData(rgb, depth, model, depthFrame.getFrameIndex(), double(depthFrame.getTimestamp()) / 1000000.0);
}
else
{
data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
}
}
}
}
else
{
ULOGGER_WARN("The camera must be initialized before requesting an image.");
}
#else
UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
#endif
return data;
}
} // namespace rtabmap

139
corelib/src/camera/CameraOpenNICV.cpp Normal file
View File

@@ -0,0 +1,139 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraOpenNICV.h>
#include <rtabmap/utilite/UTimer.h>
namespace rtabmap
{
bool CameraOpenNICV::available()
{
return cv::getBuildInformation().find("OpenNI: YES") != std::string::npos;
}
CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform) :
Camera(imageRate, localTransform),
_asus(asus),
_depthFocal(0.0f)
{
}
CameraOpenNICV::~CameraOpenNICV()
{
_capture.release();
}
bool CameraOpenNICV::init(const std::string & calibrationFolder, const std::string & cameraName)
{
if(_capture.isOpened())
{
_capture.release();
}
ULOGGER_DEBUG("Camera::init()");
_capture.open( _asus?CV_CAP_OPENNI_ASUS:CV_CAP_OPENNI );
if(_capture.isOpened())
{
_capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ );
_depthFocal = _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH );
// Print some avalible device settings.
UINFO("Depth generator output mode:");
UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
UINFO("FRAME_MAX_DEPTH %f mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
UINFO("BASELINE %f mm", _capture.get( CV_CAP_PROP_OPENNI_BASELINE ));
UINFO("FPS %f", _capture.get( CV_CAP_PROP_FPS ));
UINFO("Focal %f", _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH ));
UINFO("REGISTRATION %f", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
if(_capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) == 0.0)
{
UERROR("Depth registration is not activated on this device!");
}
if( _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
{
UINFO("Image generator output mode:");
UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ));
UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ));
UINFO("FPS %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FPS ));
}
else
{
UERROR("Camera: Device doesn't contain image generator.");
_capture.release();
return false;
}
}
else
{
ULOGGER_ERROR("Camera: Failed to create a capture object!");
_capture.release();
return false;
}
return true;
}
bool CameraOpenNICV::isCalibrated() const
{
return true;
}
SensorData CameraOpenNICV::captureImage(CameraInfo * info)
{
SensorData data;
if(_capture.isOpened())
{
_capture.grab();
cv::Mat depth, rgb;
_capture.retrieve(depth, CV_CAP_OPENNI_DEPTH_MAP );
_capture.retrieve(rgb, CV_CAP_OPENNI_BGR_IMAGE );
depth = depth.clone();
rgb = rgb.clone();
UASSERT(_depthFocal>0.0f);
if(!rgb.empty() && !depth.empty())
{
CameraModel model(
_depthFocal, //fx
_depthFocal, //fy
float(rgb.cols/2) - 0.5f, //cx
float(rgb.rows/2) - 0.5f, //cy
this->getLocalTransform(),
0,
rgb.size());
data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
}
}
else
{
ULOGGER_WARN("The camera must be initialized before requesting an image.");
}
return data;
}
} // namespace rtabmap

207
corelib/src/camera/CameraOpenni.cpp Normal file
View File

@@ -0,0 +1,207 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraOpenni.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UThreadC.h>
#include <rtabmap/utilite/UTimer.h>
#ifdef RTABMAP_OPENNI
#include <pcl/io/openni_grabber.h>
#include <pcl/io/oni_grabber.h>
#include <pcl/io/openni_camera/openni_depth_image.h>
#include <pcl/io/openni_camera/openni_image.h>
#endif
namespace rtabmap
{
CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform) :
Camera(imageRate, localTransform),
interface_(0),
deviceId_(deviceId),
depthConstant_(0.0f)
{
}
bool CameraOpenni::available()
{
#ifdef RTABMAP_OPENNI
return true;
#else
return false;
#endif
}
CameraOpenni::~CameraOpenni()
{
#ifdef RTABMAP_OPENNI
UDEBUG("");
if(connection_.connected())
{
connection_.disconnect();
}
if(interface_)
{
interface_->stop();
uSleep(1000); // make sure it is stopped
delete interface_;
interface_ = 0;
}
#endif
}
#ifdef RTABMAP_OPENNI
void CameraOpenni::image_cb (
const boost::shared_ptr<openni_wrapper::Image>& rgb,
const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
float constant)
{
UScopeMutex s(dataMutex_);
bool notify = rgb_.empty();
cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
cv::cvtColor(rgbFrame, rgb_, CV_RGB2BGR);
depth_ = cv::Mat(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depth_.data);
depthConstant_ = constant;
if(notify)
{
dataReady_.release();
}
}
#endif
bool CameraOpenni::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef RTABMAP_OPENNI
if(interface_)
{
interface_->stop();
uSleep(100); // make sure it is stopped
delete interface_;
interface_ = 0;
}
try
{
if(UFile::getExtension(deviceId_).compare("oni") == 0)
{
interface_ = new pcl::ONIGrabber(deviceId_, false, true);
}
else
{
interface_ = new pcl::OpenNIGrabber(deviceId_);
}
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);
interface_->start ();
}
catch(const pcl::IOException& ex)
{
UERROR("OpenNI exception: %s", ex.what());
if(interface_)
{
delete interface_;
interface_ = 0;
}
return false;
}
return true;
#else
UERROR("PCL not built with OpenNI! Cannot initialize CameraOpenNI");
return false;
#endif
}
bool CameraOpenni::isCalibrated() const
{
#ifdef RTABMAP_OPENNI
return true;
#else
return false;
#endif
}
std::string CameraOpenni::getSerial() const
{
#ifdef RTABMAP_OPENNI
if(interface_)
{
return interface_->getName();
}
#endif
return "";
}
SensorData CameraOpenni::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_OPENNI
if(interface_ && interface_->isRunning())
{
if(!dataReady_.acquire(1, 5000))
{
UWARN("Not received new frames since 5 seconds, end of stream reached!");
}
else
{
UScopeMutex s(dataMutex_);
if(depthConstant_ && !rgb_.empty() && !depth_.empty())
{
CameraModel model(
1.0f/depthConstant_, //fx
1.0f/depthConstant_, //fy
float(rgb_.cols/2) - 0.5f, //cx
float(rgb_.rows/2) - 0.5f, //cy
this->getLocalTransform(),
0,
rgb_.size());
data = SensorData(rgb_, depth_, model, this->getNextSeqID(), UTimer::now());
}
depth_ = cv::Mat();
rgb_ = cv::Mat();
depthConstant_ = 0.0f;
}
}
#else
UERROR("CameraOpenNI: RTAB-Map is not built with PCL having OpenNI support!");
#endif
return data;
}
} // namespace rtabmap

102
corelib/src/camera/CameraRGBDImages.cpp Normal file
View File

@@ -0,0 +1,102 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraRGBDImages.h>
namespace rtabmap
{
bool CameraRGBDImages::available()
{
return true;
}
CameraRGBDImages::CameraRGBDImages(
const std::string & pathRGBImages,
const std::string & pathDepthImages,
float depthScaleFactor,
float imageRate,
const Transform & localTransform) :
CameraImages(pathRGBImages, imageRate, localTransform)
{
UASSERT(depthScaleFactor >= 1.0);
cameraDepth_.setPath(pathDepthImages);
cameraDepth_.setDepth(true, depthScaleFactor);
}
CameraRGBDImages::~CameraRGBDImages()
{
}
bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::string & cameraName)
{
bool success = false;
if(CameraImages::init(calibrationFolder, cameraName) && cameraDepth_.init())
{
if(this->imagesCount() == cameraDepth_.imagesCount())
{
success = true;
}
else
{
UERROR("Cameras don't have the same number of images (%d vs %d)",
this->imagesCount(), cameraDepth_.imagesCount());
}
}
return success;
}
bool CameraRGBDImages::isCalibrated() const
{
return this->cameraModel().isValidForProjection();
}
std::string CameraRGBDImages::getSerial() const
{
return this->cameraModel().name();
}
SensorData CameraRGBDImages::captureImage(CameraInfo * info)
{
SensorData data;
SensorData rgb, depth;
rgb = CameraImages::captureImage(info);
if(!rgb.imageRaw().empty())
{
depth = cameraDepth_.takeImage();
if(!depth.depthRaw().empty())
{
data = SensorData(rgb.imageRaw(), depth.depthRaw(), rgb.cameraModels(), rgb.id(), rgb.stamp());
data.setGroundTruth(rgb.groundTruth());
}
}
return data;
}
} // namespace rtabmap

1076
corelib/src/camera/CameraRealSense.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

450
corelib/src/camera/CameraRealSense2.cpp Normal file
View File

@@ -0,0 +1,450 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraRealSense2.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UThreadC.h>
#include <rtabmap/utilite/UConversion.h>
#ifdef RTABMAP_REALSENSE2
#include <librealsense2/rs.hpp>
#include <librealsense2/rsutil.h>
#include <librealsense2/hpp/rs_processing.hpp>
#include <librealsense2/rs_advanced_mode.hpp>
#endif
namespace rtabmap
{
bool CameraRealSense2::available()
{
#ifdef RTABMAP_REALSENSE2
return true;
#else
return false;
#endif
}
CameraRealSense2::CameraRealSense2(
const std::string & device,
float imageRate,
const rtabmap::Transform & localTransform) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_REALSENSE2
,
ctx_(new rs2::context),
dev_(new rs2::device),
deviceId_(device),
syncer_(new rs2::syncer),
depth_scale_meters_(1.0f),
depthIntrinsics_(new rs2_intrinsics),
rgbIntrinsics_(new rs2_intrinsics),
depthToRGBExtrinsics_(new rs2_extrinsics),
emitterEnabled_(true),
irDepth_(false)
#endif
{
UDEBUG("");
}
CameraRealSense2::~CameraRealSense2()
{
#ifdef RTABMAP_REALSENSE2
delete ctx_;
delete dev_;
delete syncer_;
delete depthIntrinsics_;
delete rgbIntrinsics_;
delete depthToRGBExtrinsics_;
#endif
UDEBUG("");
}
#ifdef RTABMAP_REALSENSE2
void alignFrame(const rs2_intrinsics& from_intrin,
const rs2_intrinsics& other_intrin,
rs2::frame from_image,
uint32_t output_image_bytes_per_pixel,
const rs2_extrinsics& from_to_other,
cv::Mat & registeredDepth,
float depth_scale_meters)
{
static const auto meter_to_mm = 0.001f;
uint8_t* p_out_frame = registeredDepth.data;
auto from_vid_frame = from_image.as<rs2::video_frame>();
auto from_bytes_per_pixel = from_vid_frame.get_bytes_per_pixel();
static const auto blank_color = 0x00;
UASSERT(registeredDepth.total()*registeredDepth.channels()*registeredDepth.depth() == other_intrin.height * other_intrin.width * output_image_bytes_per_pixel);
memset(p_out_frame, blank_color, other_intrin.height * other_intrin.width * output_image_bytes_per_pixel);
auto p_from_frame = reinterpret_cast<const uint8_t*>(from_image.get_data());
auto from_stream_type = from_image.get_profile().stream_type();
float depth_units = ((from_stream_type == RS2_STREAM_DEPTH)? depth_scale_meters:1.f);
UASSERT(from_stream_type == RS2_STREAM_DEPTH);
UASSERT_MSG(depth_units > 0.0f, uFormat("depth_scale_meters=%f", depth_scale_meters).c_str());
#pragma omp parallel for schedule(dynamic)
for (int from_y = 0; from_y < from_intrin.height; ++from_y)
{
int from_pixel_index = from_y * from_intrin.width;
for (int from_x = 0; from_x < from_intrin.width; ++from_x, ++from_pixel_index)
{
// Skip over depth pixels with the value of zero
float depth = (from_stream_type == RS2_STREAM_DEPTH)?(depth_units * ((const uint16_t*)p_from_frame)[from_pixel_index]): 1.f;
if (depth)
{
// Map the top-left corner of the depth pixel onto the other image
float from_pixel[2] = { from_x - 0.5f, from_y - 0.5f }, from_point[3], other_point[3], other_pixel[2];
rs2_deproject_pixel_to_point(from_point, &from_intrin, from_pixel, depth);
rs2_transform_point_to_point(other_point, &from_to_other, from_point);
rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);
// Map the bottom-right corner of the depth pixel onto the other image
from_pixel[0] = from_x + 0.5f; from_pixel[1] = from_y + 0.5f;
rs2_deproject_pixel_to_point(from_point, &from_intrin, from_pixel, depth);
rs2_transform_point_to_point(other_point, &from_to_other, from_point);
rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);
if (other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height)
continue;
for (int y = other_y0; y <= other_y1; ++y)
{
for (int x = other_x0; x <= other_x1; ++x)
{
int out_pixel_index = y * other_intrin.width + x;
//Tranfer n-bit pixel to n-bit pixel
for (int i = 0; i < from_bytes_per_pixel; i++)
{
const auto out_offset = out_pixel_index * output_image_bytes_per_pixel + i;
const auto from_offset = from_pixel_index * output_image_bytes_per_pixel + i;
p_out_frame[out_offset] = p_from_frame[from_offset] * (depth_units / meter_to_mm);
}
}
}
}
}
}
}
#endif
bool CameraRealSense2::init(const std::string & calibrationFolder, const std::string & cameraName)
{
UDEBUG("");
#ifdef RTABMAP_REALSENSE2
UINFO("setupDevice...");
auto list = ctx_->query_devices();
if (0 == list.size())
{
UERROR("No RealSense2 devices were found!");
return false;
}
bool found=false;
for (auto&& dev : list)
{
auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
uint16_t pid;
std::stringstream ss;
ss << std::hex << pid_str;
ss >> pid;
UINFO("Device with serial number %s was found with product ID=%d.", sn, (int)pid);
if (deviceId_.empty() || deviceId_ == sn)
{
*dev_ = dev;
found=true;
break;
}
}
if (!found)
{
UERROR("The requested device %s is NOT found!", deviceId_.c_str());
return false;
}
ctx_->set_devices_changed_callback([this](rs2::event_information& info)
{
if (info.was_removed(*dev_))
{
UERROR("The device has been disconnected!");
}
});
auto camera_name = dev_->get_info(RS2_CAMERA_INFO_NAME);
UINFO("Device Name: %s", camera_name);
auto sn = dev_->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
UINFO("Device Serial No: %s", sn);
auto fw_ver = dev_->get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION);
UINFO("Device FW version: %s", fw_ver);
auto pid = dev_->get_info(RS2_CAMERA_INFO_PRODUCT_ID);
UINFO("Device Product ID: 0x%s", pid);
auto dev_sensors = dev_->query_sensors();
UINFO("Device Sensors: ");
std::vector<rs2::sensor> sensors(2); //0=rgb 1=depth
for(auto&& elem : dev_sensors)
{
std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME);
if ("Stereo Module" == module_name)
{
sensors[1] = elem;
sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
if(irDepth_)
{
sensors[0] = elem;
}
}
else if ("Coded-Light Depth Sensor" == module_name)
{
}
else if ("RGB Camera" == module_name)
{
if(!irDepth_)
{
sensors[0] = elem;
}
}
else if ("Wide FOV Camera" == module_name)
{
}
else if ("Motion Module" == module_name)
{
}
else
{
UERROR("Module Name \"%s\" isn't supported by LibRealSense!", module_name.c_str());
return false;
}
UINFO("%s was found.", elem.get_info(RS2_CAMERA_INFO_NAME));
}
UDEBUG("");
model_ = CameraModel();
rs2::stream_profile depthStreamProfile;
rs2::stream_profile rgbStreamProfile;
std::vector<std::vector<rs2::stream_profile> > profilesPerSensor(2);
for (unsigned int i=0; i<sensors.size(); ++i)
{
UDEBUG("i=%d", (int)i);
auto profiles = sensors[i].get_stream_profiles();
bool added = false;
UDEBUG("profiles=%d", (int)profiles.size());
for (auto& profile : profiles)
{
auto video_profile = profile.as<rs2::video_stream_profile>();
if (video_profile.format() == (i==1?RS2_FORMAT_Z16:irDepth_?RS2_FORMAT_Y8:RS2_FORMAT_RGB8) &&
video_profile.width() == 640 &&
video_profile.height() == 480 &&
video_profile.fps() == 30)
{
profilesPerSensor[irDepth_?1:i].push_back(profile);
auto intrinsic = video_profile.get_intrinsics();
if(i==1)
{
depthBuffer_ = cv::Mat(cv::Size(640, 480), CV_16UC1, cv::Scalar(0));
depthStreamProfile = profile;
*depthIntrinsics_ = intrinsic;
}
else
{
rgbBuffer_ = cv::Mat(cv::Size(640, 480), irDepth_?CV_8UC1:CV_8UC3, irDepth_?cv::Scalar(0):cv::Scalar(0, 0, 0));
model_ = CameraModel(camera_name, intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy, this->getLocalTransform(), 0, cv::Size(intrinsic.width, intrinsic.height));
rgbStreamProfile = profile;
*rgbIntrinsics_ = intrinsic;
}
added = true;
break;
}
}
if (!added)
{
UERROR("Given stream configuration is not supported by the device! "
"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, 640, 480, 30);
return false;
}
}
if(!model_.isValidForProjection())
{
UERROR("Calibration info not valid!");
return false;
}
*depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
for (unsigned int i=0; i<sensors.size(); ++i)
{
if(profilesPerSensor[i].size())
{
UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
sensors[i].open(profilesPerSensor[i]);
if(i ==1)
{
auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
depth_scale_meters_ = depth_sensor.get_depth_scale();
}
sensors[i].start(*syncer_);
}
}
uSleep(1000); // ignore the first frames
UINFO("Enabling streams...done!");
return true;
#else
UERROR("CameraRealSense: RTAB-Map is not built with RealSense2 support!");
return false;
#endif
}
bool CameraRealSense2::isCalibrated() const
{
return true;
}
std::string CameraRealSense2::getSerial() const
{
#ifdef RTABMAP_REALSENSE2
return dev_->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
#endif
return "NA";
}
void CameraRealSense2::setEmitterEnabled(bool enabled)
{
#ifdef RTABMAP_REALSENSE2
emitterEnabled_ = enabled;
#endif
}
void CameraRealSense2::setIRDepthFormat(bool enabled)
{
#ifdef RTABMAP_REALSENSE2
irDepth_ = enabled;
#endif
}
SensorData CameraRealSense2::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_REALSENSE2
try{
auto frameset = syncer_->wait_for_frames(5000);
UTimer timer;
while (frameset.size() != 2 && timer.elapsed() < 2.0)
{
// maybe there is a latency with the USB, try again in 100 ms (for the next 2 seconds)
frameset = syncer_->wait_for_frames(100);
}
if (frameset.size() == 2)
{
double stamp = UTimer::now();
UDEBUG("Frameset arrived.");
bool is_rgb_arrived = false;
bool is_depth_arrived = false;
rs2::frame rgb_frame;
rs2::frame depth_frame;
for (auto it = frameset.begin(); it != frameset.end(); ++it)
{
auto f = (*it);
auto stream_type = f.get_profile().stream_type();
if (stream_type == RS2_STREAM_COLOR || stream_type == RS2_STREAM_INFRARED)
{
rgb_frame = f;
is_rgb_arrived = true;
}
else if (stream_type == RS2_STREAM_DEPTH)
{
depth_frame = f;
is_depth_arrived = true;
}
}
if(is_rgb_arrived && is_depth_arrived)
{
auto from_image_frame = depth_frame.as<rs2::video_frame>();
cv::Mat depth;
if(irDepth_)
{
depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
}
else
{
depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type());
alignFrame(*depthIntrinsics_, *rgbIntrinsics_,
depth_frame, from_image_frame.get_bytes_per_pixel(),
*depthToRGBExtrinsics_, depth, depth_scale_meters_);
}
cv::Mat rgb = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data());
cv::Mat bgr;
if(rgb.channels() == 3)
{
cv::cvtColor(rgb, bgr, CV_RGB2BGR);
}
else
{
bgr = rgb.clone();
}
data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
}
else
{
UERROR("Not received depth and rgb");
}
}
else
{
UERROR("Missing frames (received %d)", (int)frameset.size());
}
}
catch(const std::exception& ex)
{
UERROR("An error has occurred during frame callback: %s", ex.what());
}
#else
UERROR("CameraRealSense2: RTAB-Map is not built with RealSense2 support!");
#endif
return data;
}
} // namespace rtabmap

439
corelib/src/camera/CameraStereoDC1394.cpp Normal file
View File

@@ -0,0 +1,439 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraStereoDC1394.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#ifdef RTABMAP_DC1394
#include <dc1394/dc1394.h>
#endif
namespace rtabmap
{
#ifdef RTABMAP_DC1394
class DC1394Device
{
public:
DC1394Device() :
camera_(0),
context_(0)
{
}
~DC1394Device()
{
if (camera_)
{
if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_OFF) ||
DC1394_SUCCESS != dc1394_capture_stop(camera_))
{
UWARN("unable to stop camera");
}
// Free resources
dc1394_capture_stop(camera_);
dc1394_camera_free(camera_);
camera_ = NULL;
}
if(context_)
{
dc1394_free(context_);
context_ = NULL;
}
}
const std::string & guid() const {return guid_;}
bool init()
{
if(camera_)
{
// Free resources
dc1394_capture_stop(camera_);
dc1394_camera_free(camera_);
camera_ = NULL;
}
// look for a camera
int err;
if(context_ == NULL)
{
context_ = dc1394_new ();
if (context_ == NULL)
{
UERROR( "Could not initialize dc1394_context.\n"
"Make sure /dev/raw1394 exists, you have access permission,\n"
"and libraw1394 development package is installed.");
return false;
}
}
dc1394camera_list_t *list;
err = dc1394_camera_enumerate(context_, &list);
if (err != DC1394_SUCCESS)
{
UERROR("Could not get camera list");
return false;
}
if (list->num == 0)
{
UERROR("No cameras found");
dc1394_camera_free_list (list);
return false;
}
uint64_t guid = list->ids[0].guid;
dc1394_camera_free_list (list);
// Create a camera
camera_ = dc1394_camera_new (context_, guid);
if (!camera_)
{
UERROR("Failed to initialize camera with GUID [%016lx]", guid);
return false;
}
uint32_t value[3];
value[0]= camera_->guid & 0xffffffff;
value[1]= (camera_->guid >>32) & 0x000000ff;
value[2]= (camera_->guid >>40) & 0xfffff;
guid_ = uFormat("%06x%02x%08x", value[2], value[1], value[0]);
UINFO("camera model: %s %s", camera_->vendor, camera_->model);
// initialize camera
// Enable IEEE1394b mode if the camera and bus support it
bool bmode = camera_->bmode_capable;
if (bmode
&& (DC1394_SUCCESS !=
dc1394_video_set_operation_mode(camera_,
DC1394_OPERATION_MODE_1394B)))
{
bmode = false;
UWARN("failed to set IEEE1394b mode");
}
// start with highest speed supported
dc1394speed_t request = DC1394_ISO_SPEED_3200;
int rate = 3200;
if (!bmode)
{
// not IEEE1394b capable: so 400Mb/s is the limit
request = DC1394_ISO_SPEED_400;
rate = 400;
}
// round requested speed down to next-lower defined value
while (rate > 400)
{
if (request <= DC1394_ISO_SPEED_MIN)
{
// get current ISO speed of the device
dc1394speed_t curSpeed;
if (DC1394_SUCCESS == dc1394_video_get_iso_speed(camera_, &curSpeed) && curSpeed <= DC1394_ISO_SPEED_MAX)
{
// Translate curSpeed back to an int for the parameter
// update, works as long as any new higher speeds keep
// doubling.
request = curSpeed;
rate = 100 << (curSpeed - DC1394_ISO_SPEED_MIN);
}
else
{
UWARN("Unable to get ISO speed; assuming 400Mb/s");
rate = 400;
request = DC1394_ISO_SPEED_400;
}
break;
}
// continue with next-lower possible value
request = (dc1394speed_t) ((int) request - 1);
rate = rate / 2;
}
// set the requested speed
if (DC1394_SUCCESS != dc1394_video_set_iso_speed(camera_, request))
{
UERROR("Failed to set iso speed");
return false;
}
// set video mode
dc1394video_modes_t vmodes;
err = dc1394_video_get_supported_modes(camera_, &vmodes);
if (err != DC1394_SUCCESS)
{
UERROR("unable to get supported video modes");
return (dc1394video_mode_t) 0;
}
// see if requested mode is available
bool found = false;
dc1394video_mode_t videoMode = DC1394_VIDEO_MODE_FORMAT7_3; // bumblebee
for (uint32_t i = 0; i < vmodes.num; ++i)
{
if (vmodes.modes[i] == videoMode)
{
found = true;
}
}
if(!found)
{
UERROR("unable to get video mode %d", videoMode);
return false;
}
if (DC1394_SUCCESS != dc1394_video_set_mode(camera_, videoMode))
{
UERROR("Failed to set video mode %d", videoMode);
return false;
}
// special handling for Format7 modes
if (dc1394_is_video_mode_scalable(videoMode) == DC1394_TRUE)
{
if (DC1394_SUCCESS != dc1394_format7_set_color_coding(camera_, videoMode, DC1394_COLOR_CODING_RAW16))
{
UERROR("Could not set color coding");
return false;
}
uint32_t packetSize;
if (DC1394_SUCCESS != dc1394_format7_get_recommended_packet_size(camera_, videoMode, &packetSize))
{
UERROR("Could not get default packet size");
return false;
}
if (DC1394_SUCCESS != dc1394_format7_set_packet_size(camera_, videoMode, packetSize))
{
UERROR("Could not set packet size");
return false;
}
}
else
{
UERROR("Video is not in mode scalable");
}
// start the device streaming data
// Set camera to use DMA, improves performance.
if (DC1394_SUCCESS != dc1394_capture_setup(camera_, 4, DC1394_CAPTURE_FLAGS_DEFAULT))
{
UERROR("Failed to open device!");
return false;
}
// Start transmitting camera data
if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_ON))
{
UERROR("Failed to start device!");
return false;
}
return true;
}
bool getImages(cv::Mat & left, cv::Mat & right)
{
if(camera_)
{
dc1394video_frame_t * frame = NULL;
UDEBUG("[%016lx] waiting camera", camera_->guid);
dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame);
if (!frame)
{
UERROR("Unable to capture frame");
return false;
}
dc1394video_frame_t frame1 = *frame;
// deinterlace frame into two imagesCount one on top the other
size_t frame1_size = frame->total_bytes;
frame1.image = (unsigned char *) malloc(frame1_size);
frame1.allocated_image_bytes = frame1_size;
frame1.color_coding = DC1394_COLOR_CODING_RAW8;
int err = dc1394_deinterlace_stereo_frames(frame, &frame1, DC1394_STEREO_METHOD_INTERLACED);
if (err != DC1394_SUCCESS)
{
free(frame1.image);
dc1394_capture_enqueue(camera_, frame);
UERROR("Could not extract stereo frames");
return false;
}
uint8_t* capture_buffer = reinterpret_cast<uint8_t *>(frame1.image);
UASSERT(capture_buffer);
cv::Mat image(frame->size[1], frame->size[0], CV_8UC3);
cv::Mat image2 = image.clone();
//DC1394_COLOR_CODING_RAW16:
//DC1394_COLOR_FILTER_BGGR
cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer), left, CV_BayerRG2BGR);
cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer+image.total()), right, CV_BayerRG2GRAY);
dc1394_capture_enqueue(camera_, frame);
free(frame1.image);
return true;
}
return false;
}
private:
dc1394camera_t *camera_;
dc1394_t *context_;
std::string guid_;
};
#endif
bool CameraStereoDC1394::available()
{
#ifdef RTABMAP_DC1394
return true;
#else
return false;
#endif
}
CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localTransform) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_DC1394
,
device_(0)
#endif
{
#ifdef RTABMAP_DC1394
device_ = new DC1394Device();
#endif
}
CameraStereoDC1394::~CameraStereoDC1394()
{
#ifdef RTABMAP_DC1394
delete device_;
#endif
}
bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef RTABMAP_DC1394
if(device_)
{
bool ok = device_->init();
if(ok)
{
// look for calibration files
if(!calibrationFolder.empty())
{
if(!stereoModel_.load(calibrationFolder, cameraName.empty()?device_->guid():cameraName, false))
{
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
cameraName.empty()?device_->guid().c_str():cameraName.c_str(), calibrationFolder.c_str());
}
else
{
UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
stereoModel_.left().fx(),
stereoModel_.left().cx(),
stereoModel_.left().cy(),
stereoModel_.baseline());
}
}
}
return ok;
}
#else
UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
#endif
return false;
}
bool CameraStereoDC1394::isCalibrated() const
{
#ifdef RTABMAP_DC1394
return stereoModel_.isValidForProjection();
#else
return false;
#endif
}
std::string CameraStereoDC1394::getSerial() const
{
#ifdef RTABMAP_DC1394
if(device_)
{
return device_->guid();
}
#endif
return "";
}
SensorData CameraStereoDC1394::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_DC1394
if(device_)
{
cv::Mat left, right;
device_->getImages(left, right);
if(!left.empty() && !right.empty())
{
// Rectification
if(stereoModel_.left().isValidForRectification())
{
left = stereoModel_.left().rectifyImage(left);
}
if(stereoModel_.right().isValidForRectification())
{
right = stereoModel_.right().rectifyImage(right);
}
StereoCameraModel model;
if(stereoModel_.isValidForProjection())
{
model = StereoCameraModel(
stereoModel_.left().fx(), //fx
stereoModel_.left().fy(), //fy
stereoModel_.left().cx(), //cx
stereoModel_.left().cy(), //cy
stereoModel_.baseline(),
this->getLocalTransform(),
left.size());
}
data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
}
}
#else
UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
#endif
return data;
}
} // namespace rtabmap

319
corelib/src/camera/CameraStereoFlyCapture2.cpp Normal file
View File

@@ -0,0 +1,319 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraStereoFlyCapture2.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#ifdef RTABMAP_FLYCAPTURE2
#include <triclops.h>
#include <fc2triclops.h>
#endif
namespace rtabmap
{
CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transform & localTransform) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_FLYCAPTURE2
,
camera_(0),
triclopsCtx_(0)
#endif
{
#ifdef RTABMAP_FLYCAPTURE2
camera_ = new FlyCapture2::Camera();
#endif
}
CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
{
#ifdef RTABMAP_FLYCAPTURE2
// Close the camera
camera_->StopCapture();
camera_->Disconnect();
// Destroy the Triclops context
triclopsDestroyContext( triclopsCtx_ ) ;
delete camera_;
#endif
}
bool CameraStereoFlyCapture2::available()
{
#ifdef RTABMAP_FLYCAPTURE2
return true;
#else
return false;
#endif
}
bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef RTABMAP_FLYCAPTURE2
if(camera_)
{
// Close the camera
camera_->StopCapture();
camera_->Disconnect();
}
if(triclopsCtx_)
{
triclopsDestroyContext(triclopsCtx_);
triclopsCtx_ = 0;
}
// connect camera
FlyCapture2::Error fc2Error = camera_->Connect();
if(fc2Error != FlyCapture2::PGRERROR_OK)
{
UERROR("Failed to connect the camera.");
return false;
}
// configure camera
Fc2Triclops::StereoCameraMode mode = Fc2Triclops::TWO_CAMERA_NARROW;
if(Fc2Triclops::setStereoMode(*camera_, mode ))
{
UERROR("Failed to set stereo mode.");
return false;
}
// generate the Triclops context
FlyCapture2::CameraInfo camInfo;
if(camera_->GetCameraInfo(&camInfo) != FlyCapture2::PGRERROR_OK)
{
UERROR("Failed to get camera info.");
return false;
}
float dummy;
unsigned packetSz;
FlyCapture2::Format7ImageSettings imageSettings;
int maxWidth = 640;
int maxHeight = 480;
if(camera_->GetFormat7Configuration(&imageSettings, &packetSz, &dummy) == FlyCapture2::PGRERROR_OK)
{
maxHeight = imageSettings.height;
maxWidth = imageSettings.width;
}
// Get calibration from th camera
if(Fc2Triclops::getContextFromCamera(camInfo.serialNumber, &triclopsCtx_))
{
UERROR("Failed to get calibration from the camera.");
return false;
}
float fx, cx, cy, baseline;
triclopsGetFocalLength(triclopsCtx_, &fx);
triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
triclopsGetBaseline(triclopsCtx_, &baseline);
UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", fx, cx, cy, baseline);
triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW );
UASSERT(triclopsSetResolutionAndPrepare(triclopsCtx_, maxHeight, maxWidth, maxHeight, maxWidth) == Fc2Triclops::ERRORTYPE_OK);
if(camera_->StartCapture() != FlyCapture2::PGRERROR_OK)
{
UERROR("Failed to start capture.");
return false;
}
return true;
#else
UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
#endif
return false;
}
bool CameraStereoFlyCapture2::isCalibrated() const
{
#ifdef RTABMAP_FLYCAPTURE2
if(triclopsCtx_)
{
float fx, cx, cy, baseline;
triclopsGetFocalLength(triclopsCtx_, &fx);
triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
triclopsGetBaseline(triclopsCtx_, &baseline);
return fx > 0.0f && cx > 0.0f && cy > 0.0f && baseline > 0.0f;
}
#endif
return false;
}
std::string CameraStereoFlyCapture2::getSerial() const
{
#ifdef RTABMAP_FLYCAPTURE2
if(camera_ && camera_->IsConnected())
{
FlyCapture2::CameraInfo camInfo;
if(camera_->GetCameraInfo(&camInfo) == FlyCapture2::PGRERROR_OK)
{
return uNumber2Str(camInfo.serialNumber);
}
}
#endif
return "";
}
// struct containing image needed for processing
#ifdef RTABMAP_FLYCAPTURE2
struct ImageContainer
{
FlyCapture2::Image tmp[2];
FlyCapture2::Image unprocessed[2];
} ;
#endif
SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_FLYCAPTURE2
if(camera_ && triclopsCtx_ && camera_->IsConnected())
{
// grab image from camera.
// this image contains both right and left imagesCount
FlyCapture2::Image grabbedImage;
if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK)
{
// right and left image extracted from grabbed image
ImageContainer imageCont;
// generate triclops input from grabbed image
FlyCapture2::Image imageRawRight;
FlyCapture2::Image imageRawLeft;
FlyCapture2::Image * unprocessedImage = imageCont.unprocessed;
// Convert the pixel interleaved raw data to de-interleaved and color processed data
if(Fc2Triclops::unpackUnprocessedRawOrMono16Image(
grabbedImage,
true /*assume little endian*/,
imageRawLeft /* right */,
imageRawRight /* left */) == Fc2Triclops::ERRORTYPE_OK)
{
// convert to color
FlyCapture2::Image srcImgRightRef(imageRawRight);
FlyCapture2::Image srcImgLeftRef(imageRawLeft);
bool ok = true;;
if ( srcImgRightRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK ||
srcImgLeftRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK)
{
ok = false;
}
if(ok)
{
FlyCapture2::Image imageColorRight;
FlyCapture2::Image imageColorLeft;
if ( srcImgRightRef.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &imageColorRight) != FlyCapture2::PGRERROR_OK ||
srcImgLeftRef.Convert(FlyCapture2::PIXEL_FORMAT_BGRU, &imageColorLeft) != FlyCapture2::PGRERROR_OK)
{
ok = false;
}
if(ok)
{
//RECTIFY RIGHT
TriclopsInput triclopsColorInputs;
triclopsBuildRGBTriclopsInput(
grabbedImage.GetCols(),
grabbedImage.GetRows(),
imageColorRight.GetStride(),
(unsigned long)grabbedImage.GetTimeStamp().seconds,
(unsigned long)grabbedImage.GetTimeStamp().microSeconds,
imageColorRight.GetData(),
imageColorRight.GetData(),
imageColorRight.GetData(),
&triclopsColorInputs);
triclopsRectify(triclopsCtx_, const_cast<TriclopsInput *>(&triclopsColorInputs) );
// Retrieve the rectified image from the triclops context
TriclopsImage rectifiedImage;
triclopsGetImage( triclopsCtx_,
TriImg_RECTIFIED,
TriCam_REFERENCE,
&rectifiedImage );
cv::Mat left,right;
right = cv::Mat(rectifiedImage.nrows, rectifiedImage.ncols, CV_8UC1, rectifiedImage.data).clone();
//RECTIFY LEFT COLOR
triclopsBuildPackedTriclopsInput(
grabbedImage.GetCols(),
grabbedImage.GetRows(),
imageColorLeft.GetStride(),
(unsigned long)grabbedImage.GetTimeStamp().seconds,
(unsigned long)grabbedImage.GetTimeStamp().microSeconds,
imageColorLeft.GetData(),
&triclopsColorInputs );
cv::Mat pixelsLeftBuffer( grabbedImage.GetRows(), grabbedImage.GetCols(), CV_8UC4);
TriclopsPackedColorImage colorImage;
triclopsSetPackedColorImageBuffer(
triclopsCtx_,
TriCam_LEFT,
(TriclopsPackedColorPixel*)pixelsLeftBuffer.data );
triclopsRectifyPackedColorImage(
triclopsCtx_,
TriCam_LEFT,
&triclopsColorInputs,
&colorImage );
cv::cvtColor(pixelsLeftBuffer, left, CV_RGBA2RGB);
// Set calibration stuff
float fx, cy, cx, baseline;
triclopsGetFocalLength(triclopsCtx_, &fx);
triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
triclopsGetBaseline(triclopsCtx_, &baseline);
StereoCameraModel model(
fx,
fx,
cx,
cy,
baseline,
this->getLocalTransform(),
left.size());
data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
}
}
}
}
}
#else
UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
#endif
return data;
}
} // namespace rtabmap

203
corelib/src/camera/CameraStereoImages.cpp Normal file
View File

@@ -0,0 +1,203 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraStereoImages.h>
#include <rtabmap/utilite/UStl.h>
namespace rtabmap
{
bool CameraStereoImages::available()
{
return true;
}
CameraStereoImages::CameraStereoImages(
const std::string & pathLeftImages,
const std::string & pathRightImages,
bool rectifyImages,
float imageRate,
const Transform & localTransform) :
CameraImages(pathLeftImages, imageRate, localTransform),
camera2_(new CameraImages(pathRightImages))
{
this->setImagesRectified(rectifyImages);
}
CameraStereoImages::CameraStereoImages(
const std::string & pathLeftRightImages,
bool rectifyImages,
float imageRate,
const Transform & localTransform) :
CameraImages("", imageRate, localTransform),
camera2_(0)
{
std::vector<std::string> paths = uListToVector(uSplit(pathLeftRightImages, uStrContains(pathLeftRightImages, ":")?':':';'));
if(paths.size() >= 1)
{
this->setPath(paths[0]);
this->setImagesRectified(rectifyImages);
if(paths.size() >= 2)
{
camera2_ = new CameraImages(paths[1]);
}
}
else
{
UERROR("The path is empty!");
}
}
CameraStereoImages::~CameraStereoImages()
{
UDEBUG("");
delete camera2_;
UDEBUG("");
}
bool CameraStereoImages::init(const std::string & calibrationFolder, const std::string & cameraName)
{
// look for calibration files
if(!calibrationFolder.empty() && !cameraName.empty())
{
if(!stereoModel_.load(calibrationFolder, cameraName, false) && !stereoModel_.isValidForProjection())
{
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
cameraName.c_str(), calibrationFolder.c_str());
}
else
{
UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
stereoModel_.left().fx(),
stereoModel_.left().cx(),
stereoModel_.left().cy(),
stereoModel_.baseline());
}
}
stereoModel_.setLocalTransform(this->getLocalTransform());
stereoModel_.setName(cameraName);
if(this->isImagesRectified() && !stereoModel_.isValidForRectification())
{
UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
return false;
}
//desactivate before init as we will do it in this class instead for convenience
bool rectify = this->isImagesRectified();
this->setImagesRectified(false);
bool success = false;
if(CameraImages::init())
{
if(camera2_)
{
camera2_->setBayerMode(this->getBayerMode());
if(camera2_->init())
{
if(this->imagesCount() == camera2_->imagesCount())
{
success = true;
}
else
{
UERROR("Cameras don't have the same number of images (%d vs %d)",
this->imagesCount(), camera2_->imagesCount());
}
}
else
{
UERROR("Cannot initialize the second camera.");
}
}
else
{
success = true;
}
}
this->setImagesRectified(rectify); // reset the flag
return success;
}
bool CameraStereoImages::isCalibrated() const
{
return stereoModel_.isValidForProjection();
}
std::string CameraStereoImages::getSerial() const
{
return stereoModel_.name();
}
SensorData CameraStereoImages::captureImage(CameraInfo * info)
{
SensorData data;
SensorData left, right;
left = CameraImages::captureImage(info);
if(!left.imageRaw().empty())
{
if(camera2_)
{
right = camera2_->takeImage(info);
}
else
{
right = this->takeImage(info);
}
if(!right.imageRaw().empty())
{
// Rectification
cv::Mat leftImage = left.imageRaw();
cv::Mat rightImage = right.imageRaw();
if(rightImage.type() != CV_8UC1)
{
cv::Mat tmp;
cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
rightImage = tmp;
}
if(this->isImagesRectified() && stereoModel_.isValidForRectification())
{
leftImage = stereoModel_.left().rectifyImage(leftImage);
rightImage = stereoModel_.right().rectifyImage(rightImage);
}
if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
{
stereoModel_.setImageSize(leftImage.size());
}
data = SensorData(left.laserScanRaw(), leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
data.setGroundTruth(left.groundTruth());
}
}
return data;
}
} // namespace rtabmap

270
corelib/src/camera/CameraStereoVideo.cpp Normal file
View File

@@ -0,0 +1,270 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraStereoVideo.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
namespace rtabmap
{
bool CameraStereoVideo::available()
{
return true;
}
CameraStereoVideo::CameraStereoVideo(
const std::string & path,
bool rectifyImages,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
path_(path),
rectifyImages_(rectifyImages),
src_(CameraVideo::kVideoFile),
usbDevice_(0),
usbDevice2_(-1)
{
}
CameraStereoVideo::CameraStereoVideo(
const std::string & pathLeft,
const std::string & pathRight,
bool rectifyImages,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
path_(pathLeft),
path2_(pathRight),
rectifyImages_(rectifyImages),
src_(CameraVideo::kVideoFile),
usbDevice_(0),
usbDevice2_(-1)
{
}
CameraStereoVideo::CameraStereoVideo(
int device,
bool rectifyImages,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
rectifyImages_(rectifyImages),
src_(CameraVideo::kUsbDevice),
usbDevice_(device),
usbDevice2_(-1)
{
}
CameraStereoVideo::CameraStereoVideo(
int deviceLeft,
int deviceRight,
bool rectifyImages,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
rectifyImages_(rectifyImages),
src_(CameraVideo::kUsbDevice),
usbDevice_(deviceLeft),
usbDevice2_(deviceRight)
{
}
CameraStereoVideo::~CameraStereoVideo()
{
capture_.release();
capture2_.release();
}
bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
{
cameraName_ = cameraName;
if(capture_.isOpened())
{
capture_.release();
}
if(capture2_.isOpened())
{
capture2_.release();
}
if (src_ == CameraVideo::kUsbDevice)
{
capture_.open(usbDevice_);
if(usbDevice2_ < 0)
{
ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
}
else
{
ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on devices %d and %d", usbDevice_, usbDevice2_);
capture2_.open(usbDevice2_);
}
}
else if (src_ == CameraVideo::kVideoFile)
{
capture_.open(path_.c_str());
if(path2_.empty())
{
ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
}
else
{
ULOGGER_DEBUG("CameraStereoVideo: filenames=\"%s\" and \"%s\"", path_.c_str(), path2_.c_str());
capture2_.open(path2_.c_str());
}
}
else
{
ULOGGER_ERROR("CameraStereoVideo: Unknown source...");
}
if(!capture_.isOpened() || ((!path2_.empty() || usbDevice2_>=0) && !capture2_.isOpened()))
{
ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!");
capture_.release();
capture2_.release();
return false;
}
if (cameraName_.empty())
{
unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID);
if (guid != 0 && guid != 0xffffffff)
{
cameraName_ = uFormat("%08x", guid);
}
}
// look for calibration files
if(!calibrationFolder.empty() && !cameraName_.empty())
{
if(!stereoModel_.load(calibrationFolder, cameraName_, false))
{
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
cameraName_.c_str(), calibrationFolder.c_str());
}
else
{
UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
stereoModel_.left().fx(),
stereoModel_.left().cx(),
stereoModel_.left().cy(),
stereoModel_.baseline());
}
}
stereoModel_.setLocalTransform(this->getLocalTransform());
if(rectifyImages_ && !stereoModel_.isValidForRectification())
{
UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
return false;
}
return true;
}
bool CameraStereoVideo::isCalibrated() const
{
return stereoModel_.isValidForProjection();
}
std::string CameraStereoVideo::getSerial() const
{
return cameraName_;
}
SensorData CameraStereoVideo::captureImage(CameraInfo * info)
{
SensorData data;
cv::Mat img;
if(capture_.isOpened() && ((path2_.empty() && usbDevice2_ < 0) || capture2_.isOpened()))
{
cv::Mat leftImage;
cv::Mat rightImage;
if(path2_.empty() && usbDevice2_ < 0)
{
if(!capture_.read(img))
{
return data;
}
// Side by side stream
leftImage = cv::Mat(img, cv::Rect( 0, 0, img.size().width/2, img.size().height ));
rightImage = cv::Mat(img, cv::Rect( img.size().width/2, 0, img.size().width/2, img.size().height ));
}
else if(!capture_.read(leftImage) || !capture2_.read(rightImage))
{
return data;
}
else if(leftImage.cols != rightImage.cols || leftImage.rows != rightImage.rows)
{
UERROR("Left and right streams don't have image of the same size: left=%dx%d right=%dx%d",
leftImage.cols, leftImage.rows, rightImage.cols, rightImage.rows);
return data;
}
// Rectification
bool rightCvt = false;
if(rightImage.type() != CV_8UC1)
{
cv::Mat tmp;
cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
rightImage = tmp;
rightCvt = true;
}
if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
{
leftImage = stereoModel_.left().rectifyImage(leftImage);
rightImage = stereoModel_.right().rectifyImage(rightImage);
}
else
{
leftImage = leftImage.clone();
if(!rightCvt)
{
rightImage = rightImage.clone();
}
}
if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
{
stereoModel_.setImageSize(leftImage.size());
}
data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), UTimer::now());
}
else
{
ULOGGER_WARN("The camera must be initialized before requesting an image.");
}
return data;
}
} // namespace rtabmap

390
corelib/src/camera/CameraStereoZed.cpp Normal file
View File

@@ -0,0 +1,390 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraStereoZed.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UThreadC.h>
#include <rtabmap/utilite/UConversion.h>
#ifdef RTABMAP_ZED
#include <sl/Camera.hpp>
#endif
namespace rtabmap
{
bool CameraStereoZed::available()
{
#ifdef RTABMAP_ZED
return true;
#else
return false;
#endif
}
CameraStereoZed::CameraStereoZed(
int deviceId,
int resolution,
int quality,
int sensingMode,
int confidenceThr,
bool computeOdometry,
float imageRate,
const Transform & localTransform,
bool selfCalibration) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_ZED
,
zed_(0),
src_(CameraVideo::kUsbDevice),
usbDevice_(deviceId),
svoFilePath_(""),
resolution_(resolution),
quality_(quality),
selfCalibration_(selfCalibration),
sensingMode_(sensingMode),
confidenceThr_(confidenceThr),
computeOdometry_(computeOdometry),
lost_(true)
#endif
{
UDEBUG("");
#ifdef RTABMAP_ZED
UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
#endif
}
CameraStereoZed::CameraStereoZed(
const std::string & filePath,
int quality,
int sensingMode,
int confidenceThr,
bool computeOdometry,
float imageRate,
const Transform & localTransform,
bool selfCalibration) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_ZED
,
zed_(0),
src_(CameraVideo::kVideoFile),
usbDevice_(0),
svoFilePath_(filePath),
resolution_(2),
quality_(quality),
selfCalibration_(selfCalibration),
sensingMode_(sensingMode),
confidenceThr_(confidenceThr),
computeOdometry_(computeOdometry),
lost_(true)
#endif
{
UDEBUG("");
#ifdef RTABMAP_ZED
UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
#endif
}
CameraStereoZed::~CameraStereoZed()
{
#ifdef RTABMAP_ZED
delete zed_;
#endif
}
bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
{
UDEBUG("");
#ifdef RTABMAP_ZED
if(zed_)
{
delete zed_;
zed_ = 0;
}
lost_ = true;
sl::InitParameters param;
param.camera_resolution=static_cast<sl::RESOLUTION>(resolution_);
param.camera_fps=getImageRate();
param.camera_linux_id=usbDevice_;
param.depth_mode=(sl::DEPTH_MODE)quality_;
param.coordinate_units=sl::UNIT_METER;
param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ;
param.sdk_verbose=true;
param.sdk_gpu_id=-1;
param.depth_minimum_distance=-1;
param.camera_disable_self_calib=!selfCalibration_;
sl::ERROR_CODE r = sl::ERROR_CODE::SUCCESS;
if(src_ == CameraVideo::kVideoFile)
{
UINFO("svo file = %s", svoFilePath_.c_str());
zed_ = new sl::Camera(); // Use in SVO playback mode
param.svo_input_filename=svoFilePath_.c_str();
r = zed_->open(param);
}
else
{
UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_);
zed_ = new sl::Camera(); // Use in Live Mode
r = zed_->open(param);
}
if(r!=sl::ERROR_CODE::SUCCESS)
{
UERROR("Camera initialization failed: \"%s\"", toString(r).c_str());
delete zed_;
zed_ = 0;
return false;
}
UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false");
UDEBUG("");
if(quality_!=sl::DEPTH_MODE_NONE)
{
zed_->setConfidenceThreshold(confidenceThr_);
}
if (computeOdometry_)
{
sl::TrackingParameters tparam;
tparam.enable_spatial_memory=false;
zed_->enableTracking(tparam);
if(r!=sl::ERROR_CODE::SUCCESS)
{
UERROR("Camera tracking initialization failed: \"%s\"", toString(r).c_str());
}
}
sl::CameraInformation infos = zed_->getCameraInformation();
sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
sl::Resolution res = stereoParams->left_cam.image_size;
stereoModel_ = StereoCameraModel(
stereoParams->left_cam.fx,
stereoParams->left_cam.fy,
stereoParams->left_cam.cx,
stereoParams->left_cam.cy,
stereoParams->T[0],//baseline
this->getLocalTransform(),
cv::Size(res.width, res.height));
UINFO("Calibration: fx=%f, fy=%f, cx=%f, cy=%f, baseline=%f, width=%d, height=%d, transform=%s",
stereoParams->left_cam.fx,
stereoParams->left_cam.fy,
stereoParams->left_cam.cx,
stereoParams->left_cam.cy,
stereoParams->T[0],//baseline
(int)res.width,
(int)res.height,
this->getLocalTransform().prettyPrint().c_str());
return true;
#else
UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
#endif
return false;
}
bool CameraStereoZed::isCalibrated() const
{
#ifdef RTABMAP_ZED
return stereoModel_.isValidForProjection();
#else
return false;
#endif
}
std::string CameraStereoZed::getSerial() const
{
#ifdef RTABMAP_ZED
if(zed_)
{
return uFormat("%x", zed_->getCameraInformation ().serial_number);
}
#endif
return "";
}
bool CameraStereoZed::odomProvided() const
{
#ifdef RTABMAP_ZED
return computeOdometry_;
#else
return false;
#endif
}
#ifdef RTABMAP_ZED
static cv::Mat slMat2cvMat(sl::Mat& input) {
//convert MAT_TYPE to CV_TYPE
int cv_type = -1;
switch (input.getDataType()) {
case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break;
case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break;
case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break;
case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break;
case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break;
case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break;
case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break;
case sl::MAT_TYPE_8U_C4: cv_type = CV_8UC4; break;
default: break;
}
// cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr<T>())
//cv::Mat and sl::Mat will share the same memory pointer
return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM_CPU));
}
Transform zedPoseToTransform(const sl::Pose & pose)
{
return Transform(
pose.pose_data.m[0], pose.pose_data.m[1], pose.pose_data.m[2], pose.pose_data.m[3],
pose.pose_data.m[4], pose.pose_data.m[5], pose.pose_data.m[6], pose.pose_data.m[7],
pose.pose_data.m[8], pose.pose_data.m[9], pose.pose_data.m[10], pose.pose_data.m[11]);
}
#endif
SensorData CameraStereoZed::captureImage(CameraInfo * info)
{
SensorData data;
#ifdef RTABMAP_ZED
sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA);
if(zed_)
{
UTimer timer;
bool res = zed_->grab(rparam);
while (src_ == CameraVideo::kUsbDevice && res!=sl::SUCCESS && timer.elapsed() < 2.0)
{
// maybe there is a latency with the USB, try again in 10 ms (for the next 2 seconds)
uSleep(10);
res = zed_->grab(rparam);
}
if(res==sl::SUCCESS)
{
// get left image
sl::Mat tmp;
zed_->retrieveImage(tmp,sl::VIEW_LEFT);
cv::Mat rgbaLeft = slMat2cvMat(tmp);
cv::Mat left;
cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
if(quality_ > 0)
{
// get depth image
cv::Mat depth;
sl::Mat tmp;
zed_->retrieveMeasure(tmp,sl::MEASURE_DEPTH);
slMat2cvMat(tmp).copyTo(depth);
data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
}
else
{
// get right image
sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_RIGHT );
cv::Mat rgbaRight = slMat2cvMat(tmp);
cv::Mat right;
cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
}
if (computeOdometry_ && info)
{
sl::Pose pose;
sl::TRACKING_STATE tracking_state = zed_->getPosition(pose);
if (tracking_state == sl::TRACKING_STATE_OK)
{
int trackingConfidence = pose.pose_confidence;
// FIXME What does pose_confidence == -1 mean?
if (trackingConfidence>0)
{
info->odomPose = zedPoseToTransform(pose);
if (!info->odomPose.isNull())
{
//transform x->forward, y->left, z->up
Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
if (lost_)
{
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
lost_ = false;
UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
}
else
{
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
}
}
else
{
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
lost_ = true;
UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
}
}
else
{
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
lost_ = true;
UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
}
}
else
{
UWARN("Tracking not ok: state=\"%s\"", toString(tracking_state).c_str());
}
}
}
else if(src_ == CameraVideo::kUsbDevice)
{
UERROR("CameraStereoZed: Failed to grab images after 2 seconds!");
}
else
{
UWARN("CameraStereoZed: end of stream is reached!");
}
}
#else
UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
#endif
return data;
}
} // namespace rtabmap

177
corelib/src/camera/CameraVideo.cpp Normal file
View File

@@ -0,0 +1,177 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraVideo.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
namespace rtabmap
{
CameraVideo::CameraVideo(
int usbDevice,
bool rectifyImages,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
_rectifyImages(rectifyImages),
_src(kUsbDevice),
_usbDevice(usbDevice)
{
}
CameraVideo::CameraVideo(
const std::string & filePath,
bool rectifyImages,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
_filePath(filePath),
_rectifyImages(rectifyImages),
_src(kVideoFile),
_usbDevice(0)
{
}
CameraVideo::~CameraVideo()
{
_capture.release();
}
bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
{
_guid = cameraName;
if(_capture.isOpened())
{
_capture.release();
}
if(_src == kUsbDevice)
{
ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d", _usbDevice);
_capture.open(_usbDevice);
}
else if(_src == kVideoFile)
{
ULOGGER_DEBUG("Camera: filename=\"%s\"", _filePath.c_str());
_capture.open(_filePath.c_str());
}
else
{
ULOGGER_ERROR("Camera: Unknown source...");
}
if(!_capture.isOpened())
{
ULOGGER_ERROR("Camera: Failed to create a capture object!");
_capture.release();
return false;
}
else
{
if (_guid.empty())
{
unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
if (guid != 0 && guid != 0xffffffff)
{
_guid = uFormat("%08x", guid);
}
}
// look for calibration files
if(!calibrationFolder.empty() && !_guid.empty())
{
if(!_model.load(calibrationFolder, _guid))
{
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
_guid.c_str(), calibrationFolder.c_str());
}
else
{
UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
_model.fx(),
_model.fy(),
_model.cx(),
_model.cy());
}
}
_model.setLocalTransform(this->getLocalTransform());
if(_rectifyImages && !_model.isValidForRectification())
{
UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");
return false;
}
}
return true;
}
bool CameraVideo::isCalibrated() const
{
return _model.isValidForProjection();
}
std::string CameraVideo::getSerial() const
{
return _guid;
}
SensorData CameraVideo::captureImage(CameraInfo * info)
{
cv::Mat img;
if(_capture.isOpened())
{
if(_capture.read(img))
{
if(_model.imageHeight() == 0 || _model.imageWidth() == 0)
{
_model.setImageSize(img.size());
}
if(_model.isValidForRectification() && _rectifyImages)
{
img = _model.rectifyImage(img);
}
else
{
// clone required
img = img.clone();
}
}
else if(_usbDevice)
{
UERROR("Camera has been disconnected!");
}
}
else
{
ULOGGER_WARN("The camera must be initialized before requesting an image.");
}
return SensorData(img, _model, this->getNextSeqID(), UTimer::now());
}
} // namespace rtabmap

2
corelib/src/OdometryDVO.cpp → corelib/src/odometry/OdometryDVO.cpp
View File

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/OdometryDVO.h"
#include "rtabmap/core/odometry/OdometryDVO.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/utilite/ULogger.h"

2
corelib/src/OdometryF2F.cpp → corelib/src/odometry/OdometryF2F.cpp
View File

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/OdometryF2F.h"
#include "rtabmap/core/odometry/OdometryF2F.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Registration.h"
#include "rtabmap/core/EpipolarGeometry.h"

2
corelib/src/OdometryF2M.cpp → corelib/src/odometry/OdometryF2M.cpp
View File

@@ -45,7 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UMath.h"
#include "rtabmap/utilite/UConversion.h"
#include <opencv2/calib3d/calib3d.hpp>
#include <rtabmap/core/OdometryF2M.h>
#include <rtabmap/core/odometry/OdometryF2M.h>
#include <pcl/common/io.h>
#if _MSC_VER

2
corelib/src/OdometryFovis.cpp → corelib/src/odometry/OdometryFovis.cpp
View File

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/OdometryFovis.h"
#include "rtabmap/core/odometry/OdometryFovis.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/utilite/ULogger.h"

2
corelib/src/OdometryLOAM.cpp → corelib/src/odometry/OdometryLOAM.cpp
View File

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/OdometryLOAM.h"
#include "rtabmap/core/odometry/OdometryLOAM.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/utilite/ULogger.h"

2
corelib/src/OdometryMSCKF.cpp → corelib/src/odometry/OdometryMSCKF.cpp
View File

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/OdometryMSCKF.h"
#include "rtabmap/core/odometry/OdometryMSCKF.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h"

2
corelib/src/OdometryMono.cpp → corelib/src/odometry/OdometryMono.cpp
View File

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/OdometryMono.h"
#include "rtabmap/core/odometry/OdometryMono.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"

2
corelib/src/OdometryORBSLAM2.cpp → corelib/src/odometry/OdometryORBSLAM2.cpp
View File

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/OdometryORBSLAM2.h"
#include "rtabmap/core/odometry/OdometryORBSLAM2.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d_transforms.h"

2
corelib/src/OdometryOkvis.cpp → corelib/src/odometry/OdometryOkvis.cpp
View File

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/OdometryOkvis.h"
#include "rtabmap/core/odometry/OdometryOkvis.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h"

2
corelib/src/OdometryViso2.cpp → corelib/src/odometry/OdometryViso2.cpp
View File

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/OdometryViso2.h"
#include "rtabmap/core/odometry/OdometryViso2.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/utilite/ULogger.h"

2
corelib/src/OptimizerCVSBA.cpp → corelib/src/optimizer/OptimizerCVSBA.cpp
View File

@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <set>
#include <rtabmap/core/OptimizerCVSBA.h>
#include <rtabmap/core/optimizer/OptimizerCVSBA.h>
#ifdef RTABMAP_CVSBA
#include <cvsba/cvsba.h>

2
corelib/src/OptimizerG2O.cpp → corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <set>
#include <rtabmap/core/Version.h>
#include <rtabmap/core/OptimizerG2O.h>
#include <rtabmap/core/optimizer/OptimizerG2O.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_motion_estimation.h>
#include <rtabmap/core/util3d.h>

2
corelib/src/OptimizerGTSAM.cpp → corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <set>
#include <rtabmap/core/OptimizerGTSAM.h>
#include <rtabmap/core/optimizer/OptimizerGTSAM.h>
#ifdef RTABMAP_GTSAM
#include <gtsam/geometry/Pose2.h>

2
corelib/src/OptimizerTORO.cpp → corelib/src/optimizer/OptimizerTORO.cpp
View File

@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <set>
#include <rtabmap/core/OptimizerTORO.h>
#include <rtabmap/core/optimizer/OptimizerTORO.h>
#ifdef RTABMAP_TORO
#include "toro3d/treeoptimizer3.hh"

0
corelib/src/toro3d/dmatrix.hh → corelib/src/optimizer/toro3d/dmatrix.hh
View File

0
corelib/src/toro3d/dmatrix.hxx → corelib/src/optimizer/toro3d/dmatrix.hxx
View File

0
corelib/src/toro3d/posegraph.hh → corelib/src/optimizer/toro3d/posegraph.hh
View File

0
corelib/src/toro3d/posegraph.hxx → corelib/src/optimizer/toro3d/posegraph.hxx
View File

0
corelib/src/toro3d/posegraph2.cpp → corelib/src/optimizer/toro3d/posegraph2.cpp
View File

0
corelib/src/toro3d/posegraph2.hh → corelib/src/optimizer/toro3d/posegraph2.hh
View File

0
corelib/src/toro3d/posegraph3.cpp → corelib/src/optimizer/toro3d/posegraph3.cpp
View File

0
corelib/src/toro3d/posegraph3.hh → corelib/src/optimizer/toro3d/posegraph3.hh
View File

0
corelib/src/toro3d/readme.txt → corelib/src/optimizer/toro3d/readme.txt
View File

0
corelib/src/toro3d/transformation2.hh → corelib/src/optimizer/toro3d/transformation2.hh
View File

0
corelib/src/toro3d/transformation3.hh → corelib/src/optimizer/toro3d/transformation3.hh
View File

0
corelib/src/toro3d/transformation3.hxx → corelib/src/optimizer/toro3d/transformation3.hxx
View File

0
corelib/src/toro3d/treeoptimizer2.cpp → corelib/src/optimizer/toro3d/treeoptimizer2.cpp
View File

0
corelib/src/toro3d/treeoptimizer2.hh → corelib/src/optimizer/toro3d/treeoptimizer2.hh
View File

0
corelib/src/toro3d/treeoptimizer3.cpp → corelib/src/optimizer/toro3d/treeoptimizer3.cpp
View File

0
corelib/src/toro3d/treeoptimizer3.hh → corelib/src/optimizer/toro3d/treeoptimizer3.hh
View File

0
corelib/src/toro3d/treeoptimizer3_iteration.cpp → corelib/src/optimizer/toro3d/treeoptimizer3_iteration.cpp
View File

0
corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp → corelib/src/optimizer/vertigo/g2o/edge_se2MaxMixture.cpp
View File

0
corelib/src/vertigo/g2o/edge_se2MaxMixture.h → corelib/src/optimizer/vertigo/g2o/edge_se2MaxMixture.h
View File

4
corelib/src/vertigo/g2o/edge_se2Switchable.cpp → corelib/src/optimizer/vertigo/g2o/edge_se2Switchable.cpp
View File

@@ -8,8 +8,8 @@
* Author: Christian Kerl <christian.kerl@in.tum.de>
*/
#include "vertigo/g2o/edge_se2Switchable.h"
#include "vertigo/g2o/vertex_switchLinear.h"
#include "edge_se2Switchable.h"
#include "vertex_switchLinear.h"
using namespace std;
using namespace Eigen;

0
corelib/src/vertigo/g2o/edge_se2Switchable.h → corelib/src/optimizer/vertigo/g2o/edge_se2Switchable.h
View File

4
corelib/src/vertigo/g2o/edge_se3Switchable.cpp → corelib/src/optimizer/vertigo/g2o/edge_se3Switchable.cpp
View File

@@ -8,8 +8,8 @@
* Author: Christian Kerl <christian.kerl@in.tum.de>
*/
#include "vertigo/g2o/edge_se3Switchable.h"
#include "vertigo/g2o/vertex_switchLinear.h"
#include "edge_se3Switchable.h"
#include "vertex_switchLinear.h"
#include "g2o/types/slam3d/vertex_se3.h"
#include "g2o/types/slam3d/isometry3d_gradients.h"

0
corelib/src/vertigo/g2o/edge_se3Switchable.h → corelib/src/optimizer/vertigo/g2o/edge_se3Switchable.h
View File

2
corelib/src/vertigo/g2o/edge_switchPrior.cpp → corelib/src/optimizer/vertigo/g2o/edge_switchPrior.cpp
View File

@@ -1,4 +1,4 @@
#include "vertigo/g2o/edge_switchPrior.h"
#include "edge_switchPrior.h"
using namespace std;
EdgeSwitchPrior::EdgeSwitchPrior()

0
corelib/src/vertigo/g2o/edge_switchPrior.h → corelib/src/optimizer/vertigo/g2o/edge_switchPrior.h
View File

10
corelib/src/vertigo/g2o/types_g2o_robust.cpp → corelib/src/optimizer/vertigo/g2o/types_g2o_robust.cpp
View File

@@ -1,11 +1,11 @@
#include "g2o/core/factory.h"
#include "g2o/stuff/macros.h"
#include "vertigo/g2o/edge_switchPrior.h"
#include "vertigo/g2o/edge_se2Switchable.h"
//#include "vertigo/g2o/edge_se2MaxMixture.h"
#include "vertigo/g2o/edge_se3Switchable.h"
#include "vertigo/g2o/vertex_switchLinear.h"
#include "edge_switchPrior.h"
#include "edge_se2Switchable.h"
//#include "edge_se2MaxMixture.h"
#include "edge_se3Switchable.h"
#include "vertex_switchLinear.h"
G2O_REGISTER_TYPE(EDGE_SWITCH_PRIOR, EdgeSwitchPrior);

2
corelib/src/vertigo/g2o/vertex_switchLinear.cpp → corelib/src/optimizer/vertigo/g2o/vertex_switchLinear.cpp
View File

@@ -8,7 +8,7 @@
* Author: Christian Kerl <christian.kerl@in.tum.de>
*/
#include "vertigo/g2o/vertex_switchLinear.h"
#include "vertex_switchLinear.h"
#include <iostream>
using namespace std;

0
corelib/src/vertigo/g2o/vertex_switchLinear.h → corelib/src/optimizer/vertigo/g2o/vertex_switchLinear.h
View File

Some files were not shown because too many files have changed in this diff Show More