32 #include <opencv2/imgproc/types_c.h>
34 #ifdef RTABMAP_FREENECT2
35 #include <libfreenect2/libfreenect2.hpp>
36 #include <libfreenect2/frame_listener_impl.h>
37 #include <libfreenect2/registration.h>
38 #include <libfreenect2/packet_pipeline.h>
39 #include <libfreenect2/config.h>
47 #ifdef RTABMAP_FREENECT2
61 bool bilateralFiltering,
62 bool edgeAwareFiltering,
64 const std::string & pipelineName) :
65 Camera(imageRate, localTransform)
66 #ifdef RTABMAP_FREENECT2
74 minKinect2Depth_(minDepth),
75 maxKinect2Depth_(maxDepth),
76 bilateralFiltering_(bilateralFiltering),
77 edgeAwareFiltering_(edgeAwareFiltering),
78 noiseFiltering_(noiseFiltering),
79 pipelineName_(pipelineName)
82 #ifdef RTABMAP_FREENECT2
83 UASSERT(minKinect2Depth_ < maxKinect2Depth_ && minKinect2Depth_>0 && maxKinect2Depth_>0 && maxKinect2Depth_<=65.535
f);
84 freenect2_ =
new libfreenect2::Freenect2();
88 listener_ =
new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
91 listener_ =
new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
97 listener_ =
new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
105 #ifdef RTABMAP_FREENECT2
120 #ifdef RTABMAP_FREENECT2
121 libfreenect2::PacketPipeline *createPacketPipelineByName(
const std::string &
name)
123 std::string availablePipelines;
124 #if defined(LIBFREENECT2_WITH_OPENGL_SUPPORT)
125 availablePipelines +=
"gl ";
128 UINFO(
"Using 'gl' pipeline.");
129 return new libfreenect2::OpenGLPacketPipeline();
132 #if defined(LIBFREENECT2_WITH_CUDA_SUPPORT)
133 availablePipelines +=
"cuda cudakde ";
136 UINFO(
"Using 'cuda' pipeline.");
137 return new libfreenect2::CudaPacketPipeline();
139 if (
name ==
"cudakde")
141 UINFO(
"Using 'cudakde' pipeline.");
142 return new libfreenect2::CudaKdePacketPipeline();
145 #if defined(LIBFREENECT2_WITH_OPENCL_SUPPORT)
146 availablePipelines +=
"cl clkde ";
149 UINFO(
"Using 'cl' pipeline.");
150 return new libfreenect2::OpenCLPacketPipeline();
154 UINFO(
"Using 'clkde' pipeline.");
155 return new libfreenect2::OpenCLKdePacketPipeline();
158 availablePipelines +=
"cpu";
161 UINFO(
"Using 'cpu' pipeline.");
162 return new libfreenect2::CpuPacketPipeline();
167 UERROR(
"'%s' pipeline is not available. Available pipelines are: \"%s\". Default one is used instead (first one in the list).",
168 name.c_str(), availablePipelines.c_str());
172 #if defined(LIBFREENECT2_WITH_OPENGL_SUPPORT)
173 UINFO(
"Using 'gl' pipeline.");
174 return new libfreenect2::OpenGLPacketPipeline();
175 #elif defined(LIBFREENECT2_WITH_CUDA_SUPPORT)
176 UINFO(
"Using 'cuda' pipeline.");
177 return new libfreenect2::CudaPacketPipeline();
178 #elif defined(LIBFREENECT2_WITH_OPENCL_SUPPORT)
179 UINFO(
"Using 'cl' pipeline.");
180 return new libfreenect2::OpenCLPacketPipeline();
182 UINFO(
"Using 'cpu' pipeline.");
183 return new libfreenect2::CpuPacketPipeline();
190 #ifdef RTABMAP_FREENECT2
204 libfreenect2::PacketPipeline * pipeline = createPacketPipelineByName(pipelineName_);
208 UDEBUG(
"Opening default device...");
209 dev_ = freenect2_->openDefaultDevice(pipeline);
214 UDEBUG(
"Opening device ID=%d...", deviceId_);
215 dev_ = freenect2_->openDevice(deviceId_, pipeline);
226 libfreenect2::Freenect2Device::Config
config;
227 config.EnableBilateralFilter = bilateralFiltering_;
228 config.EnableEdgeAwareFilter = edgeAwareFiltering_;
229 config.MinDepth = minKinect2Depth_;
230 config.MaxDepth = maxKinect2Depth_;
231 dev_->setConfiguration(
config);
233 dev_->setColorFrameListener(listener_);
234 dev_->setIrAndDepthFrameListener(listener_);
238 UINFO(
"CameraFreenect2: device serial: %s", dev_->getSerialNumber().c_str());
239 UINFO(
"CameraFreenect2: device firmware: %s", dev_->getFirmwareVersion().c_str());
242 libfreenect2::Freenect2Device::IrCameraParams depthParams = dev_->getIrCameraParams();
243 libfreenect2::Freenect2Device::ColorCameraParams colorParams = dev_->getColorCameraParams();
244 reg_ =
new libfreenect2::Registration(depthParams, colorParams);
248 if(!calibrationFolder.empty())
250 std::string calibrationName = dev_->getSerialNumber();
251 if(!cameraName.empty())
253 calibrationName = cameraName;
255 stereoModel_.setName(calibrationName,
"depth",
"rgb");
256 if(!stereoModel_.load(calibrationFolder, calibrationName,
false))
258 UWARN(
"Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration "
259 "is used. Note that from version 0.11.10, calibration suffixes for Freenect2 driver have "
260 "changed from \"_left\"->\"_depth\" and \"_right\"->\"_rgb\". You can safely rename "
261 "the calibration files to avoid recalibrating.",
262 calibrationName.c_str(), calibrationFolder.c_str());
266 UINFO(
"Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
267 "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
271 UWARN(
"Freenect2: When using custom calibration file, type "
272 "kTypeColor2DepthSD is not supported. kTypeDepth2ColorSD is used instead...");
277 cv::Mat colorP = stereoModel_.right().P();
278 cv::Size colorSize = stereoModel_.right().imageSize();
281 colorP.at<
double>(0,0)/=2.0
f;
282 colorP.at<
double>(1,1)/=2.0
f;
283 colorP.at<
double>(0,2)/=2.0
f;
284 colorP.at<
double>(1,2)/=2.0
f;
288 cv::Mat depthP = stereoModel_.left().P();
289 cv::Size depthSize = stereoModel_.left().imageSize();
290 float ratioY =
float(colorSize.height)/
float(depthSize.height);
291 float ratioX =
float(colorSize.width)/
float(depthSize.width);
292 depthP.at<
double>(0,0)*=ratioX;
293 depthP.at<
double>(1,1)*=ratioY;
294 depthP.at<
double>(0,2)*=ratioX;
295 depthP.at<
double>(1,2)*=ratioY;
296 depthSize.width*=ratioX;
297 depthSize.height*=ratioY;
303 stereoModel_.R(), stereoModel_.T(), stereoModel_.E(), stereoModel_.F());
304 stereoModel_.initRectificationMap();
312 UERROR(
"CameraFreenect2: no device connected or failure opening the default one! Note that rtabmap should link on libusb of libfreenect2. "
313 "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
316 UERROR(
"CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
328 #ifdef RTABMAP_FREENECT2
331 return dev_->getSerialNumber();
340 #ifdef RTABMAP_FREENECT2
341 if(dev_ && listener_)
343 libfreenect2::FrameMap frames;
344 #ifndef LIBFREENECT2_THREADING_STDLIB
345 UDEBUG(
"Waiting for new frames... If it is stalled here, rtabmap should link on libusb of libfreenect2. "
346 "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
347 listener_->waitForNewFrame(frames);
349 if(!listener_->waitForNewFrame(frames, 1000))
351 UWARN(
"CameraFreenect2: Failed to get frames! rtabmap should link on libusb of libfreenect2. "
352 "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
358 libfreenect2::Frame *rgbFrame = 0;
359 libfreenect2::Frame *irFrame = 0;
360 libfreenect2::Frame *depthFrame = 0;
365 rgbFrame =
uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
366 irFrame =
uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
369 irFrame =
uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
370 depthFrame =
uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
377 rgbFrame =
uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
378 depthFrame =
uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
384 if(irFrame && depthFrame)
386 cv::Mat irMat((
int)irFrame->height, (
int)irFrame->width, CV_32FC1, irFrame->data);
388 float maxIr_ = 0x7FFF;
390 const float factor = 255.0f /
float((maxIr_ - minIr_));
391 rgb = cv::Mat(irMat.rows, irMat.cols, CV_8UC1);
392 for(
int i=0;
i<irMat.rows; ++
i)
394 for(
int j=0;
j<irMat.cols; ++
j)
396 rgb.at<
unsigned char>(
i,
j) = (
unsigned char)
std::min(
float(
std::max(irMat.at<
float>(
i,
j) - minIr_, 0.0f)) * factor, 255.0f);
400 cv::Mat((
int)depthFrame->height, (
int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
402 cv::flip(rgb, rgb, 1);
403 cv::flip(depth, depth, 1);
405 if(stereoModel_.isValidForRectification())
408 rgb = stereoModel_.left().rectifyImage(rgb);
409 depth = stereoModel_.left().rectifyDepth(depth);
410 fx = stereoModel_.left().fx();
411 fy = stereoModel_.left().fy();
412 cx = stereoModel_.left().cx();
413 cy = stereoModel_.left().cy();
417 libfreenect2::Freenect2Device::IrCameraParams
params = dev_->getIrCameraParams();
427 if(stereoModel_.isValidForRectification())
429 cv::Mat rgbMatC4((
int)rgbFrame->height, (
int)rgbFrame->width, CV_8UC4, rgbFrame->data);
431 #ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
433 cv::cvtColor(rgbMatC4, rgbMat, CV_RGBA2BGR);
437 cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
440 cv::flip(rgbMat, rgb, 1);
443 rgb = stereoModel_.right().rectifyImage(rgb);
447 cv::Mat((
int)irFrame->height, (
int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
448 cv::flip(depth, depth, 1);
449 depth = stereoModel_.left().rectifyImage(depth);
454 cv::Mat((
int)depthFrame->height, (
int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
455 cv::flip(depth, depth, 1);
458 depth = stereoModel_.left().rectifyDepth(depth);
460 bool registered =
true;
465 stereoModel_.left().P().colRange(0,3).rowRange(0,3),
467 stereoModel_.right().P().colRange(0,3).rowRange(0,3),
468 stereoModel_.stereoTransform());
470 fx = stereoModel_.right().fx();
471 fy = stereoModel_.right().fy();
472 cx = stereoModel_.right().cx();
473 cy = stereoModel_.right().cy();
477 fx = stereoModel_.left().fx();
478 fy = stereoModel_.left().fy();
479 cx = stereoModel_.left().cx();
480 cy = stereoModel_.left().cy();
489 cv::Mat rgbMatC4((
int)rgbFrame->height, (
int)rgbFrame->width, CV_8UC4, rgbFrame->data);
491 #ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
493 cv::cvtColor(rgbMatC4, rgbMat, CV_RGB2BGR);
497 cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
500 cv::flip(rgbMat, rgb, 1);
502 cv::Mat((
int)irFrame->height, (
int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
503 cv::flip(depth, depth, 1);
510 float maxDepth = maxKinect2Depth_*1000.0f;
511 float minDepth = minKinect2Depth_*1000.0f;
515 libfreenect2::Frame depthUndistorted(512, 424, 4);
516 libfreenect2::Frame rgbRegistered(512, 424, 4);
521 cv::Mat depthMat = cv::Mat((
int)depthFrame->height, (
int)depthFrame->width, CV_32FC1, depthFrame->data);
522 for(
int dx=0; dx<depthMat.cols; ++dx)
524 bool onEdgeX = dx==depthMat.cols-1;
525 for(
int dy=0; dy<depthMat.rows; ++dy)
527 bool onEdge = onEdgeX || dy==depthMat.rows-1;
529 float & dz = depthMat.at<
float>(dy,dx);
530 if(dz>=minDepth && dz <= maxDepth)
533 if(noiseFiltering_ && !onEdge)
536 const float & dz1 = depthMat.at<
float>(dy,dx+1);
537 const float & dz2 = depthMat.at<
float>(dy+1,dx);
538 const float & dz3 = depthMat.at<
float>(dy+1,dx+1);
539 if( dz1>=minDepth && dz1 <= maxDepth &&
540 dz2>=minDepth && dz2 <= maxDepth &&
541 dz3>=minDepth && dz3 <= maxDepth)
543 float avg = (dz + dz1 + dz2 + dz3) / 4.0
f;
544 float thres = 0.01f*avg;
546 if(
fabs(dz-avg) < thres &&
547 fabs(dz1-avg) < thres &&
548 fabs(dz2-avg) < thres &&
549 fabs(dz3-avg) < thres)
561 libfreenect2::Frame bidDepth(1920, 1082, 4);
562 reg_->apply(rgbFrame, depthFrame, &depthUndistorted, &rgbRegistered,
true, &bidDepth);
567 rgbMatBGRA = cv::Mat((
int)rgbRegistered.height, (
int)rgbRegistered.width, CV_8UC4, rgbRegistered.data);
568 depthMat = cv::Mat((
int)depthUndistorted.height, (
int)depthUndistorted.width, CV_32FC1, depthUndistorted.data);
571 libfreenect2::Freenect2Device::IrCameraParams
params = dev_->getIrCameraParams();
579 rgbMatBGRA = cv::Mat((
int)rgbFrame->height, (
int)rgbFrame->width, CV_8UC4, rgbFrame->data);
580 depthMat = cv::Mat((
int)bidDepth.height, (
int)bidDepth.width, CV_32FC1, bidDepth.data);
584 libfreenect2::Freenect2Device::ColorCameraParams
params = dev_->getColorCameraParams();
592 depth = cv::Mat(depthMat.size(), CV_16UC1);
593 for(
int dx=0; dx<depthMat.cols; ++dx)
595 for(
int dy=0; dy<depthMat.rows; ++dy)
597 unsigned short z = 0;
598 const float & dz = depthMat.at<
float>(dy,dx);
599 if(dz>=minDepth && dz <= maxDepth)
601 z = (
unsigned short)dz;
603 depth.at<
unsigned short>(dy,(depthMat.cols-1)-dx) =
z;
608 #ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
610 cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR);
614 cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
617 cv::flip(rgb, rgb, 1);
622 cv::Mat rgbMatBGRA = cv::Mat((
int)rgbFrame->height, (
int)rgbFrame->width, CV_8UC4, rgbFrame->data);
626 cv::resize(rgbMatBGRA, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
630 #ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
632 cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR);
636 cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
639 cv::flip(rgb, rgb, 1);
641 cv::Mat depthFrameMat = cv::Mat((
int)depthFrame->height, (
int)depthFrame->width, CV_32FC1, depthFrame->data);
642 depth = cv::Mat::zeros(rgbMatBGRA.rows, rgbMatBGRA.cols, CV_16U);
643 for(
int dx=0; dx<depthFrameMat.cols-1; ++dx)
645 for(
int dy=0; dy<depthFrameMat.rows-1; ++dy)
647 float dz = depthFrameMat.at<
float>(dy,dx);
648 if(dz>=minDepth && dz<=maxDepth)
650 bool goodDepth =
true;
654 float dz1 = depthFrameMat.at<
float>(dy,dx+1);
655 float dz2 = depthFrameMat.at<
float>(dy+1,dx);
656 float dz3 = depthFrameMat.at<
float>(dy+1,dx+1);
657 if(dz1>=minDepth && dz1 <= maxDepth &&
658 dz2>=minDepth && dz2 <= maxDepth &&
659 dz3>=minDepth && dz3 <= maxDepth)
661 float avg = (dz + dz1 + dz2 + dz3) / 4.0
f;
662 float thres = 0.01 * avg;
663 if(
fabs(dz-avg) < thres &&
664 fabs(dz1-avg) < thres &&
665 fabs(dz2-avg) < thres &&
666 fabs(dz3-avg) < thres)
675 reg_->apply(dx, dy, dz,
cx,
cy);
681 int rcx = cvRound(
cx);
682 int rcy = cvRound(
cy);
685 unsigned short & zReg = depth.at<
unsigned short>(rcy, rcx);
686 if(zReg == 0 || zReg > (
unsigned short)dz)
688 zReg = (
unsigned short)dz;
697 cv::flip(depth, depth, 1);
698 libfreenect2::Freenect2Device::ColorCameraParams
params = dev_->getColorCameraParams();
722 listener_->release(frames);
726 UERROR(
"CameraFreenect2: RTAB-Map is not built with Freenect2 support!");