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

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 "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/Transform.h"
namespace rtabmap {
@@ -61,10 +62,10 @@ public:
double cy() const {return P_.at<double>(1,2);}
double Tx() const {return P_.at<double>(0,3);}
const cv::Mat & K() const {return K_;}
const cv::Mat & D() const {return D_;}
const cv::Mat & R() const {return R_;}
const cv::Mat & P() const {return P_;}
const cv::Mat & K() const {return K_;} //intrinsic camera matrix
const cv::Mat & D() const {return D_;} //intrinsic distorsion matrix
const cv::Mat & R() const {return R_;} //rectification matrix
const cv::Mat & P() const {return P_;} //projection matrix
const cv::Size & imageSize() const {return imageSize_;}
int imageWidth() const {return imageSize_.width;}
@@ -73,7 +74,9 @@ public:
bool load(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:
std::string name_;
@@ -82,20 +85,22 @@ private:
cv::Mat D_;
cv::Mat R_;
cv::Mat P_;
cv::Mat rectificationMap1_;
cv::Mat rectificationMap2_;
cv::Mat mapX_;
cv::Mat mapY_;
};
class StereoCameraModel
{
public:
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::Size & imageSize2,
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) :
left_(name+"_left", imageSize, K1, D1, R1, P1),
right_(name+"_right", imageSize, K2, D2, R2, P2),
left_(name+"_left", imageSize1, K1, D1, R1, P1),
right_(name+"_right", imageSize2, K2, D2, R2, P2),
name_(name),
R_(R),
T_(T),
@@ -110,8 +115,16 @@ public:
bool load(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();}
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 & right() const {return right_;}

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

@@ -63,6 +63,8 @@ namespace libfreenect2
class Freenect2;
class Freenect2Device;
class SyncMultiFrameListener;
class Registration;
class PacketPipeline;
}
namespace FlyCapture2
@@ -294,9 +296,12 @@ protected:
private:
int deviceId_;
Type type_;
StereoCameraModel stereoModel_;
libfreenect2::Freenect2 * freenect2_;
libfreenect2::Freenect2Device *dev_;
libfreenect2::PacketPipeline * pipeline_;
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,
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);
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
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)
@@ -65,8 +65,8 @@ bool CameraModel::load(const std::string & filePath)
D_ = cv::Mat();
R_ = cv::Mat();
P_ = cv::Mat::zeros(3, 4, CV_64FC1);
rectificationMap1_ = cv::Mat();
rectificationMap2_ = cv::Mat();
mapX_ = cv::Mat();
mapY_ = cv::Mat();
if(UFile::exists(filePath))
{
@@ -121,7 +121,7 @@ bool CameraModel::load(const std::string & filePath)
// init rectification 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;
}
@@ -172,17 +172,69 @@ bool CameraModel::save(const std::string & filePath)
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::remap(raw, rectified, rectificationMap1_, rectificationMap2_, cv::INTER_LINEAR);
cv::remap(raw, rectified, mapX_, mapY_, interpolation);
return rectified;
}
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
fs << "camera_name" << name_;
fs << "rotation_matrix" << "{";
fs << "rows" << R_.rows;
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 << "}";
fs << "camera_name" << name_;
fs << "essential_matrix" << "{";
fs << "rows" << E_.rows;
fs << "cols" << E_.cols;
fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
fs << "}";
fs << "camera_name" << name_;
fs << "fundamental_matrix" << "{";
fs << "rows" << F_.rows;
fs << "cols" << F_.cols;
@@ -297,4 +348,16 @@ bool StereoCameraModel::save(const std::string & directory, const std::string &
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 */

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/DBDriver.h"
#include "rtabmap/core/util3d.h"
#include <rtabmap/utilite/UEventsManager.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/UDirectory.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UMath.h>
#include <pcl/io/openni_grabber.h>
#include <cmath>
#ifdef WITH_FREENECT
#include <libfreenect.h>
#ifdef FREENECT_DASH_INCLUDES
@@ -52,6 +51,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifdef WITH_FREENECT2
#include <libfreenect2/libfreenect2.hpp>
#include <libfreenect2/frame_listener_impl.h>
#include <libfreenect2/registration.h>
#include <libfreenect2/packet_pipeline.h>
#endif
#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)
{
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)
{
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);
}
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)
@@ -120,7 +128,15 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & f
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),
freenect2_(0),
dev_(0),
listener_(0)
pipeline_(0),
listener_(0),
reg_(0)
{
#ifdef WITH_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);
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
}
@@ -1075,10 +1114,23 @@ CameraFreenect2::~CameraFreenect2()
{
delete listener_;
}
if(reg_)
{
delete reg_;
reg_ = 0;
}
// commented, it seems released in freenect2_
//if(pipeline_)
//{
// delete pipeline_;
//}
if(freenect2_)
{
delete freenect2_;
}
UDEBUG("");
#endif
}
@@ -1092,22 +1144,77 @@ bool CameraFreenect2::init(const std::string & calibrationFolder)
dev_ = 0;
}
if(reg_)
{
delete reg_;
reg_ = 0;
}
if(deviceId_ <= 0)
{
dev_ = freenect2_->openDefaultDevice();
dev_ = freenect2_->openDefaultDevice(pipeline_);
}
else
{
dev_ = freenect2_->openDevice(deviceId_);
dev_ = freenect2_->openDevice(deviceId_, pipeline_);
}
if(dev_)
{
dev_->setColorFrameListener(listener_);
dev_->setIrAndDepthFrameListener(listener_);
dev_->start();
UINFO("CameraFreenect2: device serial: %s", dev_->getSerialNumber().c_str());
UINFO("CameraFreenect2: device firmware: %s", dev_->getFirmwareVersion().c_str());
//default registration params
libfreenect2::Freenect2Device::IrCameraParams depthParams = dev_->getIrCameraParams();
libfreenect2::Freenect2Device::ColorCameraParams colorParams = dev_->getColorCameraParams();
reg_ = new libfreenect2::Registration(&depthParams, &colorParams);
// look for calibration files
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;
}
else
@@ -1122,8 +1229,7 @@ bool CameraFreenect2::init(const std::string & calibrationFolder)
bool CameraFreenect2::isCalibrated() const
{
UWARN("Freenect2 calibration not yet implemented!");
return false;
return true;
}
std::string CameraFreenect2::getSerial() const
@@ -1158,35 +1264,140 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
switch(type_)
{
case kTypeRGBIR:
rgbFrame = frames[libfreenect2::Frame::Color];
irFrame = frames[libfreenect2::Frame::Ir];
rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
break;
case kTypeRGBDepthSD:
case kTypeRGBDepthHD:
default:
rgbFrame = frames[libfreenect2::Frame::Color];
depthFrame = frames[libfreenect2::Frame::Depth];
rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
break;
}
cv::flip(cv::Mat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data), rgb, 1);
if(irFrame)
cv::Mat rgbMat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data);
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
{
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);
}
else

14
corelib/src/Odometry.cpp
View File

@@ -225,6 +225,7 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
OdometryBOW::~OdometryBOW()
{
delete _memory;
UDEBUG("");
}
@@ -1146,8 +1147,8 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
// new 3D points, used to compute variance
image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point);
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols-1)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows-1)))
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
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)
{
if(status[i] && pcl::isFinite(refCorners3D_->at(i)) &&
uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols-1)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows-1)))
uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
@@ -1405,8 +1406,8 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
int oi=0;
for(unsigned int i=0; i<newCorners.size(); ++i)
{
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)-1.0f) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)-1.0f))
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
@@ -1636,6 +1637,7 @@ OdometryThread::~OdometryThread()
{
delete _odometry;
}
UDEBUG("");
}
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 v = int(pt.y+0.5f);
float bad_point = std::numeric_limits<float>::quiet_NaN ();
if(uIsInBounds(u, 0, disparity.cols-1) &&
uIsInBounds(v, 0, disparity.rows-1))
if(uIsInBounds(u, 0, disparity.cols) &&
uIsInBounds(v, 0, disparity.rows))
{
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);
@@ -1049,6 +1049,170 @@ cv::Mat depthFromDisparity(const cv::Mat & disparity,
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 laserScan(1, (int)cloud.size(), CV_32FC2);