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

Added Kinect v2 calibration and registration

Browse Source
This commit is contained in:
matlabbe
2015-04-10 17:36:13 -04:00
parent 8ab867b802
commit 0e21be7f67
12 changed files with 561 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
corelib/include/rtabmap/core/CameraModel.h
View File

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/Transform.h"
namespace rtabmap { namespace rtabmap {
@@ -61,10 +62,10 @@ public:
double cy() const {return P_.at<double>(1,2);} double cy() const {return P_.at<double>(1,2);}
double Tx() const {return P_.at<double>(0,3);} double Tx() const {return P_.at<double>(0,3);}
const cv::Mat & K() const {return K_;} const cv::Mat & K() const {return K_;} //intrinsic camera matrix
const cv::Mat & D() const {return D_;} const cv::Mat & D() const {return D_;} //intrinsic distorsion matrix
const cv::Mat & R() const {return R_;} const cv::Mat & R() const {return R_;} //rectification matrix
const cv::Mat & P() const {return P_;} const cv::Mat & P() const {return P_;} //projection matrix
const cv::Size & imageSize() const {return imageSize_;} const cv::Size & imageSize() const {return imageSize_;}
int imageWidth() const {return imageSize_.width;} int imageWidth() const {return imageSize_.width;}
@@ -73,7 +74,9 @@ public:
bool load(const std::string & filePath); bool load(const std::string & filePath);
bool save(const std::string & filePath); bool save(const std::string & filePath);
cv::Mat rectifyImage(const cv::Mat & raw) const; // For depth images, your should use cv::INTER_NEAREST
cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
cv::Mat rectifyDepth(const cv::Mat & raw) const;
private: private:
std::string name_; std::string name_;
@@ -82,20 +85,22 @@ private:
cv::Mat D_; cv::Mat D_;
cv::Mat R_; cv::Mat R_;
cv::Mat P_; cv::Mat P_;
cv::Mat rectificationMap1_; cv::Mat mapX_;
cv::Mat rectificationMap2_; cv::Mat mapY_;
}; };
class StereoCameraModel class StereoCameraModel
{ {
public: public:
StereoCameraModel() {} StereoCameraModel() {}
StereoCameraModel(const std::string & name, const cv::Size & imageSize, StereoCameraModel(const std::string & name,
const cv::Size & imageSize1,
const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1, const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
const cv::Size & imageSize2,
const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2, const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F) : const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F) :
left_(name+"_left", imageSize, K1, D1, R1, P1), left_(name+"_left", imageSize1, K1, D1, R1, P1),
right_(name+"_right", imageSize, K2, D2, R2, P2), right_(name+"_right", imageSize2, K2, D2, R2, P2),
name_(name), name_(name),
R_(R), R_(R),
T_(T), T_(T),
@@ -110,8 +115,16 @@ public:
bool load(const std::string & directory, const std::string & cameraName); bool load(const std::string & directory, const std::string & cameraName);
bool save(const std::string & directory, const std::string & cameraName); bool save(const std::string & directory, const std::string & cameraName);
double baseline() const {return -right_.Tx()/right_.fx();} double baseline() const {return -right_.Tx()/right_.fx();}
const cv::Mat & R() const {return R_;} //extrinsic rotation matrix
const cv::Mat & T() const {return T_;} //extrinsic translation matrix
const cv::Mat & E() const {return E_;} //extrinsic essential matrix
const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
Transform transform() const;
const CameraModel & left() const {return left_;} const CameraModel & left() const {return left_;}
const CameraModel & right() const {return right_;} const CameraModel & right() const {return right_;}

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

@@ -63,6 +63,8 @@ namespace libfreenect2
class Freenect2; class Freenect2;
class Freenect2Device; class Freenect2Device;
class SyncMultiFrameListener; class SyncMultiFrameListener;
class Registration;
class PacketPipeline;
} }
namespace FlyCapture2 namespace FlyCapture2
@@ -294,9 +296,12 @@ protected:
private: private:
int deviceId_; int deviceId_;
Type type_; Type type_;
StereoCameraModel stereoModel_;
libfreenect2::Freenect2 * freenect2_; libfreenect2::Freenect2 * freenect2_;
libfreenect2::Freenect2Device *dev_; libfreenect2::Freenect2Device *dev_;
libfreenect2::PacketPipeline * pipeline_;
libfreenect2::SyncMultiFrameListener * listener_; libfreenect2::SyncMultiFrameListener * listener_;
libfreenect2::Registration * reg_;
}; };
///////////////////////// /////////////////////////

8
corelib/include/rtabmap/core/util3d.h
View File

@@ -197,6 +197,14 @@ cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
float fx, float baseline, float fx, float baseline,
int type = CV_32FC1); int type = CV_32FC1);
cv::Mat RTABMAP_EXP registerDepth(
const cv::Mat & depth,
const cv::Mat & depthK,
const cv::Mat & colorK,
const rtabmap::Transform & transform);
void RTABMAP_EXP fillRegisteredDepthHoles(cv::Mat & depth, bool vertical, bool horizontal, bool fillDoubleHoles = false);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan); pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);

83
corelib/src/CameraModel.cpp
View File

@@ -56,7 +56,7 @@ CameraModel::CameraModel(const std::string & cameraName, const cv::Size & imageS
// init rectification map // init rectification map
UINFO("Initialize rectify map"); UINFO("Initialize rectify map");
cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_16SC2, rectificationMap1_, rectificationMap2_); cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
} }
bool CameraModel::load(const std::string & filePath) bool CameraModel::load(const std::string & filePath)
@@ -65,8 +65,8 @@ bool CameraModel::load(const std::string & filePath)
D_ = cv::Mat(); D_ = cv::Mat();
R_ = cv::Mat(); R_ = cv::Mat();
P_ = cv::Mat::zeros(3, 4, CV_64FC1); P_ = cv::Mat::zeros(3, 4, CV_64FC1);
rectificationMap1_ = cv::Mat(); mapX_ = cv::Mat();
rectificationMap2_ = cv::Mat(); mapY_ = cv::Mat();
if(UFile::exists(filePath)) if(UFile::exists(filePath))
{ {
@@ -121,7 +121,7 @@ bool CameraModel::load(const std::string & filePath)
// init rectification map // init rectification map
UINFO("Initialize rectify map"); UINFO("Initialize rectify map");
cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_16SC2, rectificationMap1_, rectificationMap2_); cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
return true; return true;
} }
@@ -172,17 +172,69 @@ bool CameraModel::save(const std::string & filePath)
return false; return false;
} }
cv::Mat CameraModel::rectifyImage(const cv::Mat & raw) const cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
{ {
if(!rectificationMap1_.empty() && !rectificationMap2_.empty()) if(!mapX_.empty() && !mapY_.empty())
{ {
cv::Mat rectified; cv::Mat rectified;
cv::remap(raw, rectified, rectificationMap1_, rectificationMap2_, cv::INTER_LINEAR); cv::remap(raw, rectified, mapX_, mapY_, interpolation);
return rectified; return rectified;
} }
else else
{ {
return raw; return raw.clone();
}
}
//inspired from https://github.com/code-iai/iai_kinect2/blob/master/depth_registration/src/depth_registration_cpu.cpp
cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
{
UASSERT(raw.type() == CV_16UC1);
if(!mapX_.empty() && !mapY_.empty())
{
cv::Mat rectified = cv::Mat::zeros(mapX_.rows, mapX_.cols, raw.type());
for(int y=0; y<mapX_.rows; ++y)
{
for(int x=0; x<mapX_.cols; ++x)
{
cv::Point2f pt(mapX_.at<float>(y,x), mapY_.at<float>(y,x));
int xL = (int)floor(pt.x);
int xH = (int)ceil(pt.x);
int yL = (int)floor(pt.y);
int yH = (int)ceil(pt.y);
if(xL >= 0 && yL >= 0 && xH < raw.cols && yH < raw.rows)
{
const uint16_t & pLT = raw.at<uint16_t>(yL, xL);
const uint16_t & pRT = raw.at<uint16_t>(yL, xH);
const uint16_t & pLB = raw.at<uint16_t>(yH, xL);
const uint16_t & pRB = raw.at<uint16_t>(yH, xH);
if(pLT > 0 && pRT > 0 && pLB > 0 && pRB > 0)
{
uint16_t avg = (pLT + pRT + pLB + pRB) / 4;
uint16_t thres = 0.01 * avg;
if( abs(pLT - avg) < thres &&
abs(pRT - avg) < thres &&
abs(pLB - avg) < thres &&
abs(pRB - avg) < thres)
{
//bilinear interpolation
float a = pt.x - (float)xL;
float c = pt.y - (float)yL;
//http://stackoverflow.com/questions/13299409/how-to-get-the-image-pixel-at-real-locations-in-opencv
rectified.at<unsigned short>(y,x) =
(raw.at<uint16_t>(yL, xL) * (1.f - a) + raw.at<uint16_t>(yL, xH) * a) * (1.f - c) +
(raw.at<uint16_t>(yH, xL) * (1.f - a) + raw.at<uint16_t>(yH, xH) * a) * c;
}
}
}
}
}
return rectified;
}
else
{
return raw.clone();
} }
} }
@@ -263,6 +315,7 @@ bool StereoCameraModel::save(const std::string & directory, const std::string &
// export in ROS calibration format // export in ROS calibration format
fs << "camera_name" << name_; fs << "camera_name" << name_;
fs << "rotation_matrix" << "{"; fs << "rotation_matrix" << "{";
fs << "rows" << R_.rows; fs << "rows" << R_.rows;
fs << "cols" << R_.cols; fs << "cols" << R_.cols;
@@ -275,14 +328,12 @@ bool StereoCameraModel::save(const std::string & directory, const std::string &
fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols)); fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
fs << "}"; fs << "}";
fs << "camera_name" << name_;
fs << "essential_matrix" << "{"; fs << "essential_matrix" << "{";
fs << "rows" << E_.rows; fs << "rows" << E_.rows;
fs << "cols" << E_.cols; fs << "cols" << E_.cols;
fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols)); fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
fs << "}"; fs << "}";
fs << "camera_name" << name_;
fs << "fundamental_matrix" << "{"; fs << "fundamental_matrix" << "{";
fs << "rows" << F_.rows; fs << "rows" << F_.rows;
fs << "cols" << F_.cols; fs << "cols" << F_.cols;
@@ -297,4 +348,16 @@ bool StereoCameraModel::save(const std::string & directory, const std::string &
return false; return false;
} }
Transform StereoCameraModel::transform() const
{
if(!R_.empty() && !T_.empty())
{
return Transform(
R_.at<double>(0,0), R_.at<double>(0,1), R_.at<double>(0,2), T_.at<double>(0),
R_.at<double>(1,0), R_.at<double>(1,1), R_.at<double>(1,2), T_.at<double>(1),
R_.at<double>(2,0), R_.at<double>(2,1), R_.at<double>(2,2), T_.at<double>(2));
}
return Transform();
}
} /* namespace rtabmap */ } /* namespace rtabmap */

267
corelib/src/CameraRGBD.cpp
View File

@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/DBDriver.h" #include "rtabmap/core/util3d.h"
#include <rtabmap/utilite/UEventsManager.h> #include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UConversion.h> #include <rtabmap/utilite/UConversion.h>
@@ -35,11 +35,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UFile.h> #include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UDirectory.h> #include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UTimer.h> #include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UMath.h>
#include <pcl/io/openni_grabber.h> #include <pcl/io/openni_grabber.h>
#include <cmath>
#ifdef WITH_FREENECT #ifdef WITH_FREENECT
#include <libfreenect.h> #include <libfreenect.h>
#ifdef FREENECT_DASH_INCLUDES #ifdef FREENECT_DASH_INCLUDES
@@ -52,6 +51,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifdef WITH_FREENECT2 #ifdef WITH_FREENECT2
#include <libfreenect2/libfreenect2.hpp> #include <libfreenect2/libfreenect2.hpp>
#include <libfreenect2/frame_listener_impl.h> #include <libfreenect2/frame_listener_impl.h>
#include <libfreenect2/registration.h>
#include <libfreenect2/packet_pipeline.h>
#endif #endif
#ifdef WITH_DC1394 #ifdef WITH_DC1394
@@ -89,7 +90,9 @@ CameraRGBD::~CameraRGBD()
void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{ {
float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity bool warnFrameRateTooHigh = false;
float actualFrameRate = 0;
float imageRate = _imageRate==0.0f?20.0f:_imageRate; // limit to 20Hz if infinity
if(imageRate>0) if(imageRate>0)
{ {
int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime()); int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
@@ -97,6 +100,11 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & f
{ {
uSleep(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 // 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)
@@ -120,7 +128,15 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & f
cx = float(rgb.cols) - cx; cx = float(rgb.cols) - cx;
} }
} }
UDEBUG("Time capturing image = %fs", timer.ticks()); if(warnFrameRateTooHigh)
{
UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
imageRate, actualFrameRate, timer.ticks());
}
else
{
UDEBUG("Time capturing image = %fs", timer.ticks());
}
} }
///////////////////////// /////////////////////////
@@ -1044,7 +1060,9 @@ CameraFreenect2::CameraFreenect2(int deviceId, Type type, float imageRate, const
type_(type), type_(type),
freenect2_(0), freenect2_(0),
dev_(0), dev_(0),
listener_(0) pipeline_(0),
listener_(0),
reg_(0)
{ {
#ifdef WITH_FREENECT2 #ifdef WITH_FREENECT2
freenect2_ = new libfreenect2::Freenect2(); freenect2_ = new libfreenect2::Freenect2();
@@ -1059,7 +1077,28 @@ CameraFreenect2::CameraFreenect2(int deviceId, Type type, float imageRate, const
listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth); listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
break; break;
} }
UWARN("CameraFreenect2: Images are not yet registered!");
#ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
pipeline_ = new libfreenect2::OpenGLPacketPipeline();
#else
#ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
pipeline_ = new libfreenect2::OpenCLPacketPipeline();
#else
pipeline_ = new libfreenect2::CpuPacketPipeline();
#endif
#endif
//default
//MinDepth(0.5f),
//MaxDepth(4.5f),
//EnableBilateralFilter(true),
//EnableEdgeAwareFilter(true)
libfreenect2::DepthPacketProcessor::Config config;
config.EnableBilateralFilter = true;
config.EnableEdgeAwareFilter = true;
config.MinDepth = 0.1;
config.MaxDepth = 12;
pipeline_->getDepthPacketProcessor()->setConfiguration(config);
#endif #endif
} }
@@ -1075,10 +1114,23 @@ CameraFreenect2::~CameraFreenect2()
{ {
delete listener_; delete listener_;
} }
if(reg_)
{
delete reg_;
reg_ = 0;
}
// commented, it seems released in freenect2_
//if(pipeline_)
//{
// delete pipeline_;
//}
if(freenect2_) if(freenect2_)
{ {
delete freenect2_; delete freenect2_;
} }
UDEBUG("");
#endif #endif
} }
@@ -1092,22 +1144,77 @@ bool CameraFreenect2::init(const std::string & calibrationFolder)
dev_ = 0; dev_ = 0;
} }
if(reg_)
{
delete reg_;
reg_ = 0;
}
if(deviceId_ <= 0) if(deviceId_ <= 0)
{ {
dev_ = freenect2_->openDefaultDevice(); dev_ = freenect2_->openDefaultDevice(pipeline_);
} }
else else
{ {
dev_ = freenect2_->openDevice(deviceId_); dev_ = freenect2_->openDevice(deviceId_, pipeline_);
} }
if(dev_) if(dev_)
{ {
dev_->setColorFrameListener(listener_); dev_->setColorFrameListener(listener_);
dev_->setIrAndDepthFrameListener(listener_); dev_->setIrAndDepthFrameListener(listener_);
dev_->start(); dev_->start();
UINFO("CameraFreenect2: device serial: %s", dev_->getSerialNumber().c_str()); UINFO("CameraFreenect2: device serial: %s", dev_->getSerialNumber().c_str());
UINFO("CameraFreenect2: device firmware: %s", dev_->getFirmwareVersion().c_str()); UINFO("CameraFreenect2: device firmware: %s", dev_->getFirmwareVersion().c_str());
//default registration params
libfreenect2::Freenect2Device::IrCameraParams depthParams = dev_->getIrCameraParams();
libfreenect2::Freenect2Device::ColorCameraParams colorParams = dev_->getColorCameraParams();
reg_ = new libfreenect2::Registration(&depthParams, &colorParams);
// look for calibration files
if(!calibrationFolder.empty())
{
if(!stereoModel_.load(calibrationFolder, dev_->getSerialNumber()))
{
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
dev_->getSerialNumber().c_str(), calibrationFolder.c_str());
}
else
{
// downscale color image by 2
cv::Mat colorP = stereoModel_.right().P();
cv::Size colorSize = stereoModel_.right().imageSize();
if(type_ == kTypeRGBDepthSD)
{
colorP.at<double>(0,0)/=2.0f; //fx
colorP.at<double>(1,1)/=2.0f; //fy
colorP.at<double>(0,2)/=2.0f; //cx
colorP.at<double>(1,2)/=2.0f; //cy
colorSize.width/=2;
colorSize.height/=2;
}
cv::Mat depthP = stereoModel_.left().P();
cv::Size depthSize = stereoModel_.left().imageSize();
float ratioY = float(colorSize.height)/float(depthSize.height);
float ratioX = float(colorSize.width)/float(depthSize.width);
depthP.at<double>(0,0)*=ratioX; //fx
depthP.at<double>(1,1)*=ratioY; //fy
depthP.at<double>(0,2)*=ratioX; //cx
depthP.at<double>(1,2)*=ratioY; //cy
depthSize.width*=ratioX;
depthSize.height*=ratioY;
const CameraModel & l = stereoModel_.left();
const CameraModel & r = stereoModel_.right();
stereoModel_ = StereoCameraModel(stereoModel_.name(),
depthSize, l.K(), l.D(), l.R(), depthP,
colorSize, r.K(), r.D(), r.R(), colorP,
stereoModel_.R(), stereoModel_.T(), stereoModel_.E(), stereoModel_.F());
}
}
return true; return true;
} }
else else
@@ -1122,8 +1229,7 @@ bool CameraFreenect2::init(const std::string & calibrationFolder)
bool CameraFreenect2::isCalibrated() const bool CameraFreenect2::isCalibrated() const
{ {
UWARN("Freenect2 calibration not yet implemented!"); return true;
return false;
} }
std::string CameraFreenect2::getSerial() const std::string CameraFreenect2::getSerial() const
@@ -1158,35 +1264,140 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
switch(type_) switch(type_)
{ {
case kTypeRGBIR: case kTypeRGBIR:
rgbFrame = frames[libfreenect2::Frame::Color]; rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
irFrame = frames[libfreenect2::Frame::Ir]; irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
break; break;
case kTypeRGBDepthSD: case kTypeRGBDepthSD:
case kTypeRGBDepthHD: case kTypeRGBDepthHD:
default: default:
rgbFrame = frames[libfreenect2::Frame::Color]; rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
depthFrame = frames[libfreenect2::Frame::Depth]; depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
break; break;
} }
cv::flip(cv::Mat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data), rgb, 1); cv::Mat rgbMat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data);
if(irFrame) cv::flip(rgbMat, rgb, 1);
if(stereoModel_.isValid())
{ {
cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1); //rectify color
rgb = stereoModel_.right().rectifyImage(rgb);
if(irFrame)
{
//rectify IR
cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(depth, depth, 1);
depth = stereoModel_.left().rectifyImage(depth);
}
else
{
//rectify depth
cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(depth, depth, 1);
depth = stereoModel_.left().rectifyDepth(depth);
bool registered = true;
if(registered)
{
depth = util3d::registerDepth(
depth,
stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K
stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K
stereoModel_.transform());
util3d::fillRegisteredDepthHoles(depth, true, false);
fx = stereoModel_.right().fx();
fy = stereoModel_.right().fy();
cx = stereoModel_.right().cx();
cy = stereoModel_.right().cy();
}
else
{
fx = stereoModel_.left().fx();
fy = stereoModel_.left().fy();
cx = stereoModel_.left().cx();
cy = stereoModel_.left().cy();
}
}
} }
else else
{ {
cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1); //use data from libfreenect2
if(irFrame)
{
cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
}
else
{
cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
//registration of the depth
if(reg_)
{
if(type_ == kTypeRGBDepthSD)
{
cv::Mat tmp;
cv::resize(rgb, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
rgb = tmp;
}
cv::Mat depthFrameMat = cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data);
depth = cv::Mat::zeros(rgb.rows, rgb.cols, CV_16U);
for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
{
for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
{
float dz = depthFrameMat.at<float>(dy,dx);
float dz1 = depthFrameMat.at<float>(dy,dx+1);
float dz2 = depthFrameMat.at<float>(dy+1,dx);
float dz3 = depthFrameMat.at<float>(dy+1,dx+1);
if(dz && dz1 && dz2 && dz3)
{
float avg = (dz + dz1 + dz2 + dz3) / 4;
float thres = 0.01 * avg;
if( fabs(dz - avg) < thres &&
fabs(dz1 - avg) < thres &&
fabs(dz2 - avg) < thres &&
fabs(dz3 - avg) < thres)
{
float cx=-1,cy=-1;
reg_->apply(dx, dy, dz, cx, cy);
if(type_==kTypeRGBDepthSD)
{
cx/=2.0f;
cy/=2.0f;
}
int rcx = cvRound(cx);
int rcy = cvRound(cy);
if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows))
{
unsigned short & zReg = depth.at<unsigned short>(rcy, rcx);
if(zReg == 0 || zReg > (unsigned short)dz)
{
zReg = (unsigned short)dz;
}
}
}
}
}
}
util3d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeRGBDepthHD);
util3d::fillRegisteredDepthHoles(depth, type_==kTypeRGBDepthSD, type_==kTypeRGBDepthHD);//second pass
libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
fx = params.fx*(type_==kTypeRGBDepthSD?0.5:1.0f);
fy = params.fy*(type_==kTypeRGBDepthSD?0.5:1.0f);
cx = params.cx*(type_==kTypeRGBDepthSD?0.5:1.0f);
cy = params.cy*(type_==kTypeRGBDepthSD?0.5:1.0f);
}
else
{
libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
}
}
cv::flip(depth, depth, 1);
} }
cv::flip(depth, depth, 1);
//libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
listener_->release(frames); listener_->release(frames);
} }
else else

14
corelib/src/Odometry.cpp
View File

@@ -225,6 +225,7 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
OdometryBOW::~OdometryBOW() OdometryBOW::~OdometryBOW()
{ {
delete _memory; delete _memory;
UDEBUG("");
} }
@@ -1146,8 +1147,8 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
// new 3D points, used to compute variance // new 3D points, used to compute variance
image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point); image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point);
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols-1)) && if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows-1))) uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
{ {
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y, pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true); data.cx(), data.cy(), data.fx(), data.fy(), true);
@@ -1283,8 +1284,8 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
for(unsigned int i=0; i<status.size(); ++i) for(unsigned int i=0; i<status.size(); ++i)
{ {
if(status[i] && pcl::isFinite(refCorners3D_->at(i)) && if(status[i] && pcl::isFinite(refCorners3D_->at(i)) &&
uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols-1)) && uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows-1))) uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
{ {
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y, pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true); data.cx(), data.cy(), data.fx(), data.fy(), true);
@@ -1405,8 +1406,8 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
int oi=0; int oi=0;
for(unsigned int i=0; i<newCorners.size(); ++i) for(unsigned int i=0; i<newCorners.size(); ++i)
{ {
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)-1.0f) && if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)-1.0f)) uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
{ {
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y, pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true); data.cx(), data.cy(), data.fx(), data.fy(), true);
@@ -1636,6 +1637,7 @@ OdometryThread::~OdometryThread()
{ {
delete _odometry; delete _odometry;
} }
UDEBUG("");
} }
void OdometryThread::handleEvent(UEvent * event) void OdometryThread::handleEvent(UEvent * event)

168
corelib/src/util3d.cpp
View File

@@ -1010,8 +1010,8 @@ pcl::PointXYZ projectDisparityTo3D(
int u = int(pt.x+0.5f); int u = int(pt.x+0.5f);
int v = int(pt.y+0.5f); int v = int(pt.y+0.5f);
float bad_point = std::numeric_limits<float>::quiet_NaN (); float bad_point = std::numeric_limits<float>::quiet_NaN ();
if(uIsInBounds(u, 0, disparity.cols-1) && if(uIsInBounds(u, 0, disparity.cols) &&
uIsInBounds(v, 0, disparity.rows-1)) uIsInBounds(v, 0, disparity.rows))
{ {
float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u); float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
return projectDisparityTo3D(pt, d, cx, cy, fx, baseline); return projectDisparityTo3D(pt, d, cx, cy, fx, baseline);
@@ -1049,6 +1049,170 @@ cv::Mat depthFromDisparity(const cv::Mat & disparity,
return depth; return depth;
} }
cv::Mat registerDepth(
const cv::Mat & depth,
const cv::Mat & depthK,
const cv::Mat & colorK,
const rtabmap::Transform & transform)
{
UASSERT(!transform.isNull());
UASSERT(!depth.empty());
UASSERT(depth.type() == CV_16UC1); // mm
UASSERT(depthK.type() == CV_64FC1 && depthK.cols == 3 && depthK.cols == 3);
UASSERT(colorK.type() == CV_64FC1 && colorK.cols == 3 && colorK.cols == 3);
float fx = depthK.at<double>(0,0);
float fy = depthK.at<double>(1,1);
float cx = depthK.at<double>(0,2);
float cy = depthK.at<double>(1,2);
float rfx = colorK.at<double>(0,0);
float rfy = colorK.at<double>(1,1);
float rcx = colorK.at<double>(0,2);
float rcy = colorK.at<double>(1,2);
Eigen::Affine3f proj = transform.toEigen3f();
Eigen::Vector4f P4,P3;
P4[3] = 1;
cv::Mat registered = cv::Mat::zeros(depth.rows, depth.cols, depth.type());
for(int y=0; y<depth.rows; ++y)
{
for(int x=0; x<depth.cols; ++x)
{
//filtering
float dz = float(depth.at<unsigned short>(y,x))*0.001f; // put in meter for projection
if(dz>=0.0f)
{
// Project to 3D
P4[0] = (x - cx) * dz / fx; // Optimization: we could have (x-cx)/fx in a lookup table
P4[1] = (y - cy) * dz / fy; // Optimization: we could have (y-cy)/fy in a lookup table
P4[2] = dz;
P3 = proj * P4;
float z = P3[2];
float invZ = 1.0f/z;
int dx = (rfx*P3[0])*invZ + rcx;
int dy = (rfy*P3[1])*invZ + rcy;
if(uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
{
unsigned short z16 = z * 1000; //mm
unsigned short &zReg = registered.at<unsigned short>(dy, dx);
if(zReg == 0 || z16 < zReg)
{
zReg = z16;
}
}
}
}
}
return registered;
}
void fillRegisteredDepthHoles(cv::Mat & registeredDepth, bool vertical, bool horizontal, bool fillDoubleHoles)
{
UASSERT(registeredDepth.type() == CV_16UC1);
int margin = fillDoubleHoles?2:1;
for(int x=1; x<registeredDepth.cols-margin; ++x)
{
for(int y=1; y<registeredDepth.rows-margin; ++y)
{
unsigned short & b = registeredDepth.at<unsigned short>(y, x);
bool set = false;
if(vertical)
{
const unsigned short & a = registeredDepth.at<unsigned short>(y-1, x);
unsigned short & c = registeredDepth.at<unsigned short>(y+1, x);
if(a && c)
{
unsigned short error = 0.01*((a+c)/2);
if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
(a>c?a-c<=error:c-a<=error))
{
b = (a+c)/2;
set = true;
if(!horizontal)
{
++y;
}
}
}
if(!set && fillDoubleHoles)
{
const unsigned short & d = registeredDepth.at<unsigned short>(y+2, x);
if(a && d && (b==0 || c==0))
{
unsigned short error = 0.01*((a+d)/2);
if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
((c == 0 && a && d) || (c > a+error && c > d+error)) &&
(a>d?a-d<=error:d-a<=error))
{
if(a>d)
{
unsigned short tmp = (a-d)/4;
b = d + tmp;
c = d + 3*tmp;
}
else
{
unsigned short tmp = (d-a)/4;
b = a + tmp;
c = a + 3*tmp;
}
set = true;
if(!horizontal)
{
y+=2;
}
}
}
}
}
if(!set && horizontal)
{
const unsigned short & a = registeredDepth.at<unsigned short>(y, x-1);
unsigned short & c = registeredDepth.at<unsigned short>(y, x+1);
if(a && c)
{
unsigned short error = 0.01*((a+c)/2);
if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
(a>c?a-c<=error:c-a<=error))
{
b = (a+c)/2;
set = true;
}
}
if(!set && fillDoubleHoles)
{
const unsigned short & d = registeredDepth.at<unsigned short>(y, x+2);
if(a && d && (b==0 || c==0))
{
unsigned short error = 0.01*((a+d)/2);
if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
((c == 0 && a && d) || (c > a+error && c > d+error)) &&
(a>d?a-d<=error:d-a<=error))
{
if(a>d)
{
unsigned short tmp = (a-d)/4;
b = d + tmp;
c = d + 3*tmp;
}
else
{
unsigned short tmp = (d-a)/4;
b = a + tmp;
c = a + 3*tmp;
}
}
}
}
}
}
}
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
{ {
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2); cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);

24
guilib/src/CalibrationDialog.cpp
View File

@@ -778,12 +778,24 @@ void CalibrationDialog::calibrate()
validRoi[1].x, validRoi[1].y, validRoi[1].width, validRoi[1].height, validRoi[1].x, validRoi[1].y, validRoi[1].width, validRoi[1].height,
imageSize.width, imageSize.height); imageSize.width, imageSize.height);
stereoModel_ = StereoCameraModel( if(imageSize_[0].width == imageSize_[1].width)
cameraName_.toStdString(), {
imageSize, //Stereo, keep new extrinsic projection matrix
models_[0].K(), models_[0].D(), R1, P1, stereoModel_ = StereoCameraModel(
models_[1].K(), models_[1].D(), R2, P2, cameraName_.toStdString(),
R, T, E, F); imageSize_[0], models_[0].K(), models_[0].D(), R1, P1,
imageSize_[1], models_[1].K(), models_[1].D(), R2, P2,
R, T, E, F);
}
else
{
//Kinect
stereoModel_ = StereoCameraModel(
cameraName_.toStdString(),
imageSize_[0], models_[0].K(), models_[0].D(), cv::Mat::eye(3,3,CV_64FC1), models_[0].P(),
imageSize_[1], models_[1].K(), models_[1].D(), cv::Mat::eye(3,3,CV_64FC1), models_[1].P(),
R, T, E, F);
}
std::stringstream strR1, strP1, strR2, strP2; std::stringstream strR1, strP1, strR2, strP2;
strR1 << stereoModel_.left().R(); strR1 << stereoModel_.left().R();

1
guilib/src/OdometryViewer.cpp
View File

@@ -123,6 +123,7 @@ OdometryViewer::~OdometryViewer()
{ {
this->unregisterFromEventsManager(); this->unregisterFromEventsManager();
this->clear(); this->clear();
UDEBUG("");
} }
void OdometryViewer::clear() void OdometryViewer::clear()

25
tools/CameraRGBD/main.cpp
View File

@@ -52,8 +52,8 @@ int main(int argc, char * argv[])
{ {
ULogger::setType(ULogger::kTypeConsole); ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo); ULogger::setLevel(ULogger::kInfo);
ULogger::setPrintTime(false); //ULogger::setPrintTime(false);
ULogger::setPrintWhere(false); //ULogger::setPrintWhere(false);
int driver = 0; int driver = 0;
if(argc < 2) if(argc < 2)
@@ -123,7 +123,7 @@ int main(int argc, char * argv[])
UERROR("Not built with Freenect2 support..."); UERROR("Not built with Freenect2 support...");
exit(-1); exit(-1);
} }
camera = new rtabmap::CameraFreenect2(); camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBDepthSD);
} }
else if(driver == 6) else if(driver == 6)
{ {
@@ -179,11 +179,6 @@ int main(int argc, char * argv[])
{ {
depth = rtabmap::util3d::cvtDepthFromFloat(depth); depth = rtabmap::util3d::cvtDepthFromFloat(depth);
} }
cv::Mat tmp;
depth.convertTo(tmp, CV_8UC1, 255.0/2048.0);
cv::imshow("Video", rgb); // show frame
cv::imshow("Depth", tmp);
if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy) if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy)
{ {
@@ -191,6 +186,20 @@ int main(int argc, char * argv[])
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, t); cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, t);
viewer.showCloud(cloud, "cloud"); viewer.showCloud(cloud, "cloud");
} }
else if(!depth.empty() && fx && fy)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(depth, cx, cy, fx, fy);
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(cloud, t);
viewer.showCloud(cloud, "cloud");
}
cv::Mat tmp;
unsigned short min=0, max = 2048;
uMinMax((unsigned short*)depth.data, depth.rows*depth.cols, min, max);
depth.convertTo(tmp, CV_8UC1, 255.0/max);
cv::imshow("Video", rgb); // show frame
cv::imshow("Depth", tmp);
} }
else else
{ {

4
tools/OdometryViewer/main.cpp
View File

@@ -700,7 +700,7 @@ int main (int argc, char * argv[])
app.exec(); app.exec();
camera.kill(); camera.join(true);
odomThread.join(true); odomThread.join(true);
} }
} }
@@ -793,7 +793,7 @@ int main (int argc, char * argv[])
odomViewer.exec(); odomViewer.exec();
cameraThread.kill(); cameraThread.join(true);
odomThread.join(true); odomThread.join(true);
} }
else else

2
utilite/include/rtabmap/utilite/UMath.h
View File

@@ -913,7 +913,7 @@ inline std::vector<float> uHamming(unsigned int L)
template <typename T> template <typename T>
bool uIsInBounds(const T& value, const T& low, const T& high) bool uIsInBounds(const T& value, const T& low, const T& high)
{ {
return !(value < low) && !(value > high); return !(value < low) && !(value >= high);
} }
#endif // UMATH_H #endif // UMATH_H