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

GUI: Added new Freenect2 and StereoDC1394 source options.

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-06 00:28:04 -04:00
parent fee32c4e25
commit 04b35b2adf
22 changed files with 765 additions and 758 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -80,26 +80,19 @@ class RTABMAP_EXP CameraRGBD
public:
virtual ~CameraRGBD();
void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
virtual bool init() = 0;
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;}
float getFx() const {return _fx;}
float getFy() const {return _fy;}
float getCx() const {return _cx;}
float getCy() const {return _cy;}
//setters
void setImageRate(float imageRate) {_imageRate = imageRate;}
void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
void setMirroringEnabled(bool mirroring) {_mirroring = mirroring;}
void setFx(float fx) {_fx = fx;}
void setFy(float fy) {_fy = fy;}
void setCx(float cx) {_cx = cx;}
void setCy(float cy) {_cy = cy;}
protected:
/**
@@ -108,11 +101,7 @@ protected:
* @param imageRate : image/second , 0 for fast as the camera can
*/
CameraRGBD(float imageRate = 0,
const Transform & localTransform = Transform::getIdentity(),
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
const Transform & localTransform = Transform::getIdentity());
/**
* returned rgb and depth images should be already rectified
@@ -124,10 +113,6 @@ private:
Transform _localTransform;
bool _mirroring;
UTimer * _frameRateTimer;
float _fx;
float _fy;
float _cx;
float _cy;
};
/////////////////////////
@@ -143,11 +128,7 @@ public:
// default local transform z in, x right, y down));
CameraOpenni(const std::string & deviceId="",
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity(),
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraOpenni();
void image_cb (
@@ -155,7 +136,8 @@ public:
const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
float constant);
virtual bool init();
virtual bool init(const std::string & calibrationFolder = ".");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
@@ -185,14 +167,11 @@ public:
public:
CameraOpenNICV(bool asus = false,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity(),
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraOpenNICV();
virtual bool init();
virtual bool init(const std::string & calibrationFolder = ".");
virtual bool isCalibrated() const;
virtual std::string getSerial() const {return "";} // unknown with OpenCV
protected:
@@ -218,14 +197,11 @@ public:
public:
CameraOpenNI2(const std::string & deviceId = "",
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity(),
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraOpenNI2();
virtual bool init();
virtual bool init(const std::string & calibrationFolder = ".");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
bool setAutoWhiteBalance(bool enabled);
@@ -262,14 +238,11 @@ public:
// default local transform z in, x right, y down));
CameraFreenect(int deviceId= 0,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraFreenect();
bool init();
bool init(const std::string & calibrationFolder = ".");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
@@ -295,14 +268,11 @@ public:
// default local transform z in, x right, y down));
CameraFreenect2(int deviceId= 0,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraFreenect2();
bool init();
bool init(const std::string & calibrationFolder = ".");
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
protected:
@@ -320,39 +290,24 @@ private:
/////////////////////////
class DC1394Device;
class RTABMAP_EXP CameraDC1394 :
class RTABMAP_EXP CameraStereoDC1394 :
public CameraRGBD
{
public:
static bool available();
public:
// The first constructor will not check for calibration files, so
// the images returned won't be rectified
CameraDC1394( float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
CameraDC1394(const std::string & calibrationFolder,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
virtual ~CameraDC1394();
CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoDC1394();
bool init();
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);
private:
bool lookForCalibration_;
std::string calibrationFolder_;
DC1394Device *device_;
StereoCameraModel stereoModel_;
};

122
corelib/src/CameraRGBD.cpp
View File

@@ -66,15 +66,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap
{
CameraRGBD::CameraRGBD(float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
CameraRGBD::CameraRGBD(float imageRate, const Transform & localTransform) :
_imageRate(imageRate),
_localTransform(localTransform),
_mirroring(false),
_frameRateTimer(new UTimer()),
_fx(fx),
_fy(fy),
_cx(cx),
_cy(cy)
_frameRateTimer(new UTimer())
{
}
@@ -110,22 +106,6 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & f
UTimer timer;
this->captureImage(rgb, depth, fx, fy, cx, cy);
if(_fx)
{
fx = _fx; // override if set
}
if(_fy)
{
fy = _fy; // override if set
}
if(_cx)
{
cx = _cx; // override if set
}
if(_cy)
{
cy = _cy; // override if set
}
if(!rgb.empty() && !depth.empty() && _mirroring)
{
cv::flip(rgb,rgb,1);
@@ -141,8 +121,8 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & f
/////////////////////////
// CameraOpenNIPCL
/////////////////////////
CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform) :
CameraRGBD(imageRate, localTransform),
interface_(0),
deviceId_(deviceId),
depthConstant_(0.0f)
@@ -190,7 +170,7 @@ void CameraOpenni::image_cb (
}
}
bool CameraOpenni::init()
bool CameraOpenni::init(const std::string & calibrationFolder)
{
if(interface_)
{
@@ -225,6 +205,11 @@ bool CameraOpenni::init()
return true;
}
bool CameraOpenni::isCalibrated() const
{
return depthConstant_ > 0.0f;
}
std::string CameraOpenni::getSerial() const
{
if(interface_)
@@ -266,8 +251,8 @@ bool CameraOpenNICV::available()
return cv::getBuildInformation().find("OpenNI: YES") != std::string::npos;
}
CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform, float fx, float fy, float cx, float cy) :
CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform) :
CameraRGBD(imageRate, localTransform),
_asus(asus),
_depthFocal(0.0f)
{
@@ -279,7 +264,7 @@ CameraOpenNICV::~CameraOpenNICV()
_capture.release();
}
bool CameraOpenNICV::init()
bool CameraOpenNICV::init(const std::string & calibrationFolder)
{
if(_capture.isOpened())
{
@@ -328,6 +313,11 @@ bool CameraOpenNICV::init()
return true;
}
bool CameraOpenNICV::isCalibrated() const
{
return _depthFocal > 0.0f;
}
void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{
if(_capture.isOpened())
@@ -375,12 +365,8 @@ bool CameraOpenNI2::exposureGainAvailable()
CameraOpenNI2::CameraOpenNI2(
const std::string & deviceId,
float imageRate,
const rtabmap::Transform & localTransform,
float fx,
float fy,
float cx,
float cy) :
CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
const rtabmap::Transform & localTransform) :
CameraRGBD(imageRate, localTransform),
#ifdef WITH_OPENNI2
_device(new openni::Device()),
_color(new openni::VideoStream()),
@@ -484,7 +470,7 @@ bool CameraOpenNI2::setMirroring(bool enabled)
return false;
}
bool CameraOpenNI2::init()
bool CameraOpenNI2::init(const std::string & calibrationFolder)
{
#ifdef WITH_OPENNI2
openni::OpenNI::initialize();
@@ -646,6 +632,11 @@ bool CameraOpenNI2::init()
#endif
}
bool CameraOpenNI2::isCalibrated() const
{
return _depthFx > 0.0f && _depthFy > 0.0f;
}
std::string CameraOpenNI2::getSerial() const
{
if(_device)
@@ -904,8 +895,8 @@ bool CameraFreenect::available()
#endif
}
CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform) :
CameraRGBD(imageRate, localTransform),
deviceId_(deviceId),
ctx_(0),
freenectDevice_(0)
@@ -933,7 +924,7 @@ CameraFreenect::~CameraFreenect()
#endif
}
bool CameraFreenect::init()
bool CameraFreenect::init(const std::string & calibrationFolder)
{
#ifdef WITH_FREENECT
if(freenectDevice_)
@@ -969,6 +960,11 @@ bool CameraFreenect::init()
return false;
}
bool CameraFreenect::isCalibrated() const
{
return freenectDevice_ != 0 && freenectDevice_->getDepthFocal() > 0.0f;
}
std::string CameraFreenect::getSerial() const
{
#ifdef WITH_FREENECT
@@ -1032,8 +1028,8 @@ bool CameraFreenect2::available()
#endif
}
CameraFreenect2::CameraFreenect2(int deviceId, float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
CameraFreenect2::CameraFreenect2(int deviceId, float imageRate, const Transform & localTransform) :
CameraRGBD(imageRate, localTransform),
deviceId_(deviceId),
freenect2_(0),
dev_(0)
@@ -1065,7 +1061,7 @@ CameraFreenect2::~CameraFreenect2()
#endif
}
bool CameraFreenect2::init()
bool CameraFreenect2::init(const std::string & calibrationFolder)
{
#ifdef WITH_FREENECT2
if(dev_)
@@ -1103,6 +1099,12 @@ bool CameraFreenect2::init()
return false;
}
bool CameraFreenect2::isCalibrated() const
{
UWARN("Freenect2 calibration not yet implemented!");
return false;
}
std::string CameraFreenect2::getSerial() const
{
#ifdef WITH_FREENECT2
@@ -1440,7 +1442,7 @@ private:
};
#endif
bool CameraDC1394::available()
bool CameraStereoDC1394::available()
{
#ifdef WITH_DC1394
return true;
@@ -1449,9 +1451,8 @@ bool CameraDC1394::available()
#endif
}
CameraDC1394::CameraDC1394(float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
lookForCalibration_(false),
CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localTransform) :
CameraRGBD(imageRate, localTransform),
device_(0)
{
#ifdef WITH_DC1394
@@ -1459,18 +1460,7 @@ CameraDC1394::CameraDC1394(float imageRate, const Transform & localTransform, fl
#endif
}
CameraDC1394::CameraDC1394(const std::string & calibrationFolder, float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
lookForCalibration_(true),
calibrationFolder_(calibrationFolder),
device_(0)
{
#ifdef WITH_DC1394
device_ = new DC1394Device();
#endif
}
CameraDC1394::~CameraDC1394()
CameraStereoDC1394::~CameraStereoDC1394()
{
#ifdef WITH_DC1394
if(device_)
@@ -1480,7 +1470,7 @@ CameraDC1394::~CameraDC1394()
#endif
}
bool CameraDC1394::init()
bool CameraStereoDC1394::init(const std::string & calibrationFolder)
{
#ifdef WITH_DC1394
if(device_)
@@ -1489,11 +1479,12 @@ bool CameraDC1394::init()
if(ok)
{
// look for calibration files
if(lookForCalibration_)
if(!calibrationFolder.empty())
{
if(!stereoModel_.load(calibrationFolder_, device_->guid()))
if(!stereoModel_.load(calibrationFolder, device_->guid()))
{
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", device_->guid().c_str(), calibrationFolder_.c_str());
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
device_->guid().c_str(), calibrationFolder.c_str());
}
}
}
@@ -1505,7 +1496,12 @@ bool CameraDC1394::init()
return false;
}
std::string CameraDC1394::getSerial() const
bool CameraStereoDC1394::isCalibrated() const
{
return stereoModel_.isValid();
}
std::string CameraStereoDC1394::getSerial() const
{
#ifdef WITH_DC1394
if(device_)
@@ -1516,7 +1512,7 @@ std::string CameraDC1394::getSerial() const
return "";
}
void CameraDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy)
void CameraStereoDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy)
{
#ifdef WITH_DC1394
left = cv::Mat();

16
corelib/src/Rtabmap.cpp
View File

@@ -867,10 +867,22 @@ bool Rtabmap::process(const SensorData & data)
// Memory Update : Location creation + Add to STM + Weight Update (Rehearsal)
//============================================================
ULOGGER_INFO("Updating memory...");
if(!_memory->update(data, &statistics_))
if(_rgbdSlamMode)
{
return false;
if(!_memory->update(data, &statistics_))
{
return false;
}
}
else
{
SensorData dataImageOnly(data.image(), data.id(), data.stamp(), data.userData());
if(!_memory->update(dataImageOnly, &statistics_))
{
return false;
}
}
signature = _memory->getLastWorkingSignature();
if(!signature)