00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/core/CameraRGBD.h"
00029 #include "rtabmap/core/util2d.h"
00030 #include "rtabmap/core/CameraRGB.h"
00031 #include "rtabmap/core/Graph.h"
00032 #include "rtabmap/core/Version.h"
00033
00034 #include <rtabmap/utilite/UEventsManager.h>
00035 #include <rtabmap/utilite/UConversion.h>
00036 #include <rtabmap/utilite/UStl.h>
00037 #include <rtabmap/utilite/UConversion.h>
00038 #include <rtabmap/utilite/UFile.h>
00039 #include <rtabmap/utilite/UDirectory.h>
00040 #include <rtabmap/utilite/UTimer.h>
00041 #include <rtabmap/utilite/UMath.h>
00042
00043 #ifdef HAVE_OPENNI
00044 #include <pcl/io/openni_grabber.h>
00045 #include <pcl/io/oni_grabber.h>
00046 #include <pcl/io/openni_camera/openni_depth_image.h>
00047 #include <pcl/io/openni_camera/openni_image.h>
00048 #endif
00049
00050 #ifdef RTABMAP_FREENECT
00051 #include <libfreenect.h>
00052 #ifdef FREENECT_DASH_INCLUDES
00053 #include <libfreenect-registration.h>
00054 #else
00055 #include <libfreenect_registration.h>
00056 #endif
00057 #endif
00058
00059 #ifdef RTABMAP_FREENECT2
00060 #include <libfreenect2/libfreenect2.hpp>
00061 #include <libfreenect2/frame_listener_impl.h>
00062 #include <libfreenect2/registration.h>
00063 #include <libfreenect2/packet_pipeline.h>
00064 #include <libfreenect2/config.h>
00065 #endif
00066
00067 #ifdef RTABMAP_OPENNI2
00068 #include <OniVersion.h>
00069 #include <OpenNI.h>
00070 #endif
00071
00072 namespace rtabmap
00073 {
00074
00076
00078 CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform) :
00079 Camera(imageRate, localTransform),
00080 interface_(0),
00081 deviceId_(deviceId),
00082 depthConstant_(0.0f)
00083 {
00084 }
00085
00086 bool CameraOpenni::available()
00087 {
00088 #ifdef HAVE_OPENNI
00089 return true;
00090 #else
00091 return false;
00092 #endif
00093 }
00094
00095 CameraOpenni::~CameraOpenni()
00096 {
00097 #ifdef HAVE_OPENNI
00098 UDEBUG("");
00099 if(connection_.connected())
00100 {
00101 connection_.disconnect();
00102 }
00103
00104 if(interface_)
00105 {
00106 interface_->stop();
00107 uSleep(1000);
00108 delete interface_;
00109 interface_ = 0;
00110 }
00111 #endif
00112 }
00113 #ifdef HAVE_OPENNI
00114 void CameraOpenni::image_cb (
00115 const boost::shared_ptr<openni_wrapper::Image>& rgb,
00116 const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
00117 float constant)
00118 {
00119 UScopeMutex s(dataMutex_);
00120
00121 bool notify = rgb_.empty();
00122
00123 cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
00124 rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
00125 cv::cvtColor(rgbFrame, rgb_, CV_RGB2BGR);
00126
00127 depth_ = cv::Mat(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
00128 depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depth_.data);
00129
00130 depthConstant_ = constant;
00131
00132 if(notify)
00133 {
00134 dataReady_.release();
00135 }
00136 }
00137 #endif
00138
00139 bool CameraOpenni::init(const std::string & calibrationFolder, const std::string & cameraName)
00140 {
00141 #ifdef HAVE_OPENNI
00142 if(interface_)
00143 {
00144 interface_->stop();
00145 uSleep(100);
00146 delete interface_;
00147 interface_ = 0;
00148 }
00149
00150 try
00151 {
00152 if(UFile::getExtension(deviceId_).compare("oni") == 0)
00153 {
00154 interface_ = new pcl::ONIGrabber(deviceId_, false, true);
00155 }
00156 else
00157 {
00158 interface_ = new pcl::OpenNIGrabber(deviceId_);
00159 }
00160
00161 boost::function<void (
00162 const boost::shared_ptr<openni_wrapper::Image>&,
00163 const boost::shared_ptr<openni_wrapper::DepthImage>&,
00164 float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
00165 connection_ = interface_->registerCallback (f);
00166
00167 interface_->start ();
00168 }
00169 catch(const pcl::IOException& ex)
00170 {
00171 UERROR("OpenNI exception: %s", ex.what());
00172 if(interface_)
00173 {
00174 delete interface_;
00175 interface_ = 0;
00176 }
00177 return false;
00178 }
00179 return true;
00180 #else
00181 UERROR("PCL not built with OpenNI! Cannot initialize CameraOpenNI");
00182 return false;
00183 #endif
00184 }
00185
00186 bool CameraOpenni::isCalibrated() const
00187 {
00188 #ifdef HAVE_OPENNI
00189 return true;
00190 #else
00191 return false;
00192 #endif
00193 }
00194
00195 std::string CameraOpenni::getSerial() const
00196 {
00197 #ifdef HAVE_OPENNI
00198 if(interface_)
00199 {
00200 return interface_->getName();
00201 }
00202 #endif
00203 return "";
00204 }
00205
00206 SensorData CameraOpenni::captureImage(CameraInfo * info)
00207 {
00208 SensorData data;
00209 #ifdef HAVE_OPENNI
00210 if(interface_ && interface_->isRunning())
00211 {
00212 if(!dataReady_.acquire(1, 2000))
00213 {
00214 UWARN("Not received new frames since 2 seconds, end of stream reached!");
00215 }
00216 else
00217 {
00218 UScopeMutex s(dataMutex_);
00219 if(depthConstant_ && !rgb_.empty() && !depth_.empty())
00220 {
00221 CameraModel model(
00222 1.0f/depthConstant_,
00223 1.0f/depthConstant_,
00224 float(rgb_.cols/2) - 0.5f,
00225 float(rgb_.rows/2) - 0.5f,
00226 this->getLocalTransform(),
00227 0,
00228 rgb_.size());
00229 data = SensorData(rgb_, depth_, model, this->getNextSeqID(), UTimer::now());
00230 }
00231
00232 depth_ = cv::Mat();
00233 rgb_ = cv::Mat();
00234 depthConstant_ = 0.0f;
00235 }
00236 }
00237 #else
00238 UERROR("CameraOpenNI: RTAB-Map is not built with PCL having OpenNI support!");
00239 #endif
00240 return data;
00241 }
00242
00243
00244
00246
00248 bool CameraOpenNICV::available()
00249 {
00250 return cv::getBuildInformation().find("OpenNI: YES") != std::string::npos;
00251 }
00252
00253 CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform) :
00254 Camera(imageRate, localTransform),
00255 _asus(asus),
00256 _depthFocal(0.0f)
00257 {
00258
00259 }
00260
00261 CameraOpenNICV::~CameraOpenNICV()
00262 {
00263 _capture.release();
00264 }
00265
00266 bool CameraOpenNICV::init(const std::string & calibrationFolder, const std::string & cameraName)
00267 {
00268 if(_capture.isOpened())
00269 {
00270 _capture.release();
00271 }
00272
00273 ULOGGER_DEBUG("Camera::init()");
00274 _capture.open( _asus?CV_CAP_OPENNI_ASUS:CV_CAP_OPENNI );
00275 if(_capture.isOpened())
00276 {
00277 _capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ );
00278 _depthFocal = _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH );
00279
00280 UINFO("Depth generator output mode:");
00281 UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
00282 UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
00283 UINFO("FRAME_MAX_DEPTH %f mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
00284 UINFO("BASELINE %f mm", _capture.get( CV_CAP_PROP_OPENNI_BASELINE ));
00285 UINFO("FPS %f", _capture.get( CV_CAP_PROP_FPS ));
00286 UINFO("Focal %f", _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH ));
00287 UINFO("REGISTRATION %f", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
00288 if(_capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) == 0.0)
00289 {
00290 UERROR("Depth registration is not activated on this device!");
00291 }
00292 if( _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
00293 {
00294 UINFO("Image generator output mode:");
00295 UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ));
00296 UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ));
00297 UINFO("FPS %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FPS ));
00298 }
00299 else
00300 {
00301 UERROR("Camera: Device doesn't contain image generator.");
00302 _capture.release();
00303 return false;
00304 }
00305 }
00306 else
00307 {
00308 ULOGGER_ERROR("Camera: Failed to create a capture object!");
00309 _capture.release();
00310 return false;
00311 }
00312 return true;
00313 }
00314
00315 bool CameraOpenNICV::isCalibrated() const
00316 {
00317 return true;
00318 }
00319
00320 SensorData CameraOpenNICV::captureImage(CameraInfo * info)
00321 {
00322 SensorData data;
00323 if(_capture.isOpened())
00324 {
00325 _capture.grab();
00326 cv::Mat depth, rgb;
00327 _capture.retrieve(depth, CV_CAP_OPENNI_DEPTH_MAP );
00328 _capture.retrieve(rgb, CV_CAP_OPENNI_BGR_IMAGE );
00329
00330 depth = depth.clone();
00331 rgb = rgb.clone();
00332
00333 UASSERT(_depthFocal>0.0f);
00334 if(!rgb.empty() && !depth.empty())
00335 {
00336 CameraModel model(
00337 _depthFocal,
00338 _depthFocal,
00339 float(rgb.cols/2) - 0.5f,
00340 float(rgb.rows/2) - 0.5f,
00341 this->getLocalTransform(),
00342 0,
00343 rgb.size());
00344 data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
00345 }
00346 }
00347 else
00348 {
00349 ULOGGER_WARN("The camera must be initialized before requesting an image.");
00350 }
00351 return data;
00352 }
00353
00354
00356
00358 bool CameraOpenNI2::available()
00359 {
00360 #ifdef RTABMAP_OPENNI2
00361 return true;
00362 #else
00363 return false;
00364 #endif
00365 }
00366
00367 bool CameraOpenNI2::exposureGainAvailable()
00368 {
00369 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
00370 return true;
00371 #else
00372 return false;
00373 #endif
00374 }
00375
00376 CameraOpenNI2::CameraOpenNI2(
00377 const std::string & deviceId,
00378 float imageRate,
00379 const rtabmap::Transform & localTransform) :
00380 Camera(imageRate, localTransform),
00381 #ifdef RTABMAP_OPENNI2
00382 _device(new openni::Device()),
00383 _color(new openni::VideoStream()),
00384 _depth(new openni::VideoStream()),
00385 #else
00386 _device(0),
00387 _color(0),
00388 _depth(0),
00389 #endif
00390 _depthFx(0.0f),
00391 _depthFy(0.0f),
00392 _deviceId(deviceId),
00393 _openNI2StampsAndIDsUsed(false)
00394 {
00395 }
00396
00397 CameraOpenNI2::~CameraOpenNI2()
00398 {
00399 #ifdef RTABMAP_OPENNI2
00400 _color->stop();
00401 _color->destroy();
00402 _depth->stop();
00403 _depth->destroy();
00404 _device->close();
00405 openni::OpenNI::shutdown();
00406
00407 delete _device;
00408 delete _color;
00409 delete _depth;
00410 #endif
00411 }
00412
00413 bool CameraOpenNI2::setAutoWhiteBalance(bool enabled)
00414 {
00415 #ifdef RTABMAP_OPENNI2
00416 if(_color && _color->getCameraSettings())
00417 {
00418 return _color->getCameraSettings()->setAutoWhiteBalanceEnabled(enabled) == openni::STATUS_OK;
00419 }
00420 #else
00421 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00422 #endif
00423 return false;
00424 }
00425
00426 bool CameraOpenNI2::setAutoExposure(bool enabled)
00427 {
00428 #ifdef RTABMAP_OPENNI2
00429 if(_color && _color->getCameraSettings())
00430 {
00431 return _color->getCameraSettings()->setAutoExposureEnabled(enabled) == openni::STATUS_OK;
00432 }
00433 #else
00434 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00435 #endif
00436 return false;
00437 }
00438
00439 bool CameraOpenNI2::setExposure(int value)
00440 {
00441 #ifdef RTABMAP_OPENNI2
00442 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
00443 if(_color && _color->getCameraSettings())
00444 {
00445 return _color->getCameraSettings()->setExposure(value) == openni::STATUS_OK;
00446 }
00447 #else
00448 UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
00449 #endif
00450 #else
00451 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00452 #endif
00453 return false;
00454 }
00455
00456 bool CameraOpenNI2::setGain(int value)
00457 {
00458 #ifdef RTABMAP_OPENNI2
00459 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
00460 if(_color && _color->getCameraSettings())
00461 {
00462 return _color->getCameraSettings()->setGain(value) == openni::STATUS_OK;
00463 }
00464 #else
00465 UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
00466 #endif
00467 #else
00468 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00469 #endif
00470 return false;
00471 }
00472
00473 bool CameraOpenNI2::setMirroring(bool enabled)
00474 {
00475 #ifdef RTABMAP_OPENNI2
00476 if(_color->isValid() && _depth->isValid())
00477 {
00478 return _depth->setMirroringEnabled(enabled) == openni::STATUS_OK &&
00479 _color->setMirroringEnabled(enabled) == openni::STATUS_OK;
00480 }
00481 #endif
00482 return false;
00483 }
00484
00485 bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::string & cameraName)
00486 {
00487 #ifdef RTABMAP_OPENNI2
00488 openni::OpenNI::initialize();
00489
00490 if(_device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str()) != openni::STATUS_OK)
00491 {
00492 if(!_deviceId.empty())
00493 {
00494 UERROR("CameraOpenNI2: Cannot open device \"%s\".", _deviceId.c_str());
00495 }
00496 else
00497 {
00498 UERROR("CameraOpenNI2: Cannot open device.");
00499 }
00500 _device->close();
00501 openni::OpenNI::shutdown();
00502 return false;
00503 }
00504
00505 if(UFile::getExtension(_deviceId).compare("oni")==0)
00506 {
00507 if(_device->getPlaybackControl() &&
00508 _device->getPlaybackControl()->setRepeatEnabled(false) != openni::STATUS_OK)
00509 {
00510 UERROR("CameraOpenNI2: Cannot set repeat mode to false.");
00511 _device->close();
00512 openni::OpenNI::shutdown();
00513 return false;
00514 }
00515 }
00516 else if(!_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
00517 {
00518 UERROR("CameraOpenNI2: Device doesn't support depth/color registration.");
00519 _device->close();
00520 openni::OpenNI::shutdown();
00521 return false;
00522 }
00523
00524 if(_device->getSensorInfo(openni::SENSOR_DEPTH) == NULL ||
00525 _device->getSensorInfo(openni::SENSOR_COLOR) == NULL)
00526 {
00527 UERROR("CameraOpenNI2: Cannot get sensor info for depth and color.");
00528 _device->close();
00529 openni::OpenNI::shutdown();
00530 return false;
00531 }
00532
00533 if(_depth->create(*_device, openni::SENSOR_DEPTH) != openni::STATUS_OK)
00534 {
00535 UERROR("CameraOpenNI2: Cannot create depth stream.");
00536 _device->close();
00537 openni::OpenNI::shutdown();
00538 return false;
00539 }
00540
00541 if(_color->create(*_device, openni::SENSOR_COLOR) != openni::STATUS_OK)
00542 {
00543 UERROR("CameraOpenNI2: Cannot create color stream.");
00544 _depth->destroy();
00545 _device->close();
00546 openni::OpenNI::shutdown();
00547 return false;
00548 }
00549
00550 if(_device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
00551 {
00552 UERROR("CameraOpenNI2: Failed to set depth/color registration.");
00553 }
00554
00555 if (_device->setDepthColorSyncEnabled(true) != openni::STATUS_OK)
00556 {
00557 UERROR("CameraOpenNI2: Failed to set depth color sync");
00558 }
00559
00560 _depth->setMirroringEnabled(false);
00561 _color->setMirroringEnabled(false);
00562
00563 const openni::Array<openni::VideoMode>& depthVideoModes = _depth->getSensorInfo().getSupportedVideoModes();
00564 for(int i=0; i<depthVideoModes.getSize(); ++i)
00565 {
00566 UINFO("CameraOpenNI2: Depth video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
00567 i,
00568 depthVideoModes[i].getFps(),
00569 depthVideoModes[i].getPixelFormat(),
00570 depthVideoModes[i].getResolutionX(),
00571 depthVideoModes[i].getResolutionY());
00572 }
00573
00574 const openni::Array<openni::VideoMode>& colorVideoModes = _color->getSensorInfo().getSupportedVideoModes();
00575 for(int i=0; i<colorVideoModes.getSize(); ++i)
00576 {
00577 UINFO("CameraOpenNI2: Color video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
00578 i,
00579 colorVideoModes[i].getFps(),
00580 colorVideoModes[i].getPixelFormat(),
00581 colorVideoModes[i].getResolutionX(),
00582 colorVideoModes[i].getResolutionY());
00583 }
00584
00585 openni::VideoMode mMode;
00586 mMode.setFps(30);
00587 mMode.setResolution(640,480);
00588 mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
00589 _depth->setVideoMode(mMode);
00590
00591 openni::VideoMode mModeColor;
00592 mModeColor.setFps(30);
00593 mModeColor.setResolution(640,480);
00594 mModeColor.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
00595 _color->setVideoMode(mModeColor);
00596
00597 UINFO("CameraOpenNI2: Using depth video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
00598 _depth->getVideoMode().getFps(),
00599 _depth->getVideoMode().getPixelFormat(),
00600 _depth->getVideoMode().getResolutionX(),
00601 _depth->getVideoMode().getResolutionY(),
00602 _depth->getHorizontalFieldOfView(),
00603 _depth->getVerticalFieldOfView());
00604
00605 if(_color->getCameraSettings())
00606 {
00607 UINFO("CameraOpenNI2: AutoWhiteBalanceEnabled = %d", _color->getCameraSettings()->getAutoWhiteBalanceEnabled()?1:0);
00608 UINFO("CameraOpenNI2: AutoExposureEnabled = %d", _color->getCameraSettings()->getAutoExposureEnabled()?1:0);
00609 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
00610 UINFO("CameraOpenNI2: Exposure = %d", _color->getCameraSettings()->getExposure());
00611 UINFO("CameraOpenNI2: GAIN = %d", _color->getCameraSettings()->getGain());
00612 #endif
00613 }
00614
00615 bool registered = true;
00616 if(registered)
00617 {
00618 _depthFx = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
00619 _depthFy = float(_color->getVideoMode().getResolutionY()/2) / std::tan(_color->getVerticalFieldOfView()/2.0f);
00620 }
00621 else
00622 {
00623 _depthFx = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
00624 _depthFy = float(_depth->getVideoMode().getResolutionY()/2) / std::tan(_depth->getVerticalFieldOfView()/2.0f);
00625 }
00626 UINFO("depth fx=%f fy=%f", _depthFx, _depthFy);
00627
00628 UINFO("CameraOpenNI2: Using color video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
00629 _color->getVideoMode().getFps(),
00630 _color->getVideoMode().getPixelFormat(),
00631 _color->getVideoMode().getResolutionX(),
00632 _color->getVideoMode().getResolutionY(),
00633 _color->getHorizontalFieldOfView(),
00634 _color->getVerticalFieldOfView());
00635
00636 if(_depth->start() != openni::STATUS_OK ||
00637 _color->start() != openni::STATUS_OK)
00638 {
00639 UERROR("CameraOpenNI2: Cannot start depth and/or color streams.");
00640 _depth->stop();
00641 _color->stop();
00642 _depth->destroy();
00643 _color->destroy();
00644 _device->close();
00645 openni::OpenNI::shutdown();
00646 return false;
00647 }
00648
00649 uSleep(1000);
00650
00651 return true;
00652 #else
00653 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00654 return false;
00655 #endif
00656 }
00657
00658 bool CameraOpenNI2::isCalibrated() const
00659 {
00660 return true;
00661 }
00662
00663 std::string CameraOpenNI2::getSerial() const
00664 {
00665 #ifdef RTABMAP_OPENNI2
00666 if(_device)
00667 {
00668 return _device->getDeviceInfo().getName();
00669 }
00670 #endif
00671 return "";
00672 }
00673
00674 SensorData CameraOpenNI2::captureImage(CameraInfo * info)
00675 {
00676 SensorData data;
00677 #ifdef RTABMAP_OPENNI2
00678 int readyStream = -1;
00679 if(_device->isValid() &&
00680 _depth->isValid() &&
00681 _color->isValid() &&
00682 _device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
00683 _device->getSensorInfo(openni::SENSOR_COLOR) != NULL)
00684 {
00685 openni::VideoStream* depthStream[] = {_depth};
00686 openni::VideoStream* colorStream[] = {_color};
00687 if(openni::OpenNI::waitForAnyStream(depthStream, 1, &readyStream, 2000) != openni::STATUS_OK ||
00688 openni::OpenNI::waitForAnyStream(colorStream, 1, &readyStream, 2000) != openni::STATUS_OK)
00689 {
00690 UWARN("No frames received since the last 2 seconds, end of stream is reached!");
00691 }
00692 else
00693 {
00694 openni::VideoFrameRef depthFrame, colorFrame;
00695 _depth->readFrame(&depthFrame);
00696 _color->readFrame(&colorFrame);
00697 cv::Mat depth, rgb;
00698 if(depthFrame.isValid() && colorFrame.isValid())
00699 {
00700 int h=depthFrame.getHeight();
00701 int w=depthFrame.getWidth();
00702 depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
00703
00704 h=colorFrame.getHeight();
00705 w=colorFrame.getWidth();
00706 cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
00707 cv::cvtColor(tmp, rgb, CV_RGB2BGR);
00708 }
00709 UASSERT(_depthFx != 0.0f && _depthFy != 0.0f);
00710 if(!rgb.empty() && !depth.empty())
00711 {
00712 CameraModel model(
00713 _depthFx,
00714 _depthFy,
00715 float(rgb.cols/2) - 0.5f,
00716 float(rgb.rows/2) - 0.5f,
00717 this->getLocalTransform(),
00718 0,
00719 rgb.size());
00720 if(_openNI2StampsAndIDsUsed)
00721 {
00722 data = SensorData(rgb, depth, model, depthFrame.getFrameIndex(), double(depthFrame.getTimestamp()) / 1000000.0);
00723 }
00724 else
00725 {
00726 data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
00727 }
00728 }
00729 }
00730 }
00731 else
00732 {
00733 ULOGGER_WARN("The camera must be initialized before requesting an image.");
00734 }
00735 #else
00736 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00737 #endif
00738 return data;
00739 }
00740
00741 #ifdef RTABMAP_FREENECT
00742
00743
00744
00745 class FreenectDevice : public UThread {
00746 public:
00747 FreenectDevice(freenect_context * ctx, int index) :
00748 index_(index),
00749 ctx_(ctx),
00750 device_(0),
00751 depthFocal_(0.0f)
00752 {
00753 UASSERT(ctx_ != 0);
00754 }
00755
00756 ~FreenectDevice()
00757 {
00758 this->join(true);
00759 if(device_ && freenect_close_device(device_) < 0){}
00760 }
00761
00762 const std::string & getSerial() const {return serial_;}
00763
00764 bool init()
00765 {
00766 if(device_)
00767 {
00768 this->join(true);
00769 freenect_close_device(device_);
00770 device_ = 0;
00771 }
00772 serial_.clear();
00773 std::vector<std::string> deviceSerials;
00774 freenect_device_attributes* attr_list;
00775 freenect_device_attributes* item;
00776 freenect_list_device_attributes(ctx_, &attr_list);
00777 for (item = attr_list; item != NULL; item = item->next) {
00778 deviceSerials.push_back(std::string(item->camera_serial));
00779 }
00780 freenect_free_device_attributes(attr_list);
00781
00782 if(freenect_open_device(ctx_, &device_, index_) < 0)
00783 {
00784 UERROR("FreenectDevice: Cannot open Kinect");
00785 return false;
00786 }
00787
00788 if(index_ >= 0 && index_ < (int)deviceSerials.size())
00789 {
00790 serial_ = deviceSerials[index_];
00791 }
00792 else
00793 {
00794 UERROR("Could not get serial for index %d", index_);
00795 }
00796
00797 freenect_set_user(device_, this);
00798 freenect_set_video_mode(device_, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
00799 freenect_set_depth_mode(device_, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED));
00800 depthBuffer_ = cv::Mat(cv::Size(640,480),CV_16UC1);
00801 rgbBuffer_ = cv::Mat(cv::Size(640,480), CV_8UC3);
00802 freenect_set_depth_buffer(device_, depthBuffer_.data);
00803 freenect_set_video_buffer(device_, rgbBuffer_.data);
00804 freenect_set_depth_callback(device_, freenect_depth_callback);
00805 freenect_set_video_callback(device_, freenect_video_callback);
00806
00807 bool registered = true;
00808 float rgb_focal_length_sxga = 1050.0f;
00809 float width_sxga = 1280.0f;
00810 float width = freenect_get_current_depth_mode(device_).width;
00811 float scale = width / width_sxga;
00812 if(registered)
00813 {
00814 depthFocal_ = rgb_focal_length_sxga * scale;
00815 }
00816 else
00817 {
00818 freenect_registration reg = freenect_copy_registration(device_);
00819 float depth_focal_length_sxga = reg.zero_plane_info.reference_distance / reg.zero_plane_info.reference_pixel_size;
00820 freenect_destroy_registration(®);
00821
00822 depthFocal_ = depth_focal_length_sxga * scale;
00823 }
00824
00825 UINFO("FreenectDevice: Depth focal = %f", depthFocal_);
00826 return true;
00827 }
00828
00829 float getDepthFocal() const {return depthFocal_;}
00830
00831 void getData(cv::Mat & rgb, cv::Mat & depth)
00832 {
00833 if(this->isRunning())
00834 {
00835 if(!dataReady_.acquire(1, 2000))
00836 {
00837 UERROR("Not received any frames since 2 seconds, try to restart the camera again.");
00838 }
00839 else
00840 {
00841 UScopeMutex s(dataMutex_);
00842 rgb = rgbLastFrame_;
00843 depth = depthLastFrame_;
00844 rgbLastFrame_ = cv::Mat();
00845 depthLastFrame_= cv::Mat();
00846 }
00847 }
00848 }
00849
00850 private:
00851
00852 void VideoCallback(void* rgb)
00853 {
00854 UASSERT(rgbBuffer_.data == rgb);
00855 UScopeMutex s(dataMutex_);
00856 bool notify = rgbLastFrame_.empty();
00857 cv::cvtColor(rgbBuffer_, rgbLastFrame_, CV_RGB2BGR);
00858 if(!depthLastFrame_.empty() && notify)
00859 {
00860 dataReady_.release();
00861 }
00862 }
00863
00864
00865 void DepthCallback(void* depth)
00866 {
00867 UASSERT(depthBuffer_.data == depth);
00868 UScopeMutex s(dataMutex_);
00869 bool notify = depthLastFrame_.empty();
00870 depthLastFrame_ = depthBuffer_.clone();
00871 if(!rgbLastFrame_.empty() && notify)
00872 {
00873 dataReady_.release();
00874 }
00875 }
00876
00877 void startVideo() {
00878 if(device_ && freenect_start_video(device_) < 0) UERROR("Cannot start RGB callback");
00879 }
00880 void stopVideo() {
00881 if(device_ && freenect_stop_video(device_) < 0) UERROR("Cannot stop RGB callback");
00882 }
00883 void startDepth() {
00884 if(device_ && freenect_start_depth(device_) < 0) UERROR("Cannot start depth callback");
00885 }
00886 void stopDepth() {
00887 if(device_ && freenect_stop_depth(device_) < 0) UERROR("Cannot stop depth callback");
00888 }
00889
00890 virtual void mainLoopBegin()
00891 {
00892 this->startDepth();
00893 this->startVideo();
00894 }
00895
00896 virtual void mainLoop()
00897 {
00898 timeval t;
00899 t.tv_sec = 0;
00900 t.tv_usec = 10000;
00901 if(freenect_process_events_timeout(ctx_, &t) < 0)
00902 {
00903 UERROR("FreenectDevice: Cannot process freenect events");
00904 this->kill();
00905 }
00906 }
00907
00908 virtual void mainLoopEnd()
00909 {
00910 this->stopDepth();
00911 this->stopVideo();
00912 dataReady_.release();
00913 }
00914
00915 static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {
00916 FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
00917 device->DepthCallback(depth);
00918 }
00919 static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {
00920 FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
00921 device->VideoCallback(video);
00922 }
00923
00924
00925 FreenectDevice( const FreenectDevice& );
00926 const FreenectDevice& operator=( const FreenectDevice& );
00927
00928 private:
00929 int index_;
00930 std::string serial_;
00931 freenect_context * ctx_;
00932 freenect_device * device_;
00933 cv::Mat depthBuffer_;
00934 cv::Mat rgbBuffer_;
00935 UMutex dataMutex_;
00936 cv::Mat depthLastFrame_;
00937 cv::Mat rgbLastFrame_;
00938 float depthFocal_;
00939 USemaphore dataReady_;
00940 };
00941 #endif
00942
00943
00944
00945
00946 bool CameraFreenect::available()
00947 {
00948 #ifdef RTABMAP_FREENECT
00949 return true;
00950 #else
00951 return false;
00952 #endif
00953 }
00954
00955 CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform) :
00956 Camera(imageRate, localTransform),
00957 deviceId_(deviceId),
00958 ctx_(0),
00959 freenectDevice_(0)
00960 {
00961 #ifdef RTABMAP_FREENECT
00962 if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
00963
00964 freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_CAMERA));
00965 #endif
00966 }
00967
00968 CameraFreenect::~CameraFreenect()
00969 {
00970 #ifdef RTABMAP_FREENECT
00971 if(freenectDevice_)
00972 {
00973 freenectDevice_->join(true);
00974 delete freenectDevice_;
00975 freenectDevice_ = 0;
00976 }
00977 if(ctx_)
00978 {
00979 if(freenect_shutdown(ctx_) < 0){}
00980 }
00981 #endif
00982 }
00983
00984 bool CameraFreenect::init(const std::string & calibrationFolder, const std::string & cameraName)
00985 {
00986 #ifdef RTABMAP_FREENECT
00987 if(freenectDevice_)
00988 {
00989 freenectDevice_->join(true);
00990 delete freenectDevice_;
00991 freenectDevice_ = 0;
00992 }
00993
00994 if(ctx_ && freenect_num_devices(ctx_) > 0)
00995 {
00996 freenectDevice_ = new FreenectDevice(ctx_, deviceId_);
00997 if(freenectDevice_->init())
00998 {
00999 freenectDevice_->start();
01000 uSleep(3000);
01001 return true;
01002 }
01003 else
01004 {
01005 UERROR("CameraFreenect: Init failed!");
01006 }
01007 delete freenectDevice_;
01008 freenectDevice_ = 0;
01009 }
01010 else
01011 {
01012 UERROR("CameraFreenect: No devices connected!");
01013 }
01014 #else
01015 UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
01016 #endif
01017 return false;
01018 }
01019
01020 bool CameraFreenect::isCalibrated() const
01021 {
01022 return true;
01023 }
01024
01025 std::string CameraFreenect::getSerial() const
01026 {
01027 #ifdef RTABMAP_FREENECT
01028 if(freenectDevice_)
01029 {
01030 return freenectDevice_->getSerial();
01031 }
01032 #endif
01033 return "";
01034 }
01035
01036 SensorData CameraFreenect::captureImage(CameraInfo * info)
01037 {
01038 SensorData data;
01039 #ifdef RTABMAP_FREENECT
01040 if(ctx_ && freenectDevice_)
01041 {
01042 if(freenectDevice_->isRunning())
01043 {
01044 cv::Mat depth,rgb;
01045 freenectDevice_->getData(rgb, depth);
01046 if(!rgb.empty() && !depth.empty())
01047 {
01048 UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
01049 if(!rgb.empty() && !depth.empty())
01050 {
01051 CameraModel model(
01052 freenectDevice_->getDepthFocal(),
01053 freenectDevice_->getDepthFocal(),
01054 float(rgb.cols/2) - 0.5f,
01055 float(rgb.rows/2) - 0.5f,
01056 this->getLocalTransform(),
01057 0,
01058 rgb.size());
01059 data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
01060 }
01061 }
01062 }
01063 else
01064 {
01065 UERROR("CameraFreenect: Re-initialization needed!");
01066 delete freenectDevice_;
01067 freenectDevice_ = 0;
01068 }
01069 }
01070 #else
01071 UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
01072 #endif
01073 return data;
01074 }
01075
01076
01077
01078
01079 bool CameraFreenect2::available()
01080 {
01081 #ifdef RTABMAP_FREENECT2
01082 return true;
01083 #else
01084 return false;
01085 #endif
01086 }
01087
01088 CameraFreenect2::CameraFreenect2(
01089 int deviceId,
01090 Type type,
01091 float imageRate,
01092 const Transform & localTransform,
01093 float minDepth,
01094 float maxDepth,
01095 bool bilateralFiltering,
01096 bool edgeAwareFiltering,
01097 bool noiseFiltering) :
01098 Camera(imageRate, localTransform),
01099 deviceId_(deviceId),
01100 type_(type),
01101 freenect2_(0),
01102 dev_(0),
01103 listener_(0),
01104 reg_(0),
01105 minKinect2Depth_(minDepth),
01106 maxKinect2Depth_(maxDepth),
01107 bilateralFiltering_(bilateralFiltering),
01108 edgeAwareFiltering_(edgeAwareFiltering),
01109 noiseFiltering_(noiseFiltering)
01110 {
01111 #ifdef RTABMAP_FREENECT2
01112 UASSERT(minKinect2Depth_ < maxKinect2Depth_ && minKinect2Depth_>0 && maxKinect2Depth_>0 && maxKinect2Depth_<=65.535f);
01113 freenect2_ = new libfreenect2::Freenect2();
01114 switch(type_)
01115 {
01116 case kTypeColorIR:
01117 listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
01118 break;
01119 case kTypeIRDepth:
01120 listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
01121 break;
01122 case kTypeColor2DepthSD:
01123 case kTypeDepth2ColorHD:
01124 case kTypeDepth2ColorSD:
01125 default:
01126 listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
01127 break;
01128 }
01129 #endif
01130 }
01131
01132 CameraFreenect2::~CameraFreenect2()
01133 {
01134 #ifdef RTABMAP_FREENECT2
01135 UDEBUG("");
01136 if(dev_)
01137 {
01138 dev_->stop();
01139 dev_->close();
01140
01141 }
01142 if(listener_)
01143 {
01144 delete listener_;
01145 }
01146
01147 if(reg_)
01148 {
01149 delete reg_;
01150 reg_ = 0;
01151 }
01152
01153 if(freenect2_)
01154 {
01155 delete freenect2_;
01156 }
01157 UDEBUG("");
01158 #endif
01159 }
01160
01161 bool CameraFreenect2::init(const std::string & calibrationFolder, const std::string & cameraName)
01162 {
01163 #ifdef RTABMAP_FREENECT2
01164 if(dev_)
01165 {
01166 dev_->stop();
01167 dev_->close();
01168 dev_ = 0;
01169 }
01170
01171 if(reg_)
01172 {
01173 delete reg_;
01174 reg_ = 0;
01175 }
01176
01177 libfreenect2::PacketPipeline * pipeline;
01178 #ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
01179 pipeline = new libfreenect2::OpenGLPacketPipeline();
01180 #else
01181 #ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
01182 pipeline = new libfreenect2::OpenCLPacketPipeline();
01183 #else
01184 pipeline = new libfreenect2::CpuPacketPipeline();
01185 #endif
01186 #endif
01187
01188 if(deviceId_ <= 0)
01189 {
01190 UDEBUG("Opening default device...");
01191 dev_ = freenect2_->openDefaultDevice(pipeline);
01192 pipeline = 0;
01193 }
01194 else
01195 {
01196 UDEBUG("Opening device ID=%d...", deviceId_);
01197 dev_ = freenect2_->openDevice(deviceId_, pipeline);
01198 pipeline = 0;
01199 }
01200
01201 if(dev_)
01202 {
01203
01204
01205
01206
01207
01208 libfreenect2::Freenect2Device::Config config;
01209 config.EnableBilateralFilter = bilateralFiltering_;
01210 config.EnableEdgeAwareFilter = edgeAwareFiltering_;
01211 config.MinDepth = minKinect2Depth_;
01212 config.MaxDepth = maxKinect2Depth_;
01213 dev_->setConfiguration(config);
01214
01215 dev_->setColorFrameListener(listener_);
01216 dev_->setIrAndDepthFrameListener(listener_);
01217
01218 dev_->start();
01219
01220 UINFO("CameraFreenect2: device serial: %s", dev_->getSerialNumber().c_str());
01221 UINFO("CameraFreenect2: device firmware: %s", dev_->getFirmwareVersion().c_str());
01222
01223
01224 libfreenect2::Freenect2Device::IrCameraParams depthParams = dev_->getIrCameraParams();
01225 libfreenect2::Freenect2Device::ColorCameraParams colorParams = dev_->getColorCameraParams();
01226 reg_ = new libfreenect2::Registration(depthParams, colorParams);
01227
01228
01229 if(!calibrationFolder.empty())
01230 {
01231 std::string calibrationName = dev_->getSerialNumber();
01232 if(!cameraName.empty())
01233 {
01234 calibrationName = cameraName;
01235 }
01236 if(!stereoModel_.load(calibrationFolder, calibrationName, false))
01237 {
01238 UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
01239 calibrationName.c_str(), calibrationFolder.c_str());
01240 }
01241 else
01242 {
01243 if(type_==kTypeColor2DepthSD)
01244 {
01245 UWARN("Freenect2: When using custom calibration file, type "
01246 "kTypeColor2DepthSD is not supported. kTypeDepth2ColorSD is used instead...");
01247 type_ = kTypeDepth2ColorSD;
01248 }
01249
01250
01251 cv::Mat colorP = stereoModel_.right().P();
01252 cv::Size colorSize = stereoModel_.right().imageSize();
01253 if(type_ == kTypeDepth2ColorSD)
01254 {
01255 colorP.at<double>(0,0)/=2.0f;
01256 colorP.at<double>(1,1)/=2.0f;
01257 colorP.at<double>(0,2)/=2.0f;
01258 colorP.at<double>(1,2)/=2.0f;
01259 colorSize.width/=2;
01260 colorSize.height/=2;
01261 }
01262 cv::Mat depthP = stereoModel_.left().P();
01263 cv::Size depthSize = stereoModel_.left().imageSize();
01264 float ratioY = float(colorSize.height)/float(depthSize.height);
01265 float ratioX = float(colorSize.width)/float(depthSize.width);
01266 depthP.at<double>(0,0)*=ratioX;
01267 depthP.at<double>(1,1)*=ratioY;
01268 depthP.at<double>(0,2)*=ratioX;
01269 depthP.at<double>(1,2)*=ratioY;
01270 depthSize.width*=ratioX;
01271 depthSize.height*=ratioY;
01272 const CameraModel & l = stereoModel_.left();
01273 const CameraModel & r = stereoModel_.right();
01274 stereoModel_ = StereoCameraModel(stereoModel_.name(),
01275 depthSize, l.K_raw(), l.D_raw(), l.R(), depthP,
01276 colorSize, r.K_raw(), r.D_raw(), r.R(), colorP,
01277 stereoModel_.R(), stereoModel_.T(), stereoModel_.E(), stereoModel_.F());
01278 stereoModel_.initRectificationMap();
01279 }
01280 }
01281
01282 return true;
01283 }
01284 else
01285 {
01286 UERROR("CameraFreenect2: no device connected or failure opening the default one! Note that rtabmap should link on libusb of libfreenect2. "
01287 "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
01288 }
01289 #else
01290 UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
01291 #endif
01292 return false;
01293 }
01294
01295 bool CameraFreenect2::isCalibrated() const
01296 {
01297 return true;
01298 }
01299
01300 std::string CameraFreenect2::getSerial() const
01301 {
01302 #ifdef RTABMAP_FREENECT2
01303 if(dev_)
01304 {
01305 return dev_->getSerialNumber();
01306 }
01307 #endif
01308 return "";
01309 }
01310
01311 SensorData CameraFreenect2::captureImage(CameraInfo * info)
01312 {
01313 SensorData data;
01314 #ifdef RTABMAP_FREENECT2
01315 if(dev_ && listener_)
01316 {
01317 libfreenect2::FrameMap frames;
01318 #ifndef LIBFREENECT2_THREADING_STDLIB
01319 UDEBUG("Waiting for new frames... If it is stalled here, rtabmap should link on libusb of libfreenect2. "
01320 "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
01321 listener_->waitForNewFrame(frames);
01322 #else
01323 if(!listener_->waitForNewFrame(frames, 1000))
01324 {
01325 UWARN("CameraFreenect2: Failed to get frames! rtabmap should link on libusb of libfreenect2. "
01326 "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
01327 }
01328 else
01329 #endif
01330 {
01331 double stamp = UTimer::now();
01332 libfreenect2::Frame *rgbFrame = 0;
01333 libfreenect2::Frame *irFrame = 0;
01334 libfreenect2::Frame *depthFrame = 0;
01335
01336 switch(type_)
01337 {
01338 case kTypeColorIR:
01339 rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
01340 irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
01341 break;
01342 case kTypeIRDepth:
01343 irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
01344 depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
01345 break;
01346 case kTypeColor2DepthSD:
01347 case kTypeDepth2ColorSD:
01348 case kTypeDepth2ColorHD:
01349 case kTypeDepth2ColorHD2:
01350 default:
01351 rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
01352 depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
01353 break;
01354 }
01355
01356 cv::Mat rgb, depth;
01357 float fx=0,fy=0,cx=0,cy=0;
01358 if(irFrame && depthFrame)
01359 {
01360 cv::Mat irMat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data);
01361
01362 float maxIr_ = 0x7FFF;
01363 float minIr_ = 0x0;
01364 const float factor = 255.0f / float((maxIr_ - minIr_));
01365 rgb = cv::Mat(irMat.rows, irMat.cols, CV_8UC1);
01366 for(int i=0; i<irMat.rows; ++i)
01367 {
01368 for(int j=0; j<irMat.cols; ++j)
01369 {
01370 rgb.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(irMat.at<float>(i,j) - minIr_, 0.0f)) * factor, 255.0f);
01371 }
01372 }
01373
01374 cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
01375
01376 cv::flip(rgb, rgb, 1);
01377 cv::flip(depth, depth, 1);
01378
01379 if(stereoModel_.isValidForRectification())
01380 {
01381
01382 rgb = stereoModel_.left().rectifyImage(rgb);
01383 depth = stereoModel_.left().rectifyImage(depth);
01384 fx = stereoModel_.left().fx();
01385 fy = stereoModel_.left().fy();
01386 cx = stereoModel_.left().cx();
01387 cy = stereoModel_.left().cy();
01388 }
01389 else
01390 {
01391 libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
01392 fx = params.fx;
01393 fy = params.fy;
01394 cx = params.cx;
01395 cy = params.cy;
01396 }
01397 }
01398 else
01399 {
01400
01401 if(stereoModel_.isValidForRectification())
01402 {
01403 cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
01404 cv::Mat rgbMat;
01405 cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
01406 cv::flip(rgbMat, rgb, 1);
01407
01408
01409 rgb = stereoModel_.right().rectifyImage(rgb);
01410 if(irFrame)
01411 {
01412
01413 cv::Mat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
01414 cv::flip(depth, depth, 1);
01415 depth = stereoModel_.left().rectifyImage(depth);
01416 }
01417 else
01418 {
01419
01420 cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
01421 cv::flip(depth, depth, 1);
01422 depth = stereoModel_.left().rectifyDepth(depth);
01423
01424 bool registered = true;
01425 if(registered)
01426 {
01427 depth = util2d::registerDepth(
01428 depth,
01429 stereoModel_.left().P().colRange(0,3).rowRange(0,3),
01430 stereoModel_.right().P().colRange(0,3).rowRange(0,3),
01431 stereoModel_.stereoTransform());
01432 util2d::fillRegisteredDepthHoles(depth, true, false);
01433 fx = stereoModel_.right().fx();
01434 fy = stereoModel_.right().fy();
01435 cx = stereoModel_.right().cx();
01436 cy = stereoModel_.right().cy();
01437 }
01438 else
01439 {
01440 fx = stereoModel_.left().fx();
01441 fy = stereoModel_.left().fy();
01442 cx = stereoModel_.left().cx();
01443 cy = stereoModel_.left().cy();
01444 }
01445 }
01446 }
01447 else
01448 {
01449
01450 if(irFrame)
01451 {
01452 cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
01453 cv::Mat rgbMat;
01454 cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
01455 cv::flip(rgbMat, rgb, 1);
01456
01457 cv::Mat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
01458 cv::flip(depth, depth, 1);
01459 }
01460 else
01461 {
01462
01463 UASSERT(reg_!=0);
01464
01465 float maxDepth = maxKinect2Depth_*1000.0f;
01466 float minDepth = minKinect2Depth_*1000.0f;
01467 if(type_ == kTypeColor2DepthSD || type_ == kTypeDepth2ColorHD)
01468 {
01469 cv::Mat rgbMatBGRA;
01470 libfreenect2::Frame depthUndistorted(512, 424, 4);
01471 libfreenect2::Frame rgbRegistered(512, 424, 4);
01472
01473
01474 if(noiseFiltering_)
01475 {
01476 cv::Mat depthMat = cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data);
01477 for(int dx=0; dx<depthMat.cols; ++dx)
01478 {
01479 bool onEdgeX = dx==depthMat.cols-1;
01480 for(int dy=0; dy<depthMat.rows; ++dy)
01481 {
01482 bool onEdge = onEdgeX || dy==depthMat.rows-1;
01483 float z = 0.0f;
01484 float & dz = depthMat.at<float>(dy,dx);
01485 if(dz>=minDepth && dz <= maxDepth)
01486 {
01487 z = dz;
01488 if(noiseFiltering_ && !onEdge)
01489 {
01490 z=0;
01491 const float & dz1 = depthMat.at<float>(dy,dx+1);
01492 const float & dz2 = depthMat.at<float>(dy+1,dx);
01493 const float & dz3 = depthMat.at<float>(dy+1,dx+1);
01494 if( dz1>=minDepth && dz1 <= maxDepth &&
01495 dz2>=minDepth && dz2 <= maxDepth &&
01496 dz3>=minDepth && dz3 <= maxDepth)
01497 {
01498 float avg = (dz + dz1 + dz2 + dz3) / 4.0f;
01499 float thres = 0.01f*avg;
01500
01501 if( fabs(dz-avg) < thres &&
01502 fabs(dz1-avg) < thres &&
01503 fabs(dz2-avg) < thres &&
01504 fabs(dz3-avg) < thres)
01505 {
01506 z = dz;
01507 }
01508 }
01509 }
01510 }
01511 dz = z;
01512 }
01513 }
01514 }
01515
01516 libfreenect2::Frame bidDepth(1920, 1082, 4);
01517 reg_->apply(rgbFrame, depthFrame, &depthUndistorted, &rgbRegistered, true, &bidDepth);
01518
01519 cv::Mat depthMat;
01520 if(type_ == kTypeColor2DepthSD)
01521 {
01522 rgbMatBGRA = cv::Mat((int)rgbRegistered.height, (int)rgbRegistered.width, CV_8UC4, rgbRegistered.data);
01523 depthMat = cv::Mat((int)depthUndistorted.height, (int)depthUndistorted.width, CV_32FC1, depthUndistorted.data);
01524
01525
01526 libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
01527 fx = params.fx;
01528 fy = params.fy;
01529 cx = params.cx;
01530 cy = params.cy;
01531 }
01532 else
01533 {
01534 rgbMatBGRA = cv::Mat((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
01535 depthMat = cv::Mat((int)bidDepth.height, (int)bidDepth.width, CV_32FC1, bidDepth.data);
01536 depthMat = depthMat(cv::Range(1, 1081), cv::Range::all());
01537
01538
01539 libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
01540 fx = params.fx;
01541 fy = params.fy;
01542 cx = params.cx;
01543 cy = params.cy;
01544 }
01545
01546
01547 depth = cv::Mat(depthMat.size(), CV_16UC1);
01548 for(int dx=0; dx<depthMat.cols; ++dx)
01549 {
01550 for(int dy=0; dy<depthMat.rows; ++dy)
01551 {
01552 unsigned short z = 0;
01553 const float & dz = depthMat.at<float>(dy,dx);
01554 if(dz>=minDepth && dz <= maxDepth)
01555 {
01556 z = (unsigned short)dz;
01557 }
01558 depth.at<unsigned short>(dy,(depthMat.cols-1)-dx) = z;
01559 }
01560 }
01561
01562
01563 cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
01564 cv::flip(rgb, rgb, 1);
01565 }
01566 else
01567 {
01568 UASSERT(type_ == kTypeDepth2ColorSD || type_ == kTypeDepth2ColorHD2);
01569 cv::Mat rgbMatBGRA = cv::Mat((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
01570 if(type_ == kTypeDepth2ColorSD)
01571 {
01572 cv::Mat tmp;
01573 cv::resize(rgbMatBGRA, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
01574 rgbMatBGRA = tmp;
01575 }
01576
01577 cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
01578 cv::flip(rgb, rgb, 1);
01579
01580 cv::Mat depthFrameMat = cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data);
01581 depth = cv::Mat::zeros(rgbMatBGRA.rows, rgbMatBGRA.cols, CV_16U);
01582 for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
01583 {
01584 for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
01585 {
01586 float dz = depthFrameMat.at<float>(dy,dx);
01587 if(dz>=minDepth && dz<=maxDepth)
01588 {
01589 bool goodDepth = true;
01590 if(noiseFiltering_)
01591 {
01592 goodDepth = false;
01593 float dz1 = depthFrameMat.at<float>(dy,dx+1);
01594 float dz2 = depthFrameMat.at<float>(dy+1,dx);
01595 float dz3 = depthFrameMat.at<float>(dy+1,dx+1);
01596 if(dz1>=minDepth && dz1 <= maxDepth &&
01597 dz2>=minDepth && dz2 <= maxDepth &&
01598 dz3>=minDepth && dz3 <= maxDepth)
01599 {
01600 float avg = (dz + dz1 + dz2 + dz3) / 4.0f;
01601 float thres = 0.01 * avg;
01602 if( fabs(dz-avg) < thres &&
01603 fabs(dz1-avg) < thres &&
01604 fabs(dz2-avg) < thres &&
01605 fabs(dz3-avg) < thres)
01606 {
01607 goodDepth = true;
01608 }
01609 }
01610 }
01611 if(goodDepth)
01612 {
01613 float cx=-1,cy=-1;
01614 reg_->apply(dx, dy, dz, cx, cy);
01615 if(type_ == kTypeDepth2ColorSD)
01616 {
01617 cx/=2.0f;
01618 cy/=2.0f;
01619 }
01620 int rcx = cvRound(cx);
01621 int rcy = cvRound(cy);
01622 if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows))
01623 {
01624 unsigned short & zReg = depth.at<unsigned short>(rcy, rcx);
01625 if(zReg == 0 || zReg > (unsigned short)dz)
01626 {
01627 zReg = (unsigned short)dz;
01628 }
01629 }
01630 }
01631 }
01632 }
01633 }
01634 util2d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeDepth2ColorHD2);
01635 util2d::fillRegisteredDepthHoles(depth, type_==kTypeDepth2ColorSD, type_==kTypeDepth2ColorHD2);
01636 cv::flip(depth, depth, 1);
01637 libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
01638 fx = params.fx*(type_==kTypeDepth2ColorSD?0.5:1.0f);
01639 fy = params.fy*(type_==kTypeDepth2ColorSD?0.5:1.0f);
01640 cx = params.cx*(type_==kTypeDepth2ColorSD?0.5:1.0f);
01641 cy = params.cy*(type_==kTypeDepth2ColorSD?0.5:1.0f);
01642 }
01643 }
01644 }
01645 }
01646
01647 CameraModel model;
01648 if(fx && fy)
01649 {
01650 model=CameraModel(
01651 fx,
01652 fy,
01653 cx,
01654 cy,
01655 this->getLocalTransform(),
01656 0,
01657 rgb.size());
01658 }
01659 data = SensorData(rgb, depth, model, this->getNextSeqID(), stamp);
01660
01661 listener_->release(frames);
01662 }
01663 }
01664 #else
01665 UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
01666 #endif
01667 return data;
01668 }
01669
01670
01671
01672
01673 bool CameraRGBDImages::available()
01674 {
01675 return true;
01676 }
01677
01678 CameraRGBDImages::CameraRGBDImages(
01679 const std::string & pathRGBImages,
01680 const std::string & pathDepthImages,
01681 float depthScaleFactor,
01682 float imageRate,
01683 const Transform & localTransform) :
01684 CameraImages(pathRGBImages, imageRate, localTransform)
01685 {
01686 UASSERT(depthScaleFactor >= 1.0);
01687 cameraDepth_.setPath(pathDepthImages);
01688 cameraDepth_.setDepth(true, depthScaleFactor);
01689 }
01690
01691 CameraRGBDImages::~CameraRGBDImages()
01692 {
01693 }
01694
01695 bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::string & cameraName)
01696 {
01697 bool success = false;
01698 if(CameraImages::init(calibrationFolder, cameraName) && cameraDepth_.init())
01699 {
01700 if(this->imagesCount() == cameraDepth_.imagesCount())
01701 {
01702 success = true;
01703 }
01704 else
01705 {
01706 UERROR("Cameras don't have the same number of images (%d vs %d)",
01707 this->imagesCount(), cameraDepth_.imagesCount());
01708 }
01709 }
01710
01711 return success;
01712 }
01713
01714 bool CameraRGBDImages::isCalibrated() const
01715 {
01716 return this->cameraModel().isValidForProjection();
01717 }
01718
01719 std::string CameraRGBDImages::getSerial() const
01720 {
01721 return this->cameraModel().name();
01722 }
01723
01724 SensorData CameraRGBDImages::captureImage(CameraInfo * info)
01725 {
01726 SensorData data;
01727
01728 SensorData rgb, depth;
01729 rgb = CameraImages::captureImage(info);
01730 if(!rgb.imageRaw().empty())
01731 {
01732 depth = cameraDepth_.takeImage();
01733 if(!depth.depthRaw().empty())
01734 {
01735 data = SensorData(rgb.imageRaw(), depth.depthRaw(), rgb.cameraModels(), rgb.id(), rgb.stamp());
01736 data.setGroundTruth(rgb.groundTruth());
01737 }
01738 }
01739 return data;
01740 }
01741
01742
01743 }