i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Refactored Camera classes and Preferences->Source menu

Browse Source
This commit is contained in:
matlabbe
2015-06-26 18:21:32 -04:00
parent 6f1df94b18
commit 6df403ed42
39 changed files with 3543 additions and 3388 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Camera.h
+17 -95
View File
@@ -50,118 +50,40 @@ class RTABMAP_EXP Camera
{
public:
virtual ~Camera();
cv::Mat takeImage();
virtual bool init() = 0;
SensorData takeImage();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
virtual bool isCalibrated() const = 0;
virtual std::string getSerial() const = 0;
int getNextSeqID() {return ++_seq;}
//getters
void getImageSize(unsigned int & width, unsigned int & height);
float getImageRate() const {return _imageRate;}
bool isMirroringEnabled() const {return _mirroring;}
const Transform & getLocalTransform() const {return _localTransform;}
//setters
void setImageRate(float imageRate) {_imageRate = imageRate;}
void setImageSize(unsigned int width, unsigned int height);
void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
void setCalibration(const std::string & fileName);
void setCalibration(const cv::Mat & cameraMatrix, const cv::Mat & distorsionCoefficients);
void resetCalibration();
void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
protected:
/**
* Constructor
*
* @param imageRate : image/second , 0 for fast as the camera can
* @param imageRate : image/second , 0 for fast as the camera can
*/
Camera(float imageRate = 0,
unsigned int imageWidth = 0,
unsigned int imageHeight = 0);
Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
virtual cv::Mat captureImage() = 0;
/**
* returned rgb and depth images should be already rectified if calibration was loaded
*/
virtual SensorData captureImage() = 0;
private:
float _imageRate;
unsigned int _imageWidth;
unsigned int _imageHeight;
bool _mirroring;
Transform _localTransform;
cv::Size _targetImageSize;
UTimer * _frameRateTimer;
cv::Mat _k; // camera_matrix
cv::Mat _d; // distorsion_coefficients
};
/////////////////////////
// CameraImages
/////////////////////////
class RTABMAP_EXP CameraImages :
public Camera
{
public:
CameraImages(const std::string & path,
int startAt = 1,
bool refreshDir = false,
float imageRate = 0,
unsigned int imageWidth = 0,
unsigned int imageHeight = 0);
virtual ~CameraImages();
virtual bool init();
std::string getPath() const {return _path;}
unsigned int imagesCount() const;
protected:
virtual cv::Mat captureImage();
private:
std::string _path;
int _startAt;
// If the list of files in the directory is refreshed
// on each call of takeImage()
bool _refreshDir;
int _count;
UDirectory * _dir;
std::string _lastFileName;
};
/////////////////////////
// CameraVideo
/////////////////////////
class RTABMAP_EXP CameraVideo :
public Camera
{
public:
enum Source{kVideoFile, kUsbDevice};
public:
CameraVideo(int usbDevice = 0,
float imageRate = 0,
unsigned int imageWidth = 0,
unsigned int imageHeight = 0);
CameraVideo(const std::string & filePath,
float imageRate = 0,
unsigned int imageWidth = 0,
unsigned int imageHeight = 0);
virtual ~CameraVideo();
virtual bool init();
int getUsbDevice() const {return _usbDevice;}
const std::string & getFilePath() const {return _filePath;}
protected:
virtual cv::Mat captureImage();
private:
// File type
std::string _filePath;
cv::VideoCapture _capture;
Source _src;
// Usb camera
int _usbDevice;
int _seq;
};
corelib/include/rtabmap/core/CameraEvent.h
+3 -4
View File
@@ -38,14 +38,13 @@ class CameraEvent :
{
public:
enum Code {
kCodeImage,
kCodeImageDepth,
kCodeData,
kCodeNoMoreImages
};
public:
CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = "") :
UEvent(kCodeImage),
UEvent(kCodeData),
data_(image, seq, stamp),
cameraName_(cameraName)
{
@@ -57,7 +56,7 @@ public:
}
CameraEvent(const SensorData & data, const std::string & cameraName = "") :
UEvent(kCodeImageDepth),
UEvent(kCodeData),
data_(data),
cameraName_(cameraName)
{
corelib/include/rtabmap/core/CameraRGB.h
+124
View File
@@ -0,0 +1,124 @@
/*
Copyright (c) 2010-2014, 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"
#include <set>
#include <stack>
#include <list>
#include <vector>
class UDirectory;
class UTimer;
namespace rtabmap
{
/////////////////////////
// CameraImages
/////////////////////////
class RTABMAP_EXP CameraImages :
public Camera
{
public:
CameraImages(const std::string & path,
int startAt = 1,
bool refreshDir = false,
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;
std::string getPath() const {return _path;}
unsigned int imagesCount() const;
protected:
virtual SensorData captureImage();
private:
std::string _path;
int _startAt;
// If the list of files in the directory is refreshed
// on each call of takeImage()
bool _refreshDir;
int _count;
UDirectory * _dir;
std::string _lastFileName;
};
/////////////////////////
// CameraVideo
/////////////////////////
class RTABMAP_EXP CameraVideo :
public Camera
{
public:
enum Source{kVideoFile, kUsbDevice};
public:
CameraVideo(int usbDevice = 0,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
CameraVideo(const std::string & filePath,
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();
private:
// File type
std::string _filePath;
cv::VideoCapture _capture;
Source _src;
// Usb camera
int _usbDevice;
std::string _guid;
CameraModel _model;
};
} // namespace rtabmap
corelib/include/rtabmap/core/CameraRGBD.h
+16 -164
View File
@@ -29,24 +29,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/SensorData.h"
#include "rtabmap/utilite/UMutex.h"
#include "rtabmap/utilite/USemaphore.h"
#include "rtabmap/core/CameraModel.h"
#include <set>
#include <stack>
#include <list>
#include <vector>
#include "rtabmap/core/Camera.h"
#include <pcl/io/openni_camera/openni_depth_image.h>
#include <pcl/io/openni_camera/openni_image.h>
#include <boost/signals2/connection.hpp>
class UDirectory;
class UTimer;
namespace openni
{
class Device;
@@ -67,70 +59,17 @@ class Registration;
class PacketPipeline;
}
namespace FlyCapture2
{
class Camera;
}
typedef struct _freenect_context freenect_context;
typedef struct _freenect_device freenect_device;
namespace rtabmap
{
/**
* Class CameraRGBD
*
*/
class RTABMAP_EXP CameraRGBD
{
public:
virtual ~CameraRGBD();
void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
virtual bool init(const std::string & calibrationFolder = ".") = 0;
virtual bool isCalibrated() const = 0;
virtual std::string getSerial() const = 0;
//getters
float getImageRate() const {return _imageRate;}
const Transform & getLocalTransform() const {return _localTransform;}
bool isMirroringEnabled() const {return _mirroring;}
bool isColorOnly() const {return _colorOnly;}
//setters
void setImageRate(float imageRate) {_imageRate = imageRate;}
void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
void setMirroringEnabled(bool mirroring) {_mirroring = mirroring;}
void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
protected:
/**
* Constructor
*
* @param imageRate : image/second , 0 for fast as the camera can
*/
CameraRGBD(float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
/**
* returned rgb and depth images should be already rectified
*/
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) = 0;
private:
float _imageRate;
Transform _localTransform;
bool _mirroring;
bool _colorOnly;
UTimer * _frameRateTimer;
};
/////////////////////////
// CameraOpenNIPCL
/////////////////////////
class RTABMAP_EXP CameraOpenni :
public CameraRGBD
public Camera
{
public:
static bool available() {return true;}
@@ -147,12 +86,12 @@ public:
const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
float constant);
virtual bool init(const std::string & calibrationFolder = ".");
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
virtual SensorData captureImage();
private:
pcl::Grabber* interface_;
@@ -169,7 +108,7 @@ private:
// CameraOpenNICV
/////////////////////////
class RTABMAP_EXP CameraOpenNICV :
public CameraRGBD
public Camera
{
public:
@@ -181,12 +120,12 @@ public:
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraOpenNICV();
virtual bool init(const std::string & calibrationFolder = ".");
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 void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
virtual SensorData captureImage();
private:
bool _asus;
@@ -198,7 +137,7 @@ private:
// CameraOpenNI2
/////////////////////////
class RTABMAP_EXP CameraOpenNI2 :
public CameraRGBD
public Camera
{
public:
@@ -211,7 +150,7 @@ public:
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraOpenNI2();
virtual bool init(const std::string & calibrationFolder = ".");
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
@@ -222,7 +161,7 @@ public:
bool setMirroring(bool enabled);
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
virtual SensorData captureImage();
private:
openni::Device * _device;
@@ -240,7 +179,7 @@ private:
class FreenectDevice;
class RTABMAP_EXP CameraFreenect :
public CameraRGBD
public Camera
{
public:
static bool available();
@@ -252,12 +191,12 @@ public:
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraFreenect();
virtual bool init(const std::string & calibrationFolder = ".");
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
virtual SensorData captureImage();
private:
int deviceId_;
@@ -270,7 +209,7 @@ private:
/////////////////////////
class RTABMAP_EXP CameraFreenect2 :
public CameraRGBD
public Camera
{
public:
static bool available();
@@ -290,12 +229,12 @@ public:
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraFreenect2();
virtual bool init(const std::string & calibrationFolder = ".");
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
virtual SensorData captureImage();
private:
int deviceId_;
@@ -308,91 +247,4 @@ private:
libfreenect2::Registration * reg_;
};
/////////////////////////
// CameraStereoDC1394
/////////////////////////
class DC1394Device;
class RTABMAP_EXP CameraStereoDC1394 :
public CameraRGBD
{
public:
static bool available();
public:
CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoDC1394();
virtual bool init(const std::string & calibrationFolder = ".");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp);
private:
DC1394Device *device_;
StereoCameraModel stereoModel_;
};
/////////////////////////
// CameraStereoFlyCapture2
/////////////////////////
class RTABMAP_EXP CameraStereoFlyCapture2 :
public CameraRGBD
{
public:
static bool available();
public:
CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoFlyCapture2();
virtual bool init(const std::string & calibrationFolder = ".");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp);
private:
FlyCapture2::Camera * camera_;
void * triclopsCtx_; // TriclopsContext
};
/////////////////////////
// CameraStereoImages
/////////////////////////
class CameraImages;
class RTABMAP_EXP CameraStereoImages :
public CameraRGBD
{
public:
static bool available();
public:
CameraStereoImages(
const std::string & path,
const std::string & cameraName = "stereo_images", // calibration file name
const std::string & timestampsPath = "", // "times.txt"
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoImages();
virtual bool init(const std::string & calibrationFolder = ".");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp);
private:
CameraImages * camera_;
CameraImages * camera2_;
std::string cameraName_;
std::string timestampsPath_;
std::list<double> stamps_;
StereoCameraModel stereoModel_;
};
} // namespace rtabmap
corelib/include/rtabmap/core/CameraStereo.h
+130
View File
@@ -0,0 +1,130 @@
/*
Copyright (c) 2010-2014, 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 <list>
namespace FlyCapture2
{
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();
private:
DC1394Device *device_;
StereoCameraModel stereoModel_;
};
/////////////////////////
// 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();
private:
FlyCapture2::Camera * camera_;
void * triclopsCtx_; // TriclopsContext
};
/////////////////////////
// CameraStereoImages
/////////////////////////
class CameraImages;
class RTABMAP_EXP CameraStereoImages :
public Camera
{
public:
static bool available();
public:
CameraStereoImages(
const std::string & path,
const std::string & timestampsPath = "", // "times.txt"
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;
protected:
virtual SensorData captureImage();
private:
CameraImages * camera_;
CameraImages * camera2_;
std::string timestampsPath_;
std::list<double> stamps_;
StereoCameraModel stereoModel_;
std::string cameraName_;
};
} // namespace rtabmap
corelib/include/rtabmap/core/CameraThread.h
+4 -6
View File
@@ -36,7 +36,6 @@ namespace rtabmap
{
class Camera;
class CameraRGBD;
/**
* Class CameraThread
@@ -49,10 +48,10 @@ class RTABMAP_EXP CameraThread :
public:
// ownership transferred
CameraThread(Camera * camera);
CameraThread(CameraRGBD * camera);
virtual ~CameraThread();
bool init(); // call camera->init()
void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
//getters
bool isPaused() const {return !this->isRunning();}
@@ -60,15 +59,14 @@ public:
void setImageRate(float imageRate);
Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
CameraRGBD * cameraRGBD() {return _cameraRGBD;} // return null if not set, valid until CameraThread is deleted
private:
virtual void mainLoop();
private:
Camera * _camera;
CameraRGBD * _cameraRGBD;
int _seq;
bool _mirroring;
bool _colorOnly;
};
} // namespace rtabmap
corelib/include/rtabmap/core/SensorData.h
+3
View File
@@ -157,6 +157,9 @@ public:
void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;}
void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;}
void setLaserScanRaw(const cv::Mat & laserScanRaw, int laserScanMaxPts) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = laserScanMaxPts;}
void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
//for convenience
cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();}
corelib/src/CMakeLists.txt
+2
View File
@@ -13,7 +13,9 @@ SET(SRC_FILES
Camera.cpp
CameraThread.cpp
CameraRGB.cpp
CameraRGBD.cpp
CameraStereo.cpp
CameraModel.cpp
EpipolarGeometry.cpp
corelib/src/Camera.cpp
+23 -376
View File
@@ -44,14 +44,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap
{
Camera::Camera(float imageRate,
unsigned int imageWidth,
unsigned int imageHeight) :
Camera::Camera(float imageRate, const Transform & localTransform) :
_imageRate(imageRate),
_imageWidth(imageWidth),
_imageHeight(imageHeight),
_mirroring(false),
_frameRateTimer(new UTimer())
_localTransform(localTransform),
_targetImageSize(0,0),
_frameRateTimer(new UTimer()),
_seq(0)
{
}
@@ -63,397 +61,46 @@ Camera::~Camera()
}
}
void Camera::setImageSize(unsigned int width, unsigned int height)
SensorData Camera::takeImage()
{
_imageWidth = width;
_imageHeight = height;
}
void Camera::getImageSize(unsigned int & width, unsigned int & height)
{
width = _imageWidth;
height = _imageHeight;
}
void Camera::setCalibration(const std::string & fileName)
{
if(UFile::getExtension(fileName).compare("yaml") == 0)
bool warnFrameRateTooHigh = false;
float actualFrameRate = 0;
if(_imageRate>0)
{
cv::FileStorage fs;
fs.open(fileName, cv::FileStorage::READ);
if (!fs.isOpened())
{
UERROR("Failed to open file \"%s\"", fileName.c_str());
return;
}
cv::Mat k,d;
cv::FileNode n = fs["camera_matrix"];
int rows = n["rows"];
int cols = n["cols"];
std::vector<double> data;
n["data"] >> data;
if(rows > 0 && cols > 0 && (int)data.size() == rows*cols)
{
k = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
}
cv::FileNode nd = fs["distortion_coefficients"];
rows = nd["rows"];
cols = nd["cols"];
data.clear();
nd["data"] >> data;
if(rows > 0 && cols > 0 && (int)data.size() == rows*cols)
{
d = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
}
if(k.empty())
{
UERROR("Failed to load \"camera_matrix\" matrix.");
}
if(d.empty())
{
UERROR("Failed to load \"distortion_coefficients\" matrix.");
}
if(!k.empty() && !d.empty())
{
this->setCalibration(k, d);
}
}
else
{
UERROR("Calibration file must be in \"*.yaml\" format");
}
}
void Camera::setCalibration(const cv::Mat & cameraMatrix, const cv::Mat & distorsionCoefficients)
{
UASSERT(cameraMatrix.type() == CV_64FC1 &&
cameraMatrix.rows == 3 &&
cameraMatrix.cols == 3);
UASSERT(distorsionCoefficients.type() == CV_64FC1 &&
distorsionCoefficients.rows ==1 &&
(distorsionCoefficients.cols == 4 || distorsionCoefficients.cols == 5 || distorsionCoefficients.cols == 8));
_k = cameraMatrix;
_d = distorsionCoefficients;
}
void Camera::resetCalibration()
{
_k = cv::Mat();
_d = cv::Mat();
}
cv::Mat Camera::takeImage()
{
cv::Mat img;
float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
if(imageRate>0)
{
int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
int sleepTime = (1000.0f/_imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
else if(sleepTime < 0)
{
warnFrameRateTooHigh = true;
actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime());
}
// Add precision at the cost of a small overhead
while(_frameRateTimer->getElapsedTime() < 1.0/double(imageRate)-0.000001)
while(_frameRateTimer->getElapsedTime() < 1.0/double(_imageRate)-0.000001)
{
//
}
double slept = _frameRateTimer->getElapsedTime();
_frameRateTimer->start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate));
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_imageRate));
}
UTimer timer;
img = this->captureImage();
if(!img.empty() && !_k.empty() && !_d.empty())
SensorData data = this->captureImage();
if(warnFrameRateTooHigh)
{
cv::Mat temp = img.clone();
cv::undistort(temp, img, _k, _d);
}
if(!img.empty() && _mirroring)
{
cv::flip(img,img,1);
}
UDEBUG("Time capturing image = %fs", timer.ticks());
return img;
}
/////////////////////////
// CameraImages
/////////////////////////
CameraImages::CameraImages(const std::string & path,
int startAt,
bool refreshDir,
float imageRate,
unsigned int imageWidth,
unsigned int imageHeight) :
Camera(imageRate, imageWidth, imageHeight),
_path(path),
_startAt(startAt),
_refreshDir(refreshDir),
_count(0),
_dir(0)
{
}
CameraImages::~CameraImages(void)
{
if(_dir)
{
delete _dir;
}
}
bool CameraImages::init()
{
UDEBUG("");
if(_dir)
{
_dir->setPath(_path, "jpg ppm png bmp pnm tiff");
UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
_imageRate, actualFrameRate, timer.ticks());
}
else
{
_dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff");
UDEBUG("Time capturing image = %fs", timer.ticks());
}
_count = 0;
if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
{
_path.append("/");
}
if(!_dir->isValid())
{
ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
}
else if(_dir->getFileNames().size() == 0)
{
UWARN("Directory is empty \"%s\"", _path.c_str());
}
else
{
UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
}
return _dir->isValid();
}
unsigned int CameraImages::imagesCount() const
{
if(_dir)
{
return _dir->getFileNames().size();
}
return 0;
}
cv::Mat CameraImages::captureImage()
{
cv::Mat img;
UDEBUG("");
if(_dir->isValid())
{
if(_refreshDir)
{
_dir->update();
}
if(_startAt == 0)
{
const std::list<std::string> & fileNames = _dir->getFileNames();
if(fileNames.size())
{
if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
{
_lastFileName = *fileNames.rbegin();
std::string fullPath = _path + _lastFileName;
img = cv::imread(fullPath.c_str());
}
}
}
else
{
std::string fileName;
std::string fullPath;
fileName = _dir->getNextFileName();
if(fileName.size())
{
fullPath = _path + fileName;
while(++_count < _startAt && (fileName = _dir->getNextFileName()).size())
{
fullPath = _path + fileName;
}
if(fileName.size())
{
ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
#else
img = cv::imread(fullPath.c_str(), -1);
#endif
UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
img.cols, img.rows, img.channels(), img.elemSize(), img.total());
#if CV_MAJOR_VERSION < 3
// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
if(img.depth() != CV_8U)
{
// The depth should be 8U
UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
IplImage * i = cvLoadImage(fullPath.c_str());
img = cv::Mat(i, true);
cvReleaseImage(&i);
}
#endif
if(img.channels()>3)
{
UWARN("Conversion from 4 channels to 3 channels (file=%s)", fullPath.c_str());
cv::Mat out;
cv::cvtColor(img, out, CV_BGRA2BGR);
img = out;
}
}
}
}
}
else
{
UWARN("Directory is not set, camera must be initialized.");
}
unsigned int w;
unsigned int h;
this->getImageSize(w, h);
if(!img.empty() &&
w &&
h &&
w != (unsigned int)img.cols &&
h != (unsigned int)img.rows)
{
cv::Mat resampled;
cv::resize(img, resampled, cv::Size(w, h));
img = resampled;
}
return img;
}
/////////////////////////
// CameraVideo
/////////////////////////
CameraVideo::CameraVideo(int usbDevice,
float imageRate,
unsigned int imageWidth,
unsigned int imageHeight) :
Camera(imageRate, imageWidth, imageHeight),
_src(kUsbDevice),
_usbDevice(usbDevice)
{
}
CameraVideo::CameraVideo(const std::string & filePath,
float imageRate,
unsigned int imageWidth,
unsigned int imageHeight) :
Camera(imageRate, imageWidth, imageHeight),
_filePath(filePath),
_src(kVideoFile),
_usbDevice(0)
{
}
CameraVideo::~CameraVideo()
{
_capture.release();
}
bool CameraVideo::init()
{
if(_capture.isOpened())
{
_capture.release();
}
if(_src == kUsbDevice)
{
unsigned int w;
unsigned int h;
this->getImageSize(w, h);
ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d with imgSize=[%d,%d]", _usbDevice, w, h);
_capture.open(_usbDevice);
if(w && h)
{
_capture.set(CV_CAP_PROP_FRAME_WIDTH, double(w));
_capture.set(CV_CAP_PROP_FRAME_HEIGHT, double(h));
}
}
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;
}
return true;
}
cv::Mat CameraVideo::captureImage()
{
cv::Mat img;
if(_capture.isOpened())
{
if(_capture.read(img))
{
unsigned int w;
unsigned int h;
this->getImageSize(w, h);
if(!img.empty() &&
w &&
h &&
w != (unsigned int)img.cols &&
h != (unsigned int)img.rows)
{
cv::Mat resampled;
cv::resize(img, resampled, cv::Size(w, h));
img = resampled;
}
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 img;
return data;
}
} // namespace rtabmap
corelib/src/CameraRGB.cpp
+328
View File
@@ -0,0 +1,328 @@
/*
Copyright (c) 2010-2014, 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/CameraRGB.h"
#include "rtabmap/core/DBDriver.h"
#include <rtabmap/utilite/UEventsManager.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 <iostream>
#include <cmath>
namespace rtabmap
{
/////////////////////////
// CameraImages
/////////////////////////
CameraImages::CameraImages(const std::string & path,
int startAt,
bool refreshDir,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
_path(path),
_startAt(startAt),
_refreshDir(refreshDir),
_count(0),
_dir(0)
{
}
CameraImages::~CameraImages(void)
{
if(_dir)
{
delete _dir;
}
}
bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName)
{
UDEBUG("");
if(_dir)
{
_dir->setPath(_path, "jpg ppm png bmp pnm tiff");
}
else
{
_dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff");
}
_count = 0;
if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
{
_path.append("/");
}
if(!_dir->isValid())
{
ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
}
else if(_dir->getFileNames().size() == 0)
{
UWARN("Directory is empty \"%s\"", _path.c_str());
}
else
{
UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
}
return _dir->isValid();
}
bool CameraImages::isCalibrated() const
{
return false;
}
std::string CameraImages::getSerial() const
{
return "";
}
unsigned int CameraImages::imagesCount() const
{
if(_dir)
{
return _dir->getFileNames().size();
}
return 0;
}
SensorData CameraImages::captureImage()
{
cv::Mat img;
UDEBUG("");
if(_dir->isValid())
{
if(_refreshDir)
{
_dir->update();
}
if(_startAt == 0)
{
const std::list<std::string> & fileNames = _dir->getFileNames();
if(fileNames.size())
{
if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
{
_lastFileName = *fileNames.rbegin();
std::string fullPath = _path + _lastFileName;
img = cv::imread(fullPath.c_str());
}
}
}
else
{
std::string fileName;
std::string fullPath;
fileName = _dir->getNextFileName();
if(fileName.size())
{
fullPath = _path + fileName;
while(++_count < _startAt && (fileName = _dir->getNextFileName()).size())
{
fullPath = _path + fileName;
}
if(fileName.size())
{
ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
#else
img = cv::imread(fullPath.c_str(), -1);
#endif
UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
img.cols, img.rows, img.channels(), img.elemSize(), img.total());
#if CV_MAJOR_VERSION < 3
// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
if(img.depth() != CV_8U)
{
// The depth should be 8U
UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
IplImage * i = cvLoadImage(fullPath.c_str());
img = cv::Mat(i, true);
cvReleaseImage(&i);
}
#endif
if(img.channels()>3)
{
UWARN("Conversion from 4 channels to 3 channels (file=%s)", fullPath.c_str());
cv::Mat out;
cv::cvtColor(img, out, CV_BGRA2BGR);
img = out;
}
}
}
}
}
else
{
UWARN("Directory is not set, camera must be initialized.");
}
return SensorData(img);
}
/////////////////////////
// CameraVideo
/////////////////////////
CameraVideo::CameraVideo(int usbDevice,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
_src(kUsbDevice),
_usbDevice(usbDevice)
{
}
CameraVideo::CameraVideo(const std::string & filePath,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
_filePath(filePath),
_src(kVideoFile),
_usbDevice(0)
{
}
CameraVideo::~CameraVideo()
{
_capture.release();
}
bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
{
_guid.clear();
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
{
uint32_t 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() || !cameraName.empty()))
{
if(!_model.load(calibrationFolder + "/" + (cameraName.empty()?_guid:cameraName) + ".yaml"))
{
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
cameraName.empty()?_guid.c_str():cameraName.c_str(), calibrationFolder.c_str());
}
else
{
UINFO("Camera parameters: fx=%f cx=%f cy=%f cy=%f",
_model.fx(),
_model.cx(),
_model.cy(),
_model.cy());
}
}
}
return true;
}
bool CameraVideo::isCalibrated() const
{
return _model.isValid();
}
std::string CameraVideo::getSerial() const
{
return _guid;
}
SensorData CameraVideo::captureImage()
{
cv::Mat img;
if(_capture.isOpened())
{
if(_capture.read(img))
{
if(_model.isValid())
{
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);
}
} // namespace rtabmap
corelib/src/CameraRGBD.cpp
+96 -1010
View File
File diff suppressed because it is too large Load Diff
corelib/src/CameraStereo.cpp
+910
View File
@@ -0,0 +1,910 @@
/*
Copyright (c) 2010-2014, 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/CameraStereo.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/CameraRGB.h"
#include <rtabmap/utilite/UEventsManager.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 <rtabmap/utilite/UMath.h>
#ifdef WITH_DC1394
#include <dc1394/dc1394.h>
#endif
#ifdef WITH_FLYCAPTURE2
#include <triclops.h>
#include <fc2triclops.h>
#endif
namespace rtabmap
{
//
// CameraStereoDC1394
// Inspired from ROS camera1394stereo package
//
#ifdef WITH_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 WITH_DC1394
return true;
#else
return false;
#endif
}
CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localTransform) :
Camera(imageRate, localTransform),
device_(0)
{
#ifdef WITH_DC1394
device_ = new DC1394Device();
#endif
}
CameraStereoDC1394::~CameraStereoDC1394()
{
#ifdef WITH_DC1394
if(device_)
{
delete device_;
}
#endif
}
bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef WITH_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))
{
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
{
return stereoModel_.isValid();
}
std::string CameraStereoDC1394::getSerial() const
{
#ifdef WITH_DC1394
if(device_)
{
return device_->guid();
}
#endif
return "";
}
SensorData CameraStereoDC1394::captureImage()
{
SensorData data;
#ifdef WITH_DC1394
if(device_)
{
cv::Mat left, right;
device_->getImages(left, right);
if(!left.empty() && !right.empty())
{
// Rectification
left = stereoModel_.left().rectifyImage(left);
right = stereoModel_.right().rectifyImage(right);
StereoCameraModel model(
stereoModel_.left().fx(), //fx
stereoModel_.left().fy(), //fy
stereoModel_.left().cx(), //cx
stereoModel_.left().cy(), //cy
stereoModel_.baseline(),
this->getLocalTransform());
data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
}
}
#else
UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
#endif
return data;
}
//
// CameraTriclops
//
CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transform & localTransform) :
Camera(imageRate, localTransform),
camera_(0),
triclopsCtx_(0)
{
#ifdef WITH_FLYCAPTURE2
camera_ = new FlyCapture2::Camera();
#endif
}
CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
{
#ifdef WITH_FLYCAPTURE2
// Close the camera
camera_->StopCapture();
camera_->Disconnect();
// Destroy the Triclops context
triclopsDestroyContext( triclopsCtx_ ) ;
delete camera_;
#endif
}
bool CameraStereoFlyCapture2::available()
{
#ifdef WITH_FLYCAPTURE2
return true;
#else
return false;
#endif
}
bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef WITH_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 WITH_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 WITH_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 WITH_FLYCAPTURE2
struct ImageContainer
{
FlyCapture2::Image tmp[2];
FlyCapture2::Image unprocessed[2];
} ;
#endif
SensorData CameraStereoFlyCapture2::captureImage()
{
SensorData data;
#ifdef WITH_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)
{
stamp = UTimer::now();
// 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());
data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
}
}
}
}
}
#else
UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
#endif
return data;
}
//
// CameraStereoImages
//
bool CameraStereoImages::available()
{
return true;
}
CameraStereoImages::CameraStereoImages(
const std::string & path,
const std::string & timestampsPath,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
camera_(0),
camera2_(0),
timestampsPath_(timestampsPath)
{
std::vector<std::string> paths = uListToVector(uSplit(path, uStrContains(path, ":")?':':';'));
if(paths.size() >= 1)
{
camera_ = new CameraImages(paths[0]);
if(paths.size() >= 2)
{
camera2_ = new CameraImages(paths[1]);
}
}
else
{
UERROR("The path is empty!");
}
}
CameraStereoImages::~CameraStereoImages()
{
if(camera_)
{
delete camera_;
}
if(camera2_)
{
delete camera2_;
}
}
bool CameraStereoImages::init(const std::string & calibrationFolder, const std::string & cameraName)
{
// look for calibration files
cameraName_.clear();
if(!calibrationFolder.empty() && !cameraName.empty())
{
cameraName_ = cameraName;
if(!stereoModel_.load(calibrationFolder, cameraName))
{
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());
}
}
bool success = false;
if(camera_ == 0)
{
UERROR("Cannot initialize the camera.");
}
else if(camera_->init())
{
if(camera2_)
{
if(camera2_->init())
{
if(camera_->imagesCount() == camera2_->imagesCount())
{
success = true;
}
else
{
UERROR("Cameras don't have the same number of images (%d vs %d)",
camera_->imagesCount(), camera2_->imagesCount());
}
}
else
{
UERROR("Cannot initialize the second camera.");
}
}
else
{
success = true;
}
}
stamps_.clear();
if(success && timestampsPath_.size())
{
FILE * file = 0;
#ifdef _MSC_VER
fopen_s(&file, timestampsPath_.c_str(), "r");
#else
file = fopen(timestampsPath_.c_str(), "r");
#endif
if(file)
{
char line[16];
while ( fgets (line , 16 , file) != NULL )
{
stamps_.push_back(uStr2Double(uReplaceChar(line, '\n', 0)));
}
fclose(file);
}
if(stamps_.size() != camera_->imagesCount())
{
UERROR("The stamps count is not the same as the images (%d vs %d)! Please remove "
"the timestamps file path if you don't want to use them (current file path=%s).",
(int)stamps_.size(), camera_->imagesCount(), timestampsPath_.c_str());
stamps_.clear();
success = false;
}
}
return success;
}
bool CameraStereoImages::isCalibrated() const
{
return stereoModel_.isValid();
}
std::string CameraStereoImages::getSerial() const
{
return cameraName_;
}
SensorData CameraStereoImages::captureImage()
{
SensorData data;
if(camera_)
{
double stamp;
if(stamps_.size())
{
stamp = stamps_.front();
stamps_.pop_front();
}
else
{
stamp = UTimer::now();
}
SensorData left, right;
left = camera_->takeImage();
if(!left.imageRaw().empty())
{
if(camera2_)
{
right = camera2_->takeImage();
}
else
{
right = camera_->takeImage();
}
if(!right.imageRaw().empty())
{
// Rectification
//left = stereoModel_.left().rectifyImage(left);
//right = stereoModel_.right().rectifyImage(right);
StereoCameraModel model(
stereoModel_.left().fx(), //fx
stereoModel_.left().fy(), //fy
stereoModel_.left().cx(), //cx
stereoModel_.left().cy(), //cy
stereoModel_.baseline(),
this->getLocalTransform());
data = SensorData(left.imageRaw(), right.imageRaw(), model, this->getNextSeqID(), stamp);
}
}
}
return data;
}
} // namespace rtabmap
corelib/src/CameraThread.cpp
+31 -83
View File
@@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/CameraThread.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/CameraEvent.h"
#include <rtabmap/utilite/UTimer.h>
@@ -39,21 +38,12 @@ namespace rtabmap
// ownership transferred
CameraThread::CameraThread(Camera * camera) :
_camera(camera),
_cameraRGBD(0),
_seq(0)
_mirroring(false),
_colorOnly(false)
{
UASSERT(_camera != 0);
}
// ownership transferred
CameraThread::CameraThread(CameraRGBD * camera) :
_camera(0),
_cameraRGBD(camera),
_seq(0)
{
UASSERT(_cameraRGBD != 0);
}
CameraThread::~CameraThread()
{
join(true);
@@ -61,10 +51,6 @@ CameraThread::~CameraThread()
{
delete _camera;
}
if(_cameraRGBD)
{
delete _cameraRGBD;
}
}
void CameraThread::setImageRate(float imageRate)
@@ -73,86 +59,48 @@ void CameraThread::setImageRate(float imageRate)
{
_camera->setImageRate(imageRate);
}
if(_cameraRGBD)
{
_cameraRGBD->setImageRate(imageRate);
}
}
bool CameraThread::init()
{
if(!this->isRunning())
{
_seq = 0;
if(_cameraRGBD)
{
return _cameraRGBD->init();
}
else
{
return _camera->init();
}
// Added sleep time to ignore first frames (which are darker)
uSleep(1000);
}
else
{
UERROR("Cannot initialize the camera because it is already running...");
}
return false;
}
void CameraThread::mainLoop()
{
UTimer timer;
UDEBUG("");
cv::Mat rgb, depth;
float fx = 0.0f;
float fyOrBaseline = 0.0f;
float cx = 0.0f;
float cy = 0.0f;
double stamp = UTimer::now();
if(_cameraRGBD)
{
_cameraRGBD->takeImage(rgb, depth, fx, fyOrBaseline, cx, cy, stamp);
}
else
{
rgb = _camera->takeImage();
}
SensorData data = _camera->takeImage();
if(!rgb.empty())
if(!data.imageRaw().empty())
{
if(_cameraRGBD)
{
SensorData data;
if(dynamic_cast<CameraStereoFlyCapture2*>(_cameraRGBD) ||
dynamic_cast<CameraStereoDC1394*>(_cameraRGBD) ||
dynamic_cast<CameraStereoImages*>(_cameraRGBD))
{
//stereo
data = SensorData(rgb, depth, StereoCameraModel(fx, fx, cx, cy, fyOrBaseline, _cameraRGBD->getLocalTransform()), ++_seq, stamp);
UASSERT(data.stereoCameraModel().isValid());
}
else
{
data = SensorData(rgb, depth, CameraModel(fx, fyOrBaseline, cx, cy, _cameraRGBD->getLocalTransform()), ++_seq, stamp);
UASSERT(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid());
}
this->post(new CameraEvent(data, _cameraRGBD->getSerial()));
}
else
if(_colorOnly && !data.depthRaw().empty())
{
this->post(new CameraEvent(rgb, ++_seq, stamp));
data.setDepthOrRightRaw(cv::Mat());
}
if(_mirroring && data.cameraModels().size() == 1)
{
cv::Mat tmpRgb;
cv::flip(data.imageRaw(), tmpRgb, 1);
data.setImageRaw(tmpRgb);
if(data.cameraModels()[0].cx())
{
CameraModel tmpModel(
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy(),
float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].localTransform());
data.setCameraModel(tmpModel);
}
if(!data.depthRaw().empty())
{
cv::Mat tmpDepth;
cv::flip(data.depthRaw(), tmpDepth, 1);
data.setDepthOrRightRaw(tmpDepth);
}
}
this->post(new CameraEvent(data, _camera->getSerial()));
}
else if(!this->isKilled())
{
if(_cameraRGBD)
{
UWARN("no more images...");
}
UWARN("no more images...");
this->kill();
this->post(new CameraEvent());
}
corelib/src/DBReader.cpp
+4 -2
View File
@@ -51,7 +51,8 @@ DBReader::DBReader(const std::string & databasePath,
_odometryIgnored(odometryIgnored),
_ignoreGoalDelay(ignoreGoalDelay),
_dbDriver(0),
_currentId(_ids.end())
_currentId(_ids.end()),
_previousStamp(0)
{
}
@@ -64,7 +65,8 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
_odometryIgnored(odometryIgnored),
_ignoreGoalDelay(ignoreGoalDelay),
_dbDriver(0),
_currentId(_ids.end())
_currentId(_ids.end()),
_previousStamp(0)
{
}
corelib/src/OdometryThread.cpp
+1 -1
View File
@@ -60,7 +60,7 @@ void OdometryThread::handleEvent(UEvent * event)
if(event->getClassName().compare("CameraEvent") == 0)
{
CameraEvent * cameraEvent = (CameraEvent*)event;
if(cameraEvent->getCode() == CameraEvent::kCodeImageDepth)
if(cameraEvent->getCode() == CameraEvent::kCodeData)
{
this->addData(cameraEvent->data());
}
corelib/src/RtabmapThread.cpp
+1 -1
View File
@@ -302,7 +302,7 @@ void RtabmapThread::handleEvent(UEvent* event)
{
UDEBUG("CameraEvent");
CameraEvent * e = (CameraEvent*)event;
if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeImageDepth)
if(e->getCode() == CameraEvent::kCodeData)
{
this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
}
examples/BOWMapping/main.cpp
+5 -5
View File
@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraRGB.h"
#include <opencv2/core/core.hpp>
#include "rtabmap/utilite/UFile.h"
#include <stdio.h>
@@ -118,12 +118,12 @@ int main(int argc, char * argv[])
int countLoopDetected=0;
int i=0;
cv::Mat img = camera.takeImage();
rtabmap::SensorData data = camera.takeImage();
int nextIndex = rtabmap.getLastLocationId()+1;
while(!img.empty())
while(!data.imageRaw().empty())
{
// Process image : Main loop of RTAB-Map
rtabmap.process(img, nextIndex);
rtabmap.process(data.imageRaw(), nextIndex);
// Check if a loop closure is detected and print some info
if(rtabmap.getLoopClosureId())
@@ -157,7 +157,7 @@ int main(int argc, char * argv[])
++nextIndex;
//Get next image
img = camera.takeImage();
data = camera.takeImage();
}
printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);
examples/RGBDMapping/main.cpp
+5 -4
View File
@@ -71,7 +71,7 @@ int main(int argc, char * argv[])
// Create the OpenNI camera, it will send a CameraEvent at the rate specified.
// Set transform to camera so z is up, y is left and x going forward
CameraRGBD * camera = 0;
Camera * camera = 0;
Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
if(driver == 1)
{
@@ -114,13 +114,14 @@ int main(int argc, char * argv[])
camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
}
CameraThread cameraThread(camera);
if(!cameraThread.init())
if(!camera->init())
{
UERROR("Camera init failed!");
exit(1);
}
CameraThread cameraThread(camera);
// GUI stuff, there the handler will receive RtabmapEvent and construct the map
// We give it the camera so the GUI can pause/resume the camera
QApplication app(argc, argv);
examples/WifiMapping/main.cpp
+7 -7
View File
@@ -109,7 +109,7 @@ int main(int argc, char * argv[])
// Create the OpenNI camera, it will send a CameraEvent at the rate specified.
// Set transform to camera so z is up, y is left and x going forward
CameraRGBD * camera = 0;
Camera * camera = 0;
Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
if(driver == 1)
{
@@ -152,17 +152,17 @@ int main(int argc, char * argv[])
camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
}
if(mirroring)
{
camera->setMirroringEnabled(true);
}
CameraThread cameraThread(camera);
if(!cameraThread.init())
if(!camera->init())
{
UERROR("Camera init failed!");
//exit(1);
}
CameraThread cameraThread(camera);
if(mirroring)
{
cameraThread.setMirroringEnabled(true);
}
// GUI stuff, there the handler will receive RtabmapEvent and construct the map
// We give it the camera so the GUI can pause/resume the camera
guilib/include/rtabmap/gui/MainWindow.h
+1 -13
View File
@@ -86,13 +86,6 @@ public:
kMonitoringPaused
};
enum SrcType {
kSrcUndefined,
kSrcVideo,
kSrcImages,
kSrcStream
};
public:
/**
* @param prefDialog If NULL, a default dialog is created. This
@@ -141,10 +134,7 @@ private slots:
void deleteMemory();
void openWorkingDirectory();
void updateEditMenu();
void selectImages();
void selectVideo();
void selectStream();
void selectDatabase();
void selectOpenni();
void selectFreenect();
void selectOpenniCv();
@@ -161,6 +151,7 @@ private slots:
void downloadPoseGraph();
void clearTheCache();
void openPreferences();
void openPreferencesSource();
void setDefaultViews();
void selectScreenCaptureFormat(bool checked);
void takeScreenshot();
@@ -252,9 +243,6 @@ private:
rtabmap::DBReader * _dbReader;
rtabmap::OdometryThread * _odomThread;
SrcType _srcType;
QString _srcPath;
//Dialogs
PreferencesDialog * _preferencesDialog;
AboutDialog * _aboutDialog;
guilib/include/rtabmap/gui/OdometryViewer.h
+1
View File
@@ -58,6 +58,7 @@ protected:
virtual void handleEvent(UEvent * event);
private slots:
void reset();
void processData(const rtabmap::OdometryEvent & odom);
private:
guilib/include/rtabmap/gui/PreferencesDialog.h
+41 -30
View File
@@ -59,7 +59,7 @@ namespace rtabmap {
class Signature;
class LoopClosureViewer;
class CameraRGBD;
class Camera;
class CalibrationDialog;
class RTABMAPGUI_EXP PreferencesDialog : public QDialog
@@ -79,19 +79,27 @@ public:
Q_DECLARE_FLAGS(PANEL_FLAGS, PanelFlag);
enum Src {
kSrcUndef,
kSrcUsbDevice,
kSrcImages,
kSrcVideo,
kSrcOpenNI_PCL,
kSrcFreenect,
kSrcOpenNI_CV,
kSrcOpenNI_CV_ASUS,
kSrcOpenNI2,
kSrcFreenect2,
kSrcStereoDC1394,
kSrcStereoFlyCapture2,
kSrcStereoImages
kSrcUndef = -1,
kSrcRGBD = 0,
kSrcOpenNI_PCL = 0,
kSrcFreenect = 1,
kSrcOpenNI_CV = 2,
kSrcOpenNI_CV_ASUS = 3,
kSrcOpenNI2 = 4,
kSrcFreenect2 = 5,
kSrcStereo = 100,
kSrcDC1394 = 100,
kSrcFlyCapture2 = 101,
kSrcStereoImages = 102,
kSrcRGB = 200,
kSrcUsbDevice = 200,
kSrcImages = 201,
kSrcVideo = 202,
kSrcDatabase = 300
};
public:
@@ -100,6 +108,7 @@ public:
virtual QString getIniFilePath() const;
void init();
void setCurrentPanelToSource();
// save stuff
void saveSettings();
@@ -162,26 +171,23 @@ public:
// source panel
double getGeneralInputRate() const;
bool isSourceMirroring() const;
bool isSourceImageUsed() const;
bool isSourceDatabaseUsed() const;
bool isSourceRGBDUsed() const;
PreferencesDialog::Src getSourceImageType() const;
QString getSourceImageTypeStr() const;
int getSourceWidth() const;
int getSourceHeight() const;
QString getCalibrationName() const;
PreferencesDialog::Src getSourceType() const;
PreferencesDialog::Src getSourceDriver() const;
QString getSourceDriverStr() const;
QString getSourceDevice() const;
QString getSourceImagesPath() const; //Images group
QString getSourceImagesSuffix() const; //Images group
int getSourceImagesSuffixIndex() const; //Images group
int getSourceImagesStartPos() const; //Images group
bool getSourceImagesRefreshDir() const; //Images group
QString getSourceVideoPath() const; //Video group
int getSourceUsbDeviceId() const; //UsbDevice group
QString getSourceDatabasePath() const; //Database group
bool getSourceDatabaseOdometryIgnored() const; //Database group
bool getSourceDatabaseGoalDelayIgnored() const; //Database group
int getSourceDatabaseStartPos() const; //Database group
bool getSourceDatabaseStampsUsed() const;//Database group
Src getSourceRGBD() const; // Openni group
bool getSourceOpenni2AutoWhiteBalance() const; //Openni group
bool getSourceOpenni2AutoExposure() const; //Openni group
int getSourceOpenni2Exposure() const; //Openni group
@@ -189,9 +195,8 @@ public:
bool getSourceOpenni2Mirroring() const; //Openni group
int getSourceFreenect2Format() const; //Openni group
bool isSourceRGBDColorOnly() const;
QString getSourceOpenniDevice() const; //Openni group
Transform getSourceOpenniLocalTransform() const; //Openni group
CameraRGBD * createCameraRGBD(bool forCalibration = false); // return camera should be deleted if not null
Transform getSourceLocalTransform() const; //Openni group
Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null
int getIgnoredDCComponents() const;
@@ -221,9 +226,7 @@ public slots:
void setDetectionRate(double value);
void setTimeLimit(float value);
void setSLAMMode(bool enabled);
void selectSourceImage(Src src = kSrcUndef);
void selectSourceDatabase(bool user = false);
void selectSourceRGBD(Src src = kSrcUndef);
void selectSourceDriver(Src src);
void calibrate();
private slots:
@@ -251,11 +254,19 @@ private slots:
void setupTreeView();
void updateBasicParameter();
void openDatabaseViewer();
void selectSourceDatabase();
void selectSourceStereoImagesStamps();
void selectSourceStereoImagesPath();
void selectSourceImagesPath();
void selectSourceVideoPath();
void selectSourceOniPath();
void selectSourceOni2Path();
void updateSourceGrpVisibility();
void updateRGBDCameraGroupBoxVisibility();
void updateRGBCameraGroupBoxVisibility();
void updateStereoCameraGroupBoxVisibility();
void testOdometry();
void testRGBDCamera();
void testCamera();
protected:
virtual void showEvent ( QShowEvent * event );
guilib/src/AboutDialog.cpp
+1
View File
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "AboutDialog.h"
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/CameraStereo.h"
#include "rtabmap/core/Graph.h"
#include "ui_aboutDialog.h"
#include <opencv2/core/version.hpp>
guilib/src/CalibrationDialog.cpp
+2 -2
View File
@@ -167,6 +167,7 @@ void CalibrationDialog::setStereoMode(bool stereo)
ui_->lineEdit_R_2->setVisible(stereo_);
ui_->lineEdit_P_2->setVisible(stereo_);
ui_->radioButton_stereoRectified->setVisible(stereo_);
ui_->checkBox_switchImages->setVisible(stereo_);
}
void CalibrationDialog::setBoardWidth(int width)
@@ -234,8 +235,7 @@ void CalibrationDialog::handleEvent(UEvent * event)
if(event->getClassName().compare("CameraEvent") == 0)
{
rtabmap::CameraEvent * e = (rtabmap::CameraEvent *)event;
if(e->getCode() == rtabmap::CameraEvent::kCodeImage ||
e->getCode() == rtabmap::CameraEvent::kCodeImageDepth)
if(e->getCode() == rtabmap::CameraEvent::kCodeData)
{
processingData_ = true;
QMetaObject::invokeMethod(this, "processImages",
guilib/src/CameraViewer.cpp
+23 -7
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/gui/ImageView.h>
#include <rtabmap/gui/CloudViewer.h>
#include <rtabmap/gui/UCv2Qt.h>
#include <rtabmap/utilite/ULogger.h>
#include <QtCore/QMetaType>
#include <QHBoxLayout>
#include <QVBoxLayout>
@@ -76,17 +77,33 @@ CameraViewer::~CameraViewer()
void CameraViewer::showImage(const rtabmap::SensorData & data)
{
processingImages_ = true;
imageView_->setImage(uCvMat2QImage(data.imageRaw()));
imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightRaw()));
if(!data.depthOrRightRaw().empty() && (data.stereoCameraModel().isValid() || data.cameraModels().size()))
if(!data.imageRaw().empty())
{
cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data));
imageView_->setImage(uCvMat2QImage(data.imageRaw()));
}
if(!data.depthOrRightRaw().empty())
{
imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightRaw()));
}
if((data.stereoCameraModel().isValid() || data.cameraModels().size()))
{
if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
{
cloudView_->addOrUpdateCloud("cloud", util3d::cloudRGBFromSensorData(data));
cloudView_->setVisible(true);
cloudView_->update();
}
else if(!data.depthOrRightRaw().empty())
{
cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data));
cloudView_->setVisible(true);
cloudView_->update();
}
}
else
{
cloudView_->setVisible(false);
}
cloudView_->update();
processingImages_ = false;
}
@@ -95,8 +112,7 @@ void CameraViewer::handleEvent(UEvent * event)
if(event->getClassName().compare("CameraEvent") == 0)
{
CameraEvent * camEvent = (CameraEvent*)event;
if(camEvent->getCode() == CameraEvent::kCodeImageDepth ||
camEvent->getCode() == CameraEvent::kCodeImage)
if(camEvent->getCode() == CameraEvent::kCodeData)
{
if(camEvent->data().isValid())
{
guilib/src/DataRecorder.cpp
+1 -2
View File
@@ -171,8 +171,7 @@ void DataRecorder::handleEvent(UEvent * event)
if(event->getClassName().compare("CameraEvent") == 0)
{
CameraEvent * camEvent = (CameraEvent*)event;
if(camEvent->getCode() == CameraEvent::kCodeImageDepth ||
camEvent->getCode() == CameraEvent::kCodeImage)
if(camEvent->getCode() == CameraEvent::kCodeData)
{
if(camEvent->data().isValid())
{
guilib/src/GuiLib.qrc
+1
View File
@@ -29,5 +29,6 @@
<file>images/sense.png</file>
<file>images/xtion_pro_live.png</file>
<file>images/bumblebee2.png</file>
<file>images/webcam.png</file>
</qresource>
</RCC>
guilib/src/MainWindow.cpp
+58 -156
View File
@@ -29,7 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "ui_mainWindow.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraRGB.h"
#include "rtabmap/core/CameraStereo.h"
#include "rtabmap/core/CameraThread.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/DBReader.h"
@@ -118,7 +119,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_camera(0),
_dbReader(0),
_odomThread(0),
_srcType(kSrcUndefined),
_preferencesDialog(0),
_aboutDialog(0),
_exportDialog(0),
@@ -338,7 +338,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->actionPost_processing->setEnabled(false);
QToolButton* toolButton = new QToolButton(this);
toolButton->setMenu(_ui->menuRGB_D_camera);
toolButton->setMenu(_ui->menuSelect_source);
toolButton->setPopupMode(QToolButton::InstantPopup);
toolButton->setIcon(QIcon(":images/kinect_xbox_360.png"));
toolButton->setToolTip("Select sensor driver");
@@ -351,10 +351,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
#endif
//Settings menu
connect(_ui->actionImageFiles, SIGNAL(triggered()), this, SLOT(selectImages()));
connect(_ui->actionVideo, SIGNAL(triggered()), this, SLOT(selectVideo()));
connect(_ui->actionMore_options, SIGNAL(triggered()), this, SLOT(openPreferencesSource()));
connect(_ui->actionUsbCamera, SIGNAL(triggered()), this, SLOT(selectStream()));
connect(_ui->actionDatabase, SIGNAL(triggered()), this, SLOT(selectDatabase()));
connect(_ui->actionOpenNI_PCL, SIGNAL(triggered()), this, SLOT(selectOpenni()));
connect(_ui->actionOpenNI_PCL_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenni()));
connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect()));
@@ -2113,39 +2111,26 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
// Camera settings...
_ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
this->updateSelectSourceMenu();
QString src;
if(_preferencesDialog->isSourceImageUsed())
{
src = _preferencesDialog->getSourceImageTypeStr();
}
else if(_preferencesDialog->isSourceDatabaseUsed())
{
src = "Database";
}
_ui->label_stats_source->setText(src);
_ui->label_stats_source->setText(_preferencesDialog->getSourceDriverStr());
if(_camera)
{
_camera->setImageRate(_preferencesDialog->getGeneralInputRate());
if(_camera->cameraRGBD() && dynamic_cast<CameraOpenNI2*>(_camera->cameraRGBD()) != 0)
if(_camera->camera() && dynamic_cast<CameraOpenNI2*>(_camera->camera()) != 0)
{
((CameraOpenNI2*)_camera->cameraRGBD())->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance());
((CameraOpenNI2*)_camera->cameraRGBD())->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure());
((CameraOpenNI2*)_camera->camera())->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance());
((CameraOpenNI2*)_camera->camera())->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure());
if(CameraOpenNI2::exposureGainAvailable())
{
((CameraOpenNI2*)_camera->cameraRGBD())->setExposure(_preferencesDialog->getSourceOpenni2Exposure());
((CameraOpenNI2*)_camera->cameraRGBD())->setGain(_preferencesDialog->getSourceOpenni2Gain());
((CameraOpenNI2*)_camera->camera())->setExposure(_preferencesDialog->getSourceOpenni2Exposure());
((CameraOpenNI2*)_camera->camera())->setGain(_preferencesDialog->getSourceOpenni2Gain());
}
}
if(_camera->camera())
if(_camera)
{
_camera->camera()->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
}
if(_camera->cameraRGBD())
{
_camera->cameraRGBD()->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
_camera->cameraRGBD()->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
}
}
if(_dbReader)
@@ -2433,23 +2418,25 @@ bool MainWindow::eventFilter(QObject *obj, QEvent *event)
void MainWindow::updateSelectSourceMenu()
{
_ui->actionUsbCamera->setChecked(_preferencesDialog->isSourceImageUsed() && _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcUsbDevice);
_ui->actionImageFiles->setChecked(_preferencesDialog->isSourceImageUsed() && _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcImages);
_ui->actionVideo->setChecked(_preferencesDialog->isSourceImageUsed() && _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcVideo);
_ui->actionUsbCamera->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUsbDevice);
_ui->actionDatabase->setChecked(_preferencesDialog->isSourceDatabaseUsed());
_ui->actionMore_options->setChecked(
_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase ||
_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages);
_ui->actionOpenNI_PCL->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL);
_ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL);
_ui->actionFreenect->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect);
_ui->actionOpenNI_CV->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV);
_ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
_ui->actionOpenNI2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
_ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
_ui->actionOpenNI2_sense->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
_ui->actionFreenect2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect2);
_ui->actionStereoDC1394->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcStereoDC1394);
_ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcStereoFlyCapture2);
_ui->actionOpenNI_PCL->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
_ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
_ui->actionFreenect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect);
_ui->actionOpenNI_CV->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV);
_ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
_ui->actionOpenNI2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
_ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
_ui->actionOpenNI2_sense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
_ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2);
_ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394);
_ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2);
}
void MainWindow::changeImgRateSetting()
@@ -2674,11 +2661,9 @@ void MainWindow::startDetection()
{
ParametersMap parameters = _preferencesDialog->getAllParameters();
// verify source with input rates
if((_preferencesDialog->isSourceImageUsed() &&
(_preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcImages ||
_preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcVideo))
||
_preferencesDialog->isSourceDatabaseUsed())
if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase)
{
float inputRate = _preferencesDialog->getGeneralInputRate();
float detectionRate = uStr2Float(parameters.at(Parameters::kRtabmapDetectionRate()));
@@ -2748,9 +2733,7 @@ void MainWindow::startDetection()
}
// Adjust pre-requirements
if( !_preferencesDialog->isSourceImageUsed() &&
!_preferencesDialog->isSourceDatabaseUsed() &&
!_preferencesDialog->isSourceRGBDUsed())
if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUndef)
{
QMessageBox::warning(this,
tr("RTAB-Map"),
@@ -2760,41 +2743,19 @@ void MainWindow::startDetection()
return;
}
if(_preferencesDialog->isSourceRGBDUsed())
{
CameraRGBD * camera = _preferencesDialog->createCameraRGBD();
if(!camera->init(_preferencesDialog->getCameraInfoDir().toStdString()))
if(_preferencesDialog->getSourceDriver() < PreferencesDialog::kSrcDatabase)
{
Camera * camera = _preferencesDialog->createCamera();
if(!camera)
{
ULOGGER_WARN("init camera failed... ");
QMessageBox::warning(this,
tr("RTAB-Map"),
tr("Camera initialization failed..."));
emit stateChanged(kInitialized);
delete camera;
camera = 0;
if(_odomThread)
{
delete _odomThread;
_odomThread = 0;
}
return;
}
else if(dynamic_cast<CameraOpenNI2*>(camera) != 0)
{
((CameraOpenNI2*)camera)->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance());
((CameraOpenNI2*)camera)->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure());
((CameraOpenNI2*)camera)->setMirroring(_preferencesDialog->getSourceOpenni2Mirroring());
if(CameraOpenNI2::exposureGainAvailable())
{
((CameraOpenNI2*)camera)->setExposure(_preferencesDialog->getSourceOpenni2Exposure());
((CameraOpenNI2*)camera)->setGain(_preferencesDialog->getSourceOpenni2Gain());
}
}
camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
_camera = new CameraThread(camera);
_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
//Create odometry thread if rgbd slam
if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
@@ -2823,6 +2784,7 @@ void MainWindow::startDetection()
{
UERROR("OdomThread must be already deleted here?!");
delete _odomThread;
_odomThread = 0;
}
Odometry * odom;
if(_preferencesDialog->getOdomStrategy() == 1)
@@ -2845,7 +2807,7 @@ void MainWindow::startDetection()
}
}
}
else if(_preferencesDialog->isSourceDatabaseUsed())
else if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase)
{
_dbReader = new DBReader(_preferencesDialog->getSourceDatabasePath().toStdString(),
_preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate(),
@@ -2902,58 +2864,6 @@ void MainWindow::startDetection()
UEventsManager::createPipe(_dbReader, _odomThread, "CameraEvent");
}
}
else
{
if(_preferencesDialog->isSourceImageUsed())
{
Camera * camera = 0;
// Change type of the camera...
//
int sourceType = _preferencesDialog->getSourceImageType();
UASSERT(sourceType >= PreferencesDialog::kSrcUsbDevice && sourceType <= PreferencesDialog::kSrcVideo);
if(sourceType == PreferencesDialog::kSrcImages) //Images
{
camera = new CameraImages(
_preferencesDialog->getSourceImagesPath().append(QDir::separator()).toStdString(),
_preferencesDialog->getSourceImagesStartPos(),
_preferencesDialog->getSourceImagesRefreshDir(),
_preferencesDialog->getGeneralInputRate(),
_preferencesDialog->getSourceWidth(),
_preferencesDialog->getSourceHeight());
}
else if(sourceType == PreferencesDialog::kSrcVideo)
{
camera = new CameraVideo(
_preferencesDialog->getSourceVideoPath().toStdString(),
_preferencesDialog->getGeneralInputRate(),
_preferencesDialog->getSourceWidth(),
_preferencesDialog->getSourceHeight());
}
else //if(sourceType == PreferencesDialog::kSrcUsbDevice)
{
camera = new CameraVideo(
_preferencesDialog->getSourceUsbDeviceId(),
_preferencesDialog->getGeneralInputRate(),
_preferencesDialog->getSourceWidth(),
_preferencesDialog->getSourceHeight());
}
if(!camera->init())
{
ULOGGER_WARN("init camera failed... ");
QMessageBox::warning(this,
tr("RTAB-Map"),
tr("Camera initialization failed..."));
emit stateChanged(kInitialized);
delete camera;
camera = 0;
return;
}
camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
_camera = new CameraThread(camera);
}
}
if(_dataRecorder)
{
@@ -3810,64 +3720,49 @@ void MainWindow::updateEditMenu()
}
}
void MainWindow::selectImages()
{
_preferencesDialog->selectSourceImage(PreferencesDialog::kSrcImages);
}
void MainWindow::selectVideo()
{
_preferencesDialog->selectSourceImage(PreferencesDialog::kSrcVideo);
}
void MainWindow::selectStream()
{
_preferencesDialog->selectSourceImage(PreferencesDialog::kSrcUsbDevice);
}
void MainWindow::selectDatabase()
{
_preferencesDialog->selectSourceDatabase(true);
_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcUsbDevice);
}
void MainWindow::selectOpenni()
{
_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_PCL);
_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_PCL);
}
void MainWindow::selectFreenect()
{
_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect);
_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect);
}
void MainWindow::selectOpenniCv()
{
_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV);
_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV);
}
void MainWindow::selectOpenniCvAsus()
{
_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV_ASUS);
_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV_ASUS);
}
void MainWindow::selectOpenni2()
{
_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI2);
_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI2);
}
void MainWindow::selectFreenect2()
{
_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect2);
_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect2);
}
void MainWindow::selectStereoDC1394()
{
_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcStereoDC1394);
_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcDC1394);
}
void MainWindow::selectStereoFlyCapture2()
{
_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcStereoFlyCapture2);
_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFlyCapture2);
}
@@ -4116,6 +4011,13 @@ void MainWindow::openPreferences()
_preferencesDialog->exec();
}
void MainWindow::openPreferencesSource()
{
_preferencesDialog->setCurrentPanelToSource();
openPreferences();
this->updateSelectSourceMenu();
}
void MainWindow::setDefaultViews()
{
_ui->dockWidget_posterior->setVisible(false);
guilib/src/OdometryViewer.cpp
+8
View File
@@ -97,8 +97,10 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, f
decimationSpin_->setMaximum(16);
decimationSpin_->setValue(decimation);
timeLabel_ = new QLabel(this);
QPushButton * resetButton = new QPushButton("reset", this);
QPushButton * clearButton = new QPushButton("clear", this);
QPushButton * closeButton = new QPushButton("close", this);
connect(resetButton, SIGNAL(clicked()), this, SLOT(reset()));
connect(clearButton, SIGNAL(clicked()), this, SLOT(clear()));
connect(closeButton, SIGNAL(clicked()), this, SLOT(reject()));
@@ -121,6 +123,7 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, f
hlayout2->addWidget(decimationSpin_);
hlayout2->addWidget(timeLabel_);
hlayout2->addStretch(1);
hlayout2->addWidget(resetButton);
hlayout2->addWidget(clearButton);
hlayout2->addWidget(closeButton);
@@ -140,6 +143,11 @@ OdometryViewer::~OdometryViewer()
UDEBUG("");
}
void OdometryViewer::reset()
{
this->post(new OdometryResetEvent());
}
void OdometryViewer::clear()
{
addedClouds_.clear();
guilib/src/PreferencesDialog.cpp
+610 -524
View File
File diff suppressed because it is too large Load Diff
guilib/src/images/webcam.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 11 KiB

guilib/src/ui/mainWindow.ui
+39 -35
View File
@@ -95,16 +95,22 @@
</property>
<widget class="QMenu" name="menuImage">
<property name="title">
<string>Image-only</string>
<string>RGB camera</string>
</property>
<property name="icon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/webcam.png</normaloff>:/images/webcam.png</iconset>
</property>
<addaction name="actionUsbCamera"/>
<addaction name="actionImageFiles"/>
<addaction name="actionVideo"/>
</widget>
<widget class="QMenu" name="menuRGB_D_camera">
<property name="title">
<string>RGB-D camera</string>
</property>
<property name="icon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/kinect_xbox_360.png</normaloff>:/images/kinect_xbox_360.png</iconset>
</property>
<widget class="QMenu" name="menuKinect_for_Xbox_360">
<property name="title">
<string>Kinect</string>
@@ -150,7 +156,20 @@
</property>
<addaction name="actionFreenect2"/>
</widget>
<widget class="QMenu" name="menuBumblebee2">
<addaction name="menuKinect_for_Xbox_360"/>
<addaction name="menuXtion_PRO_LIVE"/>
<addaction name="menuSense_3D_scanner"/>
<addaction name="menuKinect_v2"/>
</widget>
<widget class="QMenu" name="menuStereo_camera">
<property name="title">
<string>Stereo camera</string>
</property>
<property name="icon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/bumblebee2.png</normaloff>:/images/bumblebee2.png</iconset>
</property>
<widget class="QMenu" name="menuBumblebee2_2">
<property name="title">
<string>Bumblebee2</string>
</property>
@@ -161,15 +180,12 @@
<addaction name="actionStereoDC1394"/>
<addaction name="actionStereoFlyCapture2"/>
</widget>
<addaction name="menuKinect_for_Xbox_360"/>
<addaction name="menuXtion_PRO_LIVE"/>
<addaction name="menuSense_3D_scanner"/>
<addaction name="menuKinect_v2"/>
<addaction name="menuBumblebee2"/>
<addaction name="menuBumblebee2_2"/>
</widget>
<addaction name="menuRGB_D_camera"/>
<addaction name="menuStereo_camera"/>
<addaction name="menuImage"/>
<addaction name="actionDatabase"/>
<addaction name="actionMore_options"/>
</widget>
<addaction name="menuSelect_source"/>
<addaction name="separator"/>
@@ -946,34 +962,14 @@
<property name="checkable">
<bool>true</bool>
</property>
<property name="icon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/webcam.png</normaloff>:/images/webcam.png</iconset>
</property>
<property name="text">
<string>Usb camera</string>
</property>
</action>
<action name="actionImageFiles">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Images...</string>
</property>
</action>
<action name="actionVideo">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Video...</string>
</property>
</action>
<action name="actionDatabase">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Database...</string>
</property>
</action>
<action name="actionGenerate_local_map">
<property name="text">
<string>Generate graph local map (*.dot)...</string>
@@ -1203,7 +1199,7 @@
</action>
<action name="actionStereoFlyCapture2">
<property name="text">
<string>StereoFlyCapture2</string>
<string>FlyCapture2</string>
</property>
</action>
<action name="actionSend_goal">
@@ -1226,6 +1222,14 @@
<string>Default views</string>
</property>
</action>
<action name="actionMore_options">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>More options...</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
guilib/src/ui/preferencesDialog.ui
+978 -753
View File
File diff suppressed because it is too large Load Diff
tools/Calibration/main.cpp
+5 -15
View File
@@ -25,8 +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/Camera.h"
#include "rtabmap/core/CameraRGB.h"
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/CameraStereo.h"
#include "rtabmap/core/CameraThread.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UConversion.h"
@@ -130,11 +131,10 @@ int main(int argc, char * argv[])
bool switchImages = false;
rtabmap::Camera * cameraUsb = 0;
rtabmap::CameraRGBD * camera = 0;
rtabmap::Camera * camera = 0;
if(driver == -1)
{
cameraUsb = new rtabmap::CameraVideo(device);
camera = new rtabmap::CameraVideo(device);
}
else if(driver == 0)
{
@@ -211,17 +211,7 @@ int main(int argc, char * argv[])
rtabmap::CameraThread * cameraThread = 0;
if(cameraUsb)
{
if(!cameraUsb->init())
{
printf("Camera init failed!\n");
delete cameraUsb;
exit(1);
}
cameraThread = new rtabmap::CameraThread(cameraUsb);
}
else if(camera)
if(camera)
{
if(!camera->init(""))
{
tools/Camera/main.cpp
+10 -12
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/Camera.h"
#include "rtabmap/core/CameraRGB.h"
#include "rtabmap/core/DBReader.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UFile.h"
@@ -40,7 +40,7 @@ void showUsage()
"rtabmap-camera [option] \n"
" Options:\n"
" --device # USB camera device id (default 0).\n"
" --rate # Frame rate (default 30 Hz). 0 means as fast as possible.\n"
" --rate # Frame rate (default 0 Hz). 0 means as fast as possible.\n"
" --path "" Path to a directory of images or a video file.\n"
" --calibration "" Calibration file (*.yaml).\n\n");
exit(1);
@@ -53,7 +53,7 @@ int main(int argc, char * argv[])
int device = 0;
std::string path;
float rate = 30.0f;
float rate = 0.0f;
std::string calibrationFile;
for(int i=1; i<argc; ++i)
{
@@ -164,18 +164,16 @@ int main(int argc, char * argv[])
if(camera)
{
if(!camera->init())
if(!calibrationFile.empty())
{
UINFO("Set calibration: %s", calibrationFile.c_str());
}
if(!camera->init(UDirectory::getDir(calibrationFile), UFile::getName(calibrationFile)))
{
delete camera;
UERROR("Cannot initialize the camera.");
return -1;
}
if(!calibrationFile.empty())
{
UINFO("Set calibration: %s", calibrationFile.c_str());
camera->setCalibration(calibrationFile);
}
}
if(dbReader)
@@ -189,7 +187,7 @@ int main(int argc, char * argv[])
}
cv::Mat rgb;
rgb = camera?camera->takeImage():dbReader->getNextData().data().imageRaw();
rgb = camera?camera->takeImage().imageRaw():dbReader->getNextData().data().imageRaw();
cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
while(!rgb.empty())
{
@@ -199,7 +197,7 @@ int main(int argc, char * argv[])
if(c == 27)
break; // if ESC, break and quit
rgb = camera?camera->takeImage():dbReader->getNextData().data().imageRaw();
rgb = camera?camera->takeImage().imageRaw():dbReader->getNextData().data().imageRaw();
}
cv::destroyWindow("Video");
if(camera)
tools/CameraRGBD/main.cpp
+42 -24
View File
@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/CameraStereo.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h"
@@ -77,7 +78,7 @@ int main(int argc, char * argv[])
}
UINFO("Using driver %d", driver);
rtabmap::CameraRGBD * camera = 0;
rtabmap::Camera * camera = 0;
if(driver == 0)
{
camera = new rtabmap::CameraOpenni();
@@ -156,42 +157,55 @@ int main(int argc, char * argv[])
delete camera;
exit(1);
}
cv::Mat rgb, depth;
float fx, fy, cx, cy;
double stamp = 0.0;
camera->takeImage(rgb, depth, fx, fy, cx, cy, stamp);
if(rgb.cols != depth.cols || rgb.rows != depth.rows)
rtabmap::SensorData data = camera->takeImage();
if(data.imageRaw().cols != data.depthOrRightRaw().cols || data.imageRaw().rows != data.depthOrRightRaw().rows)
{
UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.",
rgb.cols, rgb.rows, depth.cols, depth.rows);
data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
}
if(!fx || !fy)
if(!data.stereoCameraModel().isValid() && (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid()))
{
UWARN("fx and/or fy are not set! The registered cloud cannot be shown.");
UWARN("Camera not calibrated! The registered cloud cannot be shown.");
}
pcl::visualization::CloudViewer viewer("cloud");
rtabmap::Transform t(1, 0, 0, 0,
0, -1, 0, 0,
0, 0, -1, 0);
while(!rgb.empty() && !viewer.wasStopped())
while(!data.imageRaw().empty() && !viewer.wasStopped())
{
if(depth.type() == CV_16UC1 || depth.type() == CV_32FC1)
cv::Mat rgb = data.imageRaw();
if(!data.depthRaw().empty() && (data.depthRaw().type() == CV_16UC1 || data.depthRaw().type() == CV_32FC1))
{
// depth
cv::Mat depth = data.depthRaw();
if(depth.type() == CV_32FC1)
{
depth = rtabmap::util3d::cvtDepthFromFloat(depth);
}
if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy)
if(rgb.cols == depth.cols && rgb.rows == depth.rows &&
data.cameraModels().size() &&
data.cameraModels()[0].isValid())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(
rgb, depth,
data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy());
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
viewer.showCloud(cloud, "cloud");
}
else if(!depth.empty() && fx && fy)
else if(!depth.empty() &&
data.cameraModels().size() &&
data.cameraModels()[0].isValid())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(depth, cx, cy, fx, fy);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(
depth,
data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy());
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
viewer.showCloud(cloud, "cloud");
}
@@ -204,19 +218,25 @@ int main(int argc, char * argv[])
cv::imshow("Video", rgb); // show frame
cv::imshow("Depth", tmp);
}
else
else if(!data.rightRaw().empty())
{
// stereo
cv::Mat right = data.rightRaw();
cv::imshow("Left", rgb); // show frame
cv::imshow("Right", depth);
cv::imshow("Right", right);
if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy)
if(rgb.cols == right.cols && rgb.rows == right.rows && data.stereoCameraModel().isValid())
{
if(depth.channels() == 3)
if(right.channels() == 3)
{
cv::cvtColor(depth, depth, CV_BGR2GRAY);
cv::cvtColor(right, right, CV_BGR2GRAY);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(rgb, depth, cx, cy, fx, fy);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(
rgb, right,
data.stereoCameraModel().left().cx(),
data.stereoCameraModel().left().cy(),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline());
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
viewer.showCloud(cloud, "cloud");
}
@@ -226,9 +246,7 @@ int main(int argc, char * argv[])
if(c == 27)
break; // if ESC, break and quit
rgb = cv::Mat();
depth = cv::Mat();
camera->takeImage(rgb, depth, fx, fy, cx, cy, stamp);
data = camera->takeImage();
}
cv::destroyWindow("Video");
cv::destroyWindow("Depth");
tools/ConsoleApp/main.cpp
+7 -54
View File
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraRGB.h"
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UConversion.h>
@@ -53,10 +53,6 @@ void showUsage()
" -rateHz #.## Acquisition rate (Hz), for convenience\n"
" -repeat # Repeat the process on the data set # times (minimum of 1)\n"
" -createGT Generate a ground truth file\n"
" -image_width # Force an image width (Default 0: original size used).\n"
" The height must be also specified if changed.\n"
" -image_height # Force an image height (Default 0: original size used)\n"
" The height must be also specified if changed.\n"
" -start_at # When \"path\" is a directory of images, set this parameter\n"
" to start processing at image # (default 1).\n"
" -\"parameter name\" \"value\" Overwrite a specific RTAB-Map's parameter :\n"
@@ -118,8 +114,6 @@ int main(int argc, char * argv[])
int repeat = 0;
bool createGT = false;
std::string inputDbPath;
int imageWidth = 0;
int imageHeight = 0;
int startAt = 1;
ParametersMap pm;
ULogger::Level logLevel = ULogger::kError;
@@ -194,40 +188,6 @@ int main(int argc, char * argv[])
}
continue;
}
if(strcmp(argv[i], "-image_width") == 0)
{
++i;
if(i < argc)
{
imageWidth = std::atoi(argv[i]);
if(imageWidth < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-image_height") == 0)
{
++i;
if(i < argc)
{
imageHeight = std::atoi(argv[i]);
if(imageHeight < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-start_at") == 0)
{
++i;
@@ -328,12 +288,6 @@ int main(int argc, char * argv[])
printf("Cannot create a Ground truth if repeat is on.\n");
showUsage();
}
else if((imageWidth && imageHeight == 0) ||
(imageHeight && imageWidth == 0))
{
printf("If imageWidth is set, imageHeight must be too.\n");
showUsage();
}
UTimer timer;
timer.start();
@@ -342,11 +296,11 @@ int main(int argc, char * argv[])
Camera * camera = 0;
if(UDirectory::exists(path))
{
camera = new CameraImages(path, startAt, false, 1/rate, imageWidth, imageHeight);
camera = new CameraImages(path, startAt, false, 1/rate);
}
else
{
camera = new CameraVideo(path, 1/rate, imageWidth, imageHeight);
camera = new CameraVideo(path, 1/rate);
}
if(!camera || !camera->init())
@@ -395,7 +349,6 @@ int main(int argc, char * argv[])
printf(" Time threshold = %1.2f ms\n", rtabmap.getTimeThreshold());
printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate);
printf(" Repeating data set = %s\n", repeat?"true":"false");
printf(" Camera width=%d, height=%d (0 is default)\n", imageWidth, imageHeight);
printf(" Camera starts at image %d (default 1)\n", startAt);
if(createGT)
{
@@ -422,23 +375,23 @@ int main(int argc, char * argv[])
std::list<std::vector<float> > teleopActions;
while(loopDataset <= repeat && g_forever)
{
cv::Mat img = camera->takeImage();
SensorData data = camera->takeImage();
int i=0;
double maxIterationTime = 0.0;
int maxIterationTimeId = 0;
while(!img.empty() && g_forever)
while(!data.imageRaw().empty() && g_forever)
{
++imagesProcessed;
iterationTimer.start();
rtabmapTimer.start();
rtabmap.process(img);
rtabmap.process(data.imageRaw());
double rtabmapTime = rtabmapTimer.elapsed();
loopClosureId = rtabmap.getLoopClosureId();
if(rtabmap.getLoopClosureId())
{
++countLoopDetected;
}
img = camera->takeImage();
data = camera->takeImage();
if(++count % 100 == 0)
{
printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n",
tools/DataRecorder/main.cpp
+3 -2
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/CameraThread.h>
#include <rtabmap/core/CameraRGBD.h>
#include <rtabmap/core/CameraStereo.h>
#include <rtabmap/core/Camera.h>
#include <rtabmap/core/CameraThread.h>
#include <rtabmap/gui/DataRecorder.h>
@@ -174,7 +175,7 @@ int main (int argc, char * argv[])
signal(SIGTERM, &sighandler);
signal(SIGINT, &sighandler);
rtabmap::CameraRGBD * camera = 0;
rtabmap::Camera * camera = 0;
rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
if(driver == 0)
{
@@ -263,7 +264,7 @@ int main (int argc, char * argv[])
app->processEvents();
}
if(cam->init())
if(camera->init())
{
cam->start();
tools/OdometryViewer/main.cpp
+2 -1
View File
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/gui/OdometryViewer.h>
#include <rtabmap/core/CameraThread.h>
#include <rtabmap/core/CameraRGBD.h>
#include <rtabmap/core/CameraStereo.h>
#include <rtabmap/core/DBReader.h>
#include <rtabmap/core/VWDictionary.h>
#include <QApplication>
@@ -725,7 +726,7 @@ int main (int argc, char * argv[])
}
else
{
rtabmap::CameraRGBD * camera = 0;
rtabmap::Camera * camera = 0;
rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
if(driver == 0)
{