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 #include "rtabmap/core/CameraRGBD.h"
00028 #include "rtabmap/core/util2d.h"
00029 #include "rtabmap/core/CameraRGB.h"
00030 #include "rtabmap/core/Graph.h"
00031 #include "rtabmap/core/Version.h"
00032
00033 #include <rtabmap/utilite/UEventsManager.h>
00034 #include <rtabmap/utilite/UConversion.h>
00035 #include <rtabmap/utilite/UStl.h>
00036 #include <rtabmap/utilite/UConversion.h>
00037 #include <rtabmap/utilite/UFile.h>
00038 #include <rtabmap/utilite/UDirectory.h>
00039 #include <rtabmap/utilite/UTimer.h>
00040 #include <rtabmap/utilite/UMath.h>
00041
00042 #include <opencv2/imgproc/types_c.h>
00043 #if CV_MAJOR_VERSION >= 3
00044 #include <opencv2/videoio/videoio_c.h>
00045 #endif
00046
00047 #ifdef RTABMAP_OPENNI
00048 #include <pcl/io/openni_grabber.h>
00049 #include <pcl/io/oni_grabber.h>
00050 #include <pcl/io/openni_camera/openni_depth_image.h>
00051 #include <pcl/io/openni_camera/openni_image.h>
00052 #endif
00053
00054 #ifdef RTABMAP_FREENECT
00055 #include <libfreenect.h>
00056 #ifdef FREENECT_DASH_INCLUDES
00057 #include <libfreenect-registration.h>
00058 #else
00059 #include <libfreenect_registration.h>
00060 #endif
00061 #endif
00062
00063 #ifdef RTABMAP_FREENECT2
00064 #include <libfreenect2/libfreenect2.hpp>
00065 #include <libfreenect2/frame_listener_impl.h>
00066 #include <libfreenect2/registration.h>
00067 #include <libfreenect2/packet_pipeline.h>
00068 #include <libfreenect2/config.h>
00069 #endif
00070
00071 #ifdef RTABMAP_K4W2
00072 #include <Kinect.h>
00073 #endif
00074
00075 #ifdef RTABMAP_REALSENSE
00076 #include <librealsense/rs.hpp>
00077 #ifdef RTABMAP_REALSENSE_SLAM
00078 #include <rs_core.h>
00079 #include <rs_utils.h>
00080 #include <librealsense/slam/slam.h>
00081 #endif
00082 #endif
00083
00084 #ifdef RTABMAP_REALSENSE2
00085 #include <librealsense2/rs.hpp>
00086 #include <librealsense2/rsutil.h>
00087 #include <librealsense2/hpp/rs_processing.hpp>
00088 #include <librealsense2/rs_advanced_mode.hpp>
00089 #endif
00090
00091 #ifdef RTABMAP_OPENNI2
00092 #include <OniVersion.h>
00093 #include <OpenNI.h>
00094 #endif
00095
00096 namespace rtabmap
00097 {
00098
00100
00102 CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform) :
00103 Camera(imageRate, localTransform),
00104 interface_(0),
00105 deviceId_(deviceId),
00106 depthConstant_(0.0f)
00107 {
00108 }
00109
00110 bool CameraOpenni::available()
00111 {
00112 #ifdef RTABMAP_OPENNI
00113 return true;
00114 #else
00115 return false;
00116 #endif
00117 }
00118
00119 CameraOpenni::~CameraOpenni()
00120 {
00121 #ifdef RTABMAP_OPENNI
00122 UDEBUG("");
00123 if(connection_.connected())
00124 {
00125 connection_.disconnect();
00126 }
00127
00128 if(interface_)
00129 {
00130 interface_->stop();
00131 uSleep(1000);
00132 delete interface_;
00133 interface_ = 0;
00134 }
00135 #endif
00136 }
00137 #ifdef RTABMAP_OPENNI
00138 void CameraOpenni::image_cb (
00139 const boost::shared_ptr<openni_wrapper::Image>& rgb,
00140 const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
00141 float constant)
00142 {
00143 UScopeMutex s(dataMutex_);
00144
00145 bool notify = rgb_.empty();
00146
00147 cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
00148 rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
00149 cv::cvtColor(rgbFrame, rgb_, CV_RGB2BGR);
00150
00151 depth_ = cv::Mat(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
00152 depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depth_.data);
00153
00154 depthConstant_ = constant;
00155
00156 if(notify)
00157 {
00158 dataReady_.release();
00159 }
00160 }
00161 #endif
00162
00163 bool CameraOpenni::init(const std::string & calibrationFolder, const std::string & cameraName)
00164 {
00165 #ifdef RTABMAP_OPENNI
00166 if(interface_)
00167 {
00168 interface_->stop();
00169 uSleep(100);
00170 delete interface_;
00171 interface_ = 0;
00172 }
00173
00174 try
00175 {
00176 if(UFile::getExtension(deviceId_).compare("oni") == 0)
00177 {
00178 interface_ = new pcl::ONIGrabber(deviceId_, false, true);
00179 }
00180 else
00181 {
00182 interface_ = new pcl::OpenNIGrabber(deviceId_);
00183 }
00184
00185 boost::function<void (
00186 const boost::shared_ptr<openni_wrapper::Image>&,
00187 const boost::shared_ptr<openni_wrapper::DepthImage>&,
00188 float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
00189 connection_ = interface_->registerCallback (f);
00190
00191 interface_->start ();
00192 }
00193 catch(const pcl::IOException& ex)
00194 {
00195 UERROR("OpenNI exception: %s", ex.what());
00196 if(interface_)
00197 {
00198 delete interface_;
00199 interface_ = 0;
00200 }
00201 return false;
00202 }
00203 return true;
00204 #else
00205 UERROR("PCL not built with OpenNI! Cannot initialize CameraOpenNI");
00206 return false;
00207 #endif
00208 }
00209
00210 bool CameraOpenni::isCalibrated() const
00211 {
00212 #ifdef RTABMAP_OPENNI
00213 return true;
00214 #else
00215 return false;
00216 #endif
00217 }
00218
00219 std::string CameraOpenni::getSerial() const
00220 {
00221 #ifdef RTABMAP_OPENNI
00222 if(interface_)
00223 {
00224 return interface_->getName();
00225 }
00226 #endif
00227 return "";
00228 }
00229
00230 SensorData CameraOpenni::captureImage(CameraInfo * info)
00231 {
00232 SensorData data;
00233 #ifdef RTABMAP_OPENNI
00234 if(interface_ && interface_->isRunning())
00235 {
00236 if(!dataReady_.acquire(1, 5000))
00237 {
00238 UWARN("Not received new frames since 5 seconds, end of stream reached!");
00239 }
00240 else
00241 {
00242 UScopeMutex s(dataMutex_);
00243 if(depthConstant_ && !rgb_.empty() && !depth_.empty())
00244 {
00245 CameraModel model(
00246 1.0f/depthConstant_,
00247 1.0f/depthConstant_,
00248 float(rgb_.cols/2) - 0.5f,
00249 float(rgb_.rows/2) - 0.5f,
00250 this->getLocalTransform(),
00251 0,
00252 rgb_.size());
00253 data = SensorData(rgb_, depth_, model, this->getNextSeqID(), UTimer::now());
00254 }
00255
00256 depth_ = cv::Mat();
00257 rgb_ = cv::Mat();
00258 depthConstant_ = 0.0f;
00259 }
00260 }
00261 #else
00262 UERROR("CameraOpenNI: RTAB-Map is not built with PCL having OpenNI support!");
00263 #endif
00264 return data;
00265 }
00266
00267
00268
00270
00272 bool CameraOpenNICV::available()
00273 {
00274 return cv::getBuildInformation().find("OpenNI: YES") != std::string::npos;
00275 }
00276
00277 CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform) :
00278 Camera(imageRate, localTransform),
00279 _asus(asus),
00280 _depthFocal(0.0f)
00281 {
00282
00283 }
00284
00285 CameraOpenNICV::~CameraOpenNICV()
00286 {
00287 _capture.release();
00288 }
00289
00290 bool CameraOpenNICV::init(const std::string & calibrationFolder, const std::string & cameraName)
00291 {
00292 if(_capture.isOpened())
00293 {
00294 _capture.release();
00295 }
00296
00297 ULOGGER_DEBUG("Camera::init()");
00298 _capture.open( _asus?CV_CAP_OPENNI_ASUS:CV_CAP_OPENNI );
00299 if(_capture.isOpened())
00300 {
00301 _capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ );
00302 _depthFocal = _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH );
00303
00304 UINFO("Depth generator output mode:");
00305 UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
00306 UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
00307 UINFO("FRAME_MAX_DEPTH %f mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
00308 UINFO("BASELINE %f mm", _capture.get( CV_CAP_PROP_OPENNI_BASELINE ));
00309 UINFO("FPS %f", _capture.get( CV_CAP_PROP_FPS ));
00310 UINFO("Focal %f", _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH ));
00311 UINFO("REGISTRATION %f", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
00312 if(_capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) == 0.0)
00313 {
00314 UERROR("Depth registration is not activated on this device!");
00315 }
00316 if( _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
00317 {
00318 UINFO("Image generator output mode:");
00319 UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ));
00320 UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ));
00321 UINFO("FPS %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FPS ));
00322 }
00323 else
00324 {
00325 UERROR("Camera: Device doesn't contain image generator.");
00326 _capture.release();
00327 return false;
00328 }
00329 }
00330 else
00331 {
00332 ULOGGER_ERROR("Camera: Failed to create a capture object!");
00333 _capture.release();
00334 return false;
00335 }
00336 return true;
00337 }
00338
00339 bool CameraOpenNICV::isCalibrated() const
00340 {
00341 return true;
00342 }
00343
00344 SensorData CameraOpenNICV::captureImage(CameraInfo * info)
00345 {
00346 SensorData data;
00347 if(_capture.isOpened())
00348 {
00349 _capture.grab();
00350 cv::Mat depth, rgb;
00351 _capture.retrieve(depth, CV_CAP_OPENNI_DEPTH_MAP );
00352 _capture.retrieve(rgb, CV_CAP_OPENNI_BGR_IMAGE );
00353
00354 depth = depth.clone();
00355 rgb = rgb.clone();
00356
00357 UASSERT(_depthFocal>0.0f);
00358 if(!rgb.empty() && !depth.empty())
00359 {
00360 CameraModel model(
00361 _depthFocal,
00362 _depthFocal,
00363 float(rgb.cols/2) - 0.5f,
00364 float(rgb.rows/2) - 0.5f,
00365 this->getLocalTransform(),
00366 0,
00367 rgb.size());
00368 data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
00369 }
00370 }
00371 else
00372 {
00373 ULOGGER_WARN("The camera must be initialized before requesting an image.");
00374 }
00375 return data;
00376 }
00377
00378
00380
00382 bool CameraOpenNI2::available()
00383 {
00384 #ifdef RTABMAP_OPENNI2
00385 return true;
00386 #else
00387 return false;
00388 #endif
00389 }
00390
00391 bool CameraOpenNI2::exposureGainAvailable()
00392 {
00393 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
00394 return true;
00395 #else
00396 return false;
00397 #endif
00398 }
00399
00400 CameraOpenNI2::CameraOpenNI2(
00401 const std::string & deviceId,
00402 Type type,
00403 float imageRate,
00404 const rtabmap::Transform & localTransform) :
00405 Camera(imageRate, localTransform)
00406 #ifdef RTABMAP_OPENNI2
00407 ,
00408 _type(type),
00409 _device(new openni::Device()),
00410 _color(new openni::VideoStream()),
00411 _depth(new openni::VideoStream()),
00412 _depthFx(0.0f),
00413 _depthFy(0.0f),
00414 _deviceId(deviceId),
00415 _openNI2StampsAndIDsUsed(false),
00416 _depthHShift(0),
00417 _depthVShift(0)
00418 #endif
00419 {
00420 }
00421
00422 CameraOpenNI2::~CameraOpenNI2()
00423 {
00424 #ifdef RTABMAP_OPENNI2
00425 _color->stop();
00426 _color->destroy();
00427 _depth->stop();
00428 _depth->destroy();
00429 _device->close();
00430 openni::OpenNI::shutdown();
00431
00432 delete _device;
00433 delete _color;
00434 delete _depth;
00435 #endif
00436 }
00437
00438 bool CameraOpenNI2::setAutoWhiteBalance(bool enabled)
00439 {
00440 #ifdef RTABMAP_OPENNI2
00441 if(_color && _color->getCameraSettings())
00442 {
00443 return _color->getCameraSettings()->setAutoWhiteBalanceEnabled(enabled) == openni::STATUS_OK;
00444 }
00445 #else
00446 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00447 #endif
00448 return false;
00449 }
00450
00451 bool CameraOpenNI2::setAutoExposure(bool enabled)
00452 {
00453 #ifdef RTABMAP_OPENNI2
00454 if(_color && _color->getCameraSettings())
00455 {
00456 return _color->getCameraSettings()->setAutoExposureEnabled(enabled) == openni::STATUS_OK;
00457 }
00458 #else
00459 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00460 #endif
00461 return false;
00462 }
00463
00464 bool CameraOpenNI2::setExposure(int value)
00465 {
00466 #ifdef RTABMAP_OPENNI2
00467 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
00468 if(_color && _color->getCameraSettings())
00469 {
00470 return _color->getCameraSettings()->setExposure(value) == openni::STATUS_OK;
00471 }
00472 #else
00473 UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
00474 #endif
00475 #else
00476 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00477 #endif
00478 return false;
00479 }
00480
00481 bool CameraOpenNI2::setGain(int value)
00482 {
00483 #ifdef RTABMAP_OPENNI2
00484 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
00485 if(_color && _color->getCameraSettings())
00486 {
00487 return _color->getCameraSettings()->setGain(value) == openni::STATUS_OK;
00488 }
00489 #else
00490 UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
00491 #endif
00492 #else
00493 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00494 #endif
00495 return false;
00496 }
00497
00498 bool CameraOpenNI2::setMirroring(bool enabled)
00499 {
00500 #ifdef RTABMAP_OPENNI2
00501 if(_color->isValid() && _depth->isValid())
00502 {
00503 return _depth->setMirroringEnabled(enabled) == openni::STATUS_OK &&
00504 _color->setMirroringEnabled(enabled) == openni::STATUS_OK;
00505 }
00506 #endif
00507 return false;
00508 }
00509
00510 void CameraOpenNI2::setOpenNI2StampsAndIDsUsed(bool used)
00511 {
00512 #ifdef RTABMAP_OPENNI2
00513 _openNI2StampsAndIDsUsed = used;
00514 #endif
00515 }
00516
00517 void CameraOpenNI2::setIRDepthShift(int horizontal, int vertical)
00518 {
00519 #ifdef RTABMAP_OPENNI2
00520 UASSERT(horizontal >= 0);
00521 UASSERT(vertical >= 0);
00522 _depthHShift = horizontal;
00523 _depthVShift = vertical;
00524 #endif
00525 }
00526
00527 bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::string & cameraName)
00528 {
00529 #ifdef RTABMAP_OPENNI2
00530 openni::OpenNI::initialize();
00531
00532 openni::Array<openni::DeviceInfo> devices;
00533 openni::OpenNI::enumerateDevices(&devices);
00534 for(int i=0; i<devices.getSize(); ++i)
00535 {
00536 UINFO("Device %d: Name=%s URI=%s Vendor=%s",
00537 i,
00538 devices[i].getName(),
00539 devices[i].getUri(),
00540 devices[i].getVendor());
00541 }
00542 if(_deviceId.empty() && devices.getSize() == 0)
00543 {
00544 UERROR("CameraOpenNI2: No device detected!");
00545 return false;
00546 }
00547
00548 openni::Status error = _device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str());
00549 if(error != openni::STATUS_OK)
00550 {
00551 if(!_deviceId.empty())
00552 {
00553 UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d).", _deviceId.c_str(), error);
00554 }
00555 else
00556 {
00557 #ifdef _WIN32
00558 UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d).", devices[0].getName(), error);
00559 #else
00560 UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d). Verify if \"%s\" is in udev rules: \"/lib/udev/rules.d/40-libopenni2-0.rules\". If not, add it and reboot.", devices[0].getName(), error, devices[0].getUri());
00561 #endif
00562 }
00563
00564 _device->close();
00565 openni::OpenNI::shutdown();
00566 return false;
00567 }
00568
00569
00570 _stereoModel = StereoCameraModel();
00571 bool hardwareRegistration = true;
00572 if(!calibrationFolder.empty())
00573 {
00574
00575 std::string calibrationName = _device->getDeviceInfo().getName();
00576 if(!cameraName.empty())
00577 {
00578 calibrationName = cameraName;
00579 }
00580 _stereoModel.setName(calibrationName, "depth", "rgb");
00581 hardwareRegistration = !_stereoModel.load(calibrationFolder, calibrationName, false);
00582
00583 if(_type != kTypeColorDepth)
00584 {
00585 hardwareRegistration = false;
00586 }
00587
00588
00589 if((_type != kTypeColorDepth && !_stereoModel.left().isValidForRectification()) ||
00590 (_type == kTypeColorDepth && !_stereoModel.right().isValidForRectification()))
00591 {
00592 UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
00593 calibrationName.c_str(), calibrationFolder.c_str());
00594 }
00595 else if(_type == kTypeColorDepth && _stereoModel.right().isValidForRectification() && hardwareRegistration)
00596 {
00597 UWARN("Missing extrinsic calibration file for camera \"%s\" in \"%s\" folder, default registration is used even if rgb is rectified!",
00598 calibrationName.c_str(), calibrationFolder.c_str());
00599 }
00600 else if(_type == kTypeColorDepth && _stereoModel.right().isValidForRectification() && !hardwareRegistration)
00601 {
00602 UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
00603 "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
00604 }
00605 }
00606
00607 if(UFile::getExtension(_deviceId).compare("oni")==0)
00608 {
00609 if(_device->getPlaybackControl() &&
00610 _device->getPlaybackControl()->setRepeatEnabled(false) != openni::STATUS_OK)
00611 {
00612 UERROR("CameraOpenNI2: Cannot set repeat mode to false.");
00613 _device->close();
00614 openni::OpenNI::shutdown();
00615 return false;
00616 }
00617 }
00618 else if(_type==kTypeColorDepth && hardwareRegistration &&
00619 !_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
00620 {
00621 UERROR("CameraOpenNI2: Device doesn't support depth/color registration.");
00622 _device->close();
00623 openni::OpenNI::shutdown();
00624 return false;
00625 }
00626
00627 if(_device->getSensorInfo(openni::SENSOR_DEPTH) == NULL ||
00628 _device->getSensorInfo(_type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) == NULL)
00629 {
00630 UERROR("CameraOpenNI2: Cannot get sensor info for depth and %s.", _type==kTypeColorDepth?"color":"ir");
00631 _device->close();
00632 openni::OpenNI::shutdown();
00633 return false;
00634 }
00635
00636 if(_depth->create(*_device, openni::SENSOR_DEPTH) != openni::STATUS_OK)
00637 {
00638 UERROR("CameraOpenNI2: Cannot create depth stream.");
00639 _device->close();
00640 openni::OpenNI::shutdown();
00641 return false;
00642 }
00643
00644 if(_color->create(*_device, _type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) != openni::STATUS_OK)
00645 {
00646 UERROR("CameraOpenNI2: Cannot create %s stream.", _type==kTypeColorDepth?"color":"ir");
00647 _depth->destroy();
00648 _device->close();
00649 openni::OpenNI::shutdown();
00650 return false;
00651 }
00652
00653 if(_type==kTypeColorDepth && hardwareRegistration &&
00654 _device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
00655 {
00656 UERROR("CameraOpenNI2: Failed to set depth/color registration.");
00657 }
00658
00659 if (_device->setDepthColorSyncEnabled(true) != openni::STATUS_OK)
00660 {
00661 UERROR("CameraOpenNI2: Failed to set depth color sync");
00662 }
00663
00664 _depth->setMirroringEnabled(false);
00665 _color->setMirroringEnabled(false);
00666
00667 const openni::Array<openni::VideoMode>& depthVideoModes = _depth->getSensorInfo().getSupportedVideoModes();
00668 for(int i=0; i<depthVideoModes.getSize(); ++i)
00669 {
00670 UINFO("CameraOpenNI2: Depth video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
00671 i,
00672 depthVideoModes[i].getFps(),
00673 depthVideoModes[i].getPixelFormat(),
00674 depthVideoModes[i].getResolutionX(),
00675 depthVideoModes[i].getResolutionY());
00676 }
00677
00678 const openni::Array<openni::VideoMode>& colorVideoModes = _color->getSensorInfo().getSupportedVideoModes();
00679 for(int i=0; i<colorVideoModes.getSize(); ++i)
00680 {
00681 UINFO("CameraOpenNI2: %s video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
00682 _type==kTypeColorDepth?"color":"ir",
00683 i,
00684 colorVideoModes[i].getFps(),
00685 colorVideoModes[i].getPixelFormat(),
00686 colorVideoModes[i].getResolutionX(),
00687 colorVideoModes[i].getResolutionY());
00688 }
00689
00690 openni::VideoMode mMode;
00691 mMode.setFps(30);
00692 mMode.setResolution(640,480);
00693 mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
00694 _depth->setVideoMode(mMode);
00695
00696 openni::VideoMode mModeColor;
00697 mModeColor.setFps(30);
00698 mModeColor.setResolution(640,480);
00699 mModeColor.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
00700 _color->setVideoMode(mModeColor);
00701
00702 UINFO("CameraOpenNI2: Using depth video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
00703 _depth->getVideoMode().getFps(),
00704 _depth->getVideoMode().getPixelFormat(),
00705 _depth->getVideoMode().getResolutionX(),
00706 _depth->getVideoMode().getResolutionY(),
00707 _depth->getHorizontalFieldOfView(),
00708 _depth->getVerticalFieldOfView());
00709 UINFO("CameraOpenNI2: Using %s video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
00710 _type==kTypeColorDepth?"color":"ir",
00711 _color->getVideoMode().getFps(),
00712 _color->getVideoMode().getPixelFormat(),
00713 _color->getVideoMode().getResolutionX(),
00714 _color->getVideoMode().getResolutionY(),
00715 _color->getHorizontalFieldOfView(),
00716 _color->getVerticalFieldOfView());
00717
00718 if(_depth->getVideoMode().getResolutionX() != 640 ||
00719 _depth->getVideoMode().getResolutionY() != 480 ||
00720 _depth->getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_DEPTH_1_MM)
00721 {
00722 UERROR("Could not set depth format to 640x480 pixel=%d(mm)!",
00723 openni::PIXEL_FORMAT_DEPTH_1_MM);
00724 _depth->destroy();
00725 _color->destroy();
00726 _device->close();
00727 openni::OpenNI::shutdown();
00728 return false;
00729 }
00730 if(_color->getVideoMode().getResolutionX() != 640 ||
00731 _color->getVideoMode().getResolutionY() != 480 ||
00732 _color->getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_RGB888)
00733 {
00734 UERROR("Could not set %s format to 640x480 pixel=%d!",
00735 _type==kTypeColorDepth?"color":"ir",
00736 openni::PIXEL_FORMAT_RGB888);
00737 _depth->destroy();
00738 _color->destroy();
00739 _device->close();
00740 openni::OpenNI::shutdown();
00741 return false;
00742 }
00743
00744 if(_color->getCameraSettings())
00745 {
00746 UINFO("CameraOpenNI2: AutoWhiteBalanceEnabled = %d", _color->getCameraSettings()->getAutoWhiteBalanceEnabled()?1:0);
00747 UINFO("CameraOpenNI2: AutoExposureEnabled = %d", _color->getCameraSettings()->getAutoExposureEnabled()?1:0);
00748 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
00749 UINFO("CameraOpenNI2: Exposure = %d", _color->getCameraSettings()->getExposure());
00750 UINFO("CameraOpenNI2: GAIN = %d", _color->getCameraSettings()->getGain());
00751 #endif
00752 }
00753
00754 if(_type==kTypeColorDepth && hardwareRegistration)
00755 {
00756 _depthFx = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
00757 _depthFy = float(_color->getVideoMode().getResolutionY()/2) / std::tan(_color->getVerticalFieldOfView()/2.0f);
00758 }
00759 else
00760 {
00761 _depthFx = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
00762 _depthFy = float(_depth->getVideoMode().getResolutionY()/2) / std::tan(_depth->getVerticalFieldOfView()/2.0f);
00763 }
00764 UINFO("depth fx=%f fy=%f", _depthFx, _depthFy);
00765
00766 if(_type == kTypeIR)
00767 {
00768 UWARN("With type IR-only, depth stream will not be started");
00769 }
00770
00771 if((_type != kTypeIR && _depth->start() != openni::STATUS_OK) ||
00772 _color->start() != openni::STATUS_OK)
00773 {
00774 UERROR("CameraOpenNI2: Cannot start depth and/or color streams.");
00775 _depth->stop();
00776 _color->stop();
00777 _depth->destroy();
00778 _color->destroy();
00779 _device->close();
00780 openni::OpenNI::shutdown();
00781 return false;
00782 }
00783
00784 uSleep(3000);
00785
00786 return true;
00787 #else
00788 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00789 return false;
00790 #endif
00791 }
00792
00793 bool CameraOpenNI2::isCalibrated() const
00794 {
00795 return true;
00796 }
00797
00798 std::string CameraOpenNI2::getSerial() const
00799 {
00800 #ifdef RTABMAP_OPENNI2
00801 if(_device)
00802 {
00803 return _device->getDeviceInfo().getName();
00804 }
00805 #endif
00806 return "";
00807 }
00808
00809 SensorData CameraOpenNI2::captureImage(CameraInfo * info)
00810 {
00811 SensorData data;
00812 #ifdef RTABMAP_OPENNI2
00813 int readyStream = -1;
00814 if(_device->isValid() &&
00815 _depth->isValid() &&
00816 _color->isValid() &&
00817 _device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
00818 _device->getSensorInfo(_type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) != NULL)
00819 {
00820 openni::VideoStream* depthStream[] = {_depth};
00821 openni::VideoStream* colorStream[] = {_color};
00822 if((_type != kTypeIR && openni::OpenNI::waitForAnyStream(depthStream, 1, &readyStream, 5000) != openni::STATUS_OK) ||
00823 openni::OpenNI::waitForAnyStream(colorStream, 1, &readyStream, 5000) != openni::STATUS_OK)
00824 {
00825 UWARN("No frames received since the last 5 seconds, end of stream is reached!");
00826 }
00827 else
00828 {
00829 openni::VideoFrameRef depthFrame, colorFrame;
00830 if(_type != kTypeIR)
00831 {
00832 _depth->readFrame(&depthFrame);
00833 }
00834 _color->readFrame(&colorFrame);
00835 cv::Mat depth, rgb;
00836 if((_type == kTypeIR || depthFrame.isValid()) && colorFrame.isValid())
00837 {
00838 int h,w;
00839 if(_type != kTypeIR)
00840 {
00841 h=depthFrame.getHeight();
00842 w=depthFrame.getWidth();
00843 depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
00844 }
00845 h=colorFrame.getHeight();
00846 w=colorFrame.getWidth();
00847 cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
00848 if(_type==kTypeColorDepth)
00849 {
00850 cv::cvtColor(tmp, rgb, CV_RGB2BGR);
00851 }
00852 else
00853 {
00854 rgb = tmp.clone();
00855 }
00856 }
00857 UASSERT(_depthFx != 0.0f && _depthFy != 0.0f);
00858 if(!rgb.empty() && (_type == kTypeIR || !depth.empty()))
00859 {
00860
00861 CameraModel model(
00862 _depthFx,
00863 _depthFy,
00864 float(rgb.cols/2) - 0.5f,
00865 float(rgb.rows/2) - 0.5f,
00866 this->getLocalTransform(),
00867 0,
00868 rgb.size());
00869
00870 if(_type==kTypeColorDepth)
00871 {
00872 if(_stereoModel.right().isValidForRectification())
00873 {
00874 rgb = _stereoModel.right().rectifyImage(rgb);
00875 model = _stereoModel.right();
00876
00877 if(_stereoModel.left().isValidForRectification() && !_stereoModel.stereoTransform().isNull())
00878 {
00879 if (_depthHShift > 0 || _depthVShift > 0)
00880 {
00881 cv::Mat out = cv::Mat::zeros(depth.size(), depth.type());
00882 depth(cv::Rect(_depthHShift, _depthVShift, depth.cols - _depthHShift, depth.rows - _depthVShift)).copyTo(out(cv::Rect(0, 0, depth.cols - _depthHShift, depth.rows - _depthVShift)));
00883 depth = out;
00884 }
00885 depth = _stereoModel.left().rectifyImage(depth, 0);
00886 depth = util2d::registerDepth(depth, _stereoModel.left().K(), rgb.size(), _stereoModel.right().K(), _stereoModel.stereoTransform());
00887 }
00888 }
00889 }
00890 else
00891 {
00892 if(_stereoModel.left().isValidForRectification())
00893 {
00894 rgb = _stereoModel.left().rectifyImage(rgb);
00895 if(_type!=kTypeIR)
00896 {
00897 depth = _stereoModel.left().rectifyImage(depth, 0);
00898 }
00899 model = _stereoModel.left();
00900 }
00901 }
00902 model.setLocalTransform(this->getLocalTransform());
00903
00904 if(_openNI2StampsAndIDsUsed)
00905 {
00906 data = SensorData(rgb, depth, model, depthFrame.getFrameIndex(), double(depthFrame.getTimestamp()) / 1000000.0);
00907 }
00908 else
00909 {
00910 data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
00911 }
00912 }
00913 }
00914 }
00915 else
00916 {
00917 ULOGGER_WARN("The camera must be initialized before requesting an image.");
00918 }
00919 #else
00920 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00921 #endif
00922 return data;
00923 }
00924
00925 #ifdef RTABMAP_FREENECT
00926
00927
00928
00929 class FreenectDevice : public UThread {
00930 public:
00931 FreenectDevice(freenect_context * ctx, int index, bool color = true, bool registered = true) :
00932 index_(index),
00933 color_(color),
00934 registered_(registered),
00935 ctx_(ctx),
00936 device_(0),
00937 depthFocal_(0.0f)
00938 {
00939 UASSERT(ctx_ != 0);
00940 }
00941
00942 ~FreenectDevice()
00943 {
00944 this->join(true);
00945 if(device_ && freenect_close_device(device_) < 0){}
00946 }
00947
00948 const std::string & getSerial() const {return serial_;}
00949
00950 bool init()
00951 {
00952 if(device_)
00953 {
00954 this->join(true);
00955 freenect_close_device(device_);
00956 device_ = 0;
00957 }
00958 serial_.clear();
00959 std::vector<std::string> deviceSerials;
00960 freenect_device_attributes* attr_list;
00961 freenect_device_attributes* item;
00962 freenect_list_device_attributes(ctx_, &attr_list);
00963 for (item = attr_list; item != NULL; item = item->next) {
00964 deviceSerials.push_back(std::string(item->camera_serial));
00965 }
00966 freenect_free_device_attributes(attr_list);
00967
00968 if(freenect_open_device(ctx_, &device_, index_) < 0)
00969 {
00970 UERROR("FreenectDevice: Cannot open Kinect");
00971 return false;
00972 }
00973
00974 if(index_ >= 0 && index_ < (int)deviceSerials.size())
00975 {
00976 serial_ = deviceSerials[index_];
00977 }
00978 else
00979 {
00980 UERROR("Could not get serial for index %d", index_);
00981 }
00982
00983 UINFO("color=%d registered=%d", color_?1:0, registered_?1:0);
00984
00985 freenect_set_user(device_, this);
00986 freenect_frame_mode videoMode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, color_?FREENECT_VIDEO_RGB:FREENECT_VIDEO_IR_8BIT);
00987 freenect_frame_mode depthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, color_ && registered_?FREENECT_DEPTH_REGISTERED:FREENECT_DEPTH_MM);
00988 if(!videoMode.is_valid)
00989 {
00990 UERROR("Freenect: video mode selected not valid!");
00991 return false;
00992 }
00993 if(!depthMode.is_valid)
00994 {
00995 UERROR("Freenect: depth mode selected not valid!");
00996 return false;
00997 }
00998 UASSERT(videoMode.data_bits_per_pixel == 8 || videoMode.data_bits_per_pixel == 24);
00999 UASSERT(depthMode.data_bits_per_pixel == 16);
01000 freenect_set_video_mode(device_, videoMode);
01001 freenect_set_depth_mode(device_, depthMode);
01002 rgbIrBuffer_ = cv::Mat(cv::Size(videoMode.width,videoMode.height), color_?CV_8UC3:CV_8UC1);
01003 depthBuffer_ = cv::Mat(cv::Size(depthMode.width,depthMode.height), CV_16UC1);
01004 freenect_set_depth_buffer(device_, depthBuffer_.data);
01005 freenect_set_video_buffer(device_, rgbIrBuffer_.data);
01006 freenect_set_depth_callback(device_, freenect_depth_callback);
01007 freenect_set_video_callback(device_, freenect_video_callback);
01008
01009 float rgb_focal_length_sxga = 1050.0f;
01010 float width_sxga = 1280.0f;
01011 float width = freenect_get_current_depth_mode(device_).width;
01012 float scale = width / width_sxga;
01013 if(color_ && registered_)
01014 {
01015 depthFocal_ = rgb_focal_length_sxga * scale;
01016 }
01017 else
01018 {
01019 freenect_registration reg = freenect_copy_registration(device_);
01020 float depth_focal_length_sxga = reg.zero_plane_info.reference_distance / reg.zero_plane_info.reference_pixel_size;
01021 freenect_destroy_registration(®);
01022
01023 depthFocal_ = depth_focal_length_sxga * scale;
01024 }
01025
01026 UINFO("FreenectDevice: Depth focal = %f", depthFocal_);
01027 return true;
01028 }
01029
01030 float getDepthFocal() const {return depthFocal_;}
01031
01032 void getData(cv::Mat & rgb, cv::Mat & depth)
01033 {
01034 if(this->isRunning())
01035 {
01036 if(!dataReady_.acquire(1, 5000))
01037 {
01038 UERROR("Not received any frames since 5 seconds, try to restart the camera again.");
01039 }
01040 else
01041 {
01042 UScopeMutex s(dataMutex_);
01043 rgb = rgbIrLastFrame_;
01044 depth = depthLastFrame_;
01045 rgbIrLastFrame_ = cv::Mat();
01046 depthLastFrame_= cv::Mat();
01047 }
01048 }
01049 }
01050
01051 private:
01052
01053 void VideoCallback(void* rgb)
01054 {
01055 UASSERT(rgbIrBuffer_.data == rgb);
01056 UScopeMutex s(dataMutex_);
01057 bool notify = rgbIrLastFrame_.empty();
01058
01059 if(color_)
01060 {
01061 cv::cvtColor(rgbIrBuffer_, rgbIrLastFrame_, CV_RGB2BGR);
01062 }
01063 else
01064 {
01065 rgbIrLastFrame_ = rgbIrBuffer_.clone();
01066 }
01067 if(!depthLastFrame_.empty() && notify)
01068 {
01069 dataReady_.release();
01070 }
01071 }
01072
01073
01074 void DepthCallback(void* depth)
01075 {
01076 UASSERT(depthBuffer_.data == depth);
01077 UScopeMutex s(dataMutex_);
01078 bool notify = depthLastFrame_.empty();
01079 depthLastFrame_ = depthBuffer_.clone();
01080 if(!rgbIrLastFrame_.empty() && notify)
01081 {
01082 dataReady_.release();
01083 }
01084 }
01085
01086 void startVideo() {
01087 if(device_ && freenect_start_video(device_) < 0) UERROR("Cannot start RGB callback");
01088 }
01089 void stopVideo() {
01090 if(device_ && freenect_stop_video(device_) < 0) UERROR("Cannot stop RGB callback");
01091 }
01092 void startDepth() {
01093 if(device_ && freenect_start_depth(device_) < 0) UERROR("Cannot start depth callback");
01094 }
01095 void stopDepth() {
01096 if(device_ && freenect_stop_depth(device_) < 0) UERROR("Cannot stop depth callback");
01097 }
01098
01099 virtual void mainLoopBegin()
01100 {
01101 this->startDepth();
01102 this->startVideo();
01103 }
01104
01105 virtual void mainLoop()
01106 {
01107 timeval t;
01108 t.tv_sec = 0;
01109 t.tv_usec = 10000;
01110 if(freenect_process_events_timeout(ctx_, &t) < 0)
01111 {
01112 UERROR("FreenectDevice: Cannot process freenect events");
01113 this->kill();
01114 }
01115 }
01116
01117 virtual void mainLoopEnd()
01118 {
01119 this->stopDepth();
01120 this->stopVideo();
01121 dataReady_.release();
01122 }
01123
01124 static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {
01125 FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
01126 device->DepthCallback(depth);
01127 }
01128 static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {
01129 FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
01130 device->VideoCallback(video);
01131 }
01132
01133
01134 FreenectDevice( const FreenectDevice& );
01135 const FreenectDevice& operator=( const FreenectDevice& );
01136
01137 private:
01138 int index_;
01139 bool color_;
01140 bool registered_;
01141 std::string serial_;
01142 freenect_context * ctx_;
01143 freenect_device * device_;
01144 cv::Mat depthBuffer_;
01145 cv::Mat rgbIrBuffer_;
01146 UMutex dataMutex_;
01147 cv::Mat depthLastFrame_;
01148 cv::Mat rgbIrLastFrame_;
01149 float depthFocal_;
01150 USemaphore dataReady_;
01151 };
01152 #endif
01153
01154
01155
01156
01157 bool CameraFreenect::available()
01158 {
01159 #ifdef RTABMAP_FREENECT
01160 return true;
01161 #else
01162 return false;
01163 #endif
01164 }
01165
01166 CameraFreenect::CameraFreenect(int deviceId, Type type, float imageRate, const Transform & localTransform) :
01167 Camera(imageRate, localTransform)
01168 #ifdef RTABMAP_FREENECT
01169 ,
01170 deviceId_(deviceId),
01171 type_(type),
01172 ctx_(0),
01173 freenectDevice_(0)
01174 #endif
01175 {
01176 #ifdef RTABMAP_FREENECT
01177 if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
01178
01179 freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_CAMERA));
01180 #endif
01181 }
01182
01183 CameraFreenect::~CameraFreenect()
01184 {
01185 #ifdef RTABMAP_FREENECT
01186 if(freenectDevice_)
01187 {
01188 freenectDevice_->join(true);
01189 delete freenectDevice_;
01190 freenectDevice_ = 0;
01191 }
01192 if(ctx_)
01193 {
01194 if(freenect_shutdown(ctx_) < 0){}
01195 }
01196 #endif
01197 }
01198
01199 bool CameraFreenect::init(const std::string & calibrationFolder, const std::string & cameraName)
01200 {
01201 #ifdef RTABMAP_FREENECT
01202 if(freenectDevice_)
01203 {
01204 freenectDevice_->join(true);
01205 delete freenectDevice_;
01206 freenectDevice_ = 0;
01207 }
01208
01209 if(ctx_ && freenect_num_devices(ctx_) > 0)
01210 {
01211
01212 bool hardwareRegistration = true;
01213 stereoModel_ = StereoCameraModel();
01214 if(!calibrationFolder.empty())
01215 {
01216
01217 FreenectDevice dev(ctx_, deviceId_);
01218 if(!dev.init())
01219 {
01220 UERROR("CameraFreenect: Init failed!");
01221 }
01222 std::string calibrationName = dev.getSerial();
01223 if(!cameraName.empty())
01224 {
01225 calibrationName = cameraName;
01226 }
01227 stereoModel_.setName(calibrationName, "depth", "rgb");
01228 hardwareRegistration = !stereoModel_.load(calibrationFolder, calibrationName, false);
01229
01230 if(type_ == kTypeIRDepth)
01231 {
01232 hardwareRegistration = false;
01233 }
01234
01235
01236 if((type_ == kTypeIRDepth && !stereoModel_.left().isValidForRectification()) ||
01237 (type_ == kTypeColorDepth && !stereoModel_.right().isValidForRectification()))
01238 {
01239 UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
01240 calibrationName.c_str(), calibrationFolder.c_str());
01241 }
01242 else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && hardwareRegistration)
01243 {
01244 UWARN("Missing extrinsic calibration file for camera \"%s\" in \"%s\" folder, default registration is used even if rgb is rectified!",
01245 calibrationName.c_str(), calibrationFolder.c_str());
01246 }
01247 else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && !hardwareRegistration)
01248 {
01249 UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
01250 "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
01251 }
01252 }
01253
01254 freenectDevice_ = new FreenectDevice(ctx_, deviceId_, type_==kTypeColorDepth, hardwareRegistration);
01255 if(freenectDevice_->init())
01256 {
01257 freenectDevice_->start();
01258 uSleep(3000);
01259 return true;
01260 }
01261 else
01262 {
01263 UERROR("CameraFreenect: Init failed!");
01264 }
01265 delete freenectDevice_;
01266 freenectDevice_ = 0;
01267 }
01268 else
01269 {
01270 UERROR("CameraFreenect: No devices connected!");
01271 }
01272 #else
01273 UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
01274 #endif
01275 return false;
01276 }
01277
01278 bool CameraFreenect::isCalibrated() const
01279 {
01280 return true;
01281 }
01282
01283 std::string CameraFreenect::getSerial() const
01284 {
01285 #ifdef RTABMAP_FREENECT
01286 if(freenectDevice_)
01287 {
01288 return freenectDevice_->getSerial();
01289 }
01290 #endif
01291 return "";
01292 }
01293
01294 SensorData CameraFreenect::captureImage(CameraInfo * info)
01295 {
01296 SensorData data;
01297 #ifdef RTABMAP_FREENECT
01298 if(ctx_ && freenectDevice_)
01299 {
01300 if(freenectDevice_->isRunning())
01301 {
01302 cv::Mat depth,rgb;
01303 freenectDevice_->getData(rgb, depth);
01304 if(!rgb.empty() && !depth.empty())
01305 {
01306 UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
01307
01308
01309 CameraModel model(
01310 freenectDevice_->getDepthFocal(),
01311 freenectDevice_->getDepthFocal(),
01312 float(rgb.cols/2) - 0.5f,
01313 float(rgb.rows/2) - 0.5f,
01314 this->getLocalTransform(),
01315 0,
01316 rgb.size());
01317
01318 if(type_==kTypeIRDepth)
01319 {
01320 if(stereoModel_.left().isValidForRectification())
01321 {
01322 rgb = stereoModel_.left().rectifyImage(rgb);
01323 depth = stereoModel_.left().rectifyImage(depth, 0);
01324 model = stereoModel_.left();
01325 }
01326 }
01327 else
01328 {
01329 if(stereoModel_.right().isValidForRectification())
01330 {
01331 rgb = stereoModel_.right().rectifyImage(rgb);
01332 model = stereoModel_.right();
01333
01334 if(stereoModel_.left().isValidForRectification() && !stereoModel_.stereoTransform().isNull())
01335 {
01336 depth = stereoModel_.left().rectifyImage(depth, 0);
01337 depth = util2d::registerDepth(depth, stereoModel_.left().K(), rgb.size(), stereoModel_.right().K(), stereoModel_.stereoTransform());
01338 }
01339 }
01340 }
01341 model.setLocalTransform(this->getLocalTransform());
01342
01343 data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
01344 }
01345 }
01346 else
01347 {
01348 UERROR("CameraFreenect: Re-initialization needed!");
01349 delete freenectDevice_;
01350 freenectDevice_ = 0;
01351 }
01352 }
01353 #else
01354 UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
01355 #endif
01356 return data;
01357 }
01358
01359
01360
01361
01362 bool CameraFreenect2::available()
01363 {
01364 #ifdef RTABMAP_FREENECT2
01365 return true;
01366 #else
01367 return false;
01368 #endif
01369 }
01370
01371 CameraFreenect2::CameraFreenect2(
01372 int deviceId,
01373 Type type,
01374 float imageRate,
01375 const Transform & localTransform,
01376 float minDepth,
01377 float maxDepth,
01378 bool bilateralFiltering,
01379 bool edgeAwareFiltering,
01380 bool noiseFiltering,
01381 const std::string & pipelineName) :
01382 Camera(imageRate, localTransform)
01383 #ifdef RTABMAP_FREENECT2
01384 ,
01385 deviceId_(deviceId),
01386 type_(type),
01387 freenect2_(0),
01388 dev_(0),
01389 listener_(0),
01390 reg_(0),
01391 minKinect2Depth_(minDepth),
01392 maxKinect2Depth_(maxDepth),
01393 bilateralFiltering_(bilateralFiltering),
01394 edgeAwareFiltering_(edgeAwareFiltering),
01395 noiseFiltering_(noiseFiltering),
01396 pipelineName_(pipelineName)
01397 #endif
01398 {
01399 #ifdef RTABMAP_FREENECT2
01400 UASSERT(minKinect2Depth_ < maxKinect2Depth_ && minKinect2Depth_>0 && maxKinect2Depth_>0 && maxKinect2Depth_<=65.535f);
01401 freenect2_ = new libfreenect2::Freenect2();
01402 switch(type_)
01403 {
01404 case kTypeColorIR:
01405 listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
01406 break;
01407 case kTypeIRDepth:
01408 listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
01409 break;
01410 case kTypeColor2DepthSD:
01411 case kTypeDepth2ColorHD:
01412 case kTypeDepth2ColorSD:
01413 default:
01414 listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
01415 break;
01416 }
01417 #endif
01418 }
01419
01420 CameraFreenect2::~CameraFreenect2()
01421 {
01422 #ifdef RTABMAP_FREENECT2
01423 UDEBUG("");
01424 if(dev_)
01425 {
01426 dev_->stop();
01427 dev_->close();
01428
01429 }
01430 delete listener_;
01431 delete reg_;
01432 delete freenect2_;
01433 UDEBUG("");
01434 #endif
01435 }
01436
01437 #ifdef RTABMAP_FREENECT2
01438 libfreenect2::PacketPipeline *createPacketPipelineByName(const std::string & name)
01439 {
01440 std::string availablePipelines;
01441 #if defined(LIBFREENECT2_WITH_OPENGL_SUPPORT)
01442 availablePipelines += "gl ";
01443 if (name == "gl")
01444 {
01445 UINFO("Using 'gl' pipeline.");
01446 return new libfreenect2::OpenGLPacketPipeline();
01447 }
01448 #endif
01449 #if defined(LIBFREENECT2_WITH_CUDA_SUPPORT)
01450 availablePipelines += "cuda cudakde ";
01451 if (name == "cuda")
01452 {
01453 UINFO("Using 'cuda' pipeline.");
01454 return new libfreenect2::CudaPacketPipeline();
01455 }
01456 if (name == "cudakde")
01457 {
01458 UINFO("Using 'cudakde' pipeline.");
01459 return new libfreenect2::CudaKdePacketPipeline();
01460 }
01461 #endif
01462 #if defined(LIBFREENECT2_WITH_OPENCL_SUPPORT)
01463 availablePipelines += "cl clkde ";
01464 if (name == "cl")
01465 {
01466 UINFO("Using 'cl' pipeline.");
01467 return new libfreenect2::OpenCLPacketPipeline();
01468 }
01469 if (name == "clkde")
01470 {
01471 UINFO("Using 'clkde' pipeline.");
01472 return new libfreenect2::OpenCLKdePacketPipeline();
01473 }
01474 #endif
01475 availablePipelines += "cpu";
01476 if (name == "cpu")
01477 {
01478 UINFO("Using 'cpu' pipeline.");
01479 return new libfreenect2::CpuPacketPipeline();
01480 }
01481
01482 if (!name.empty())
01483 {
01484 UERROR("'%s' pipeline is not available. Available pipelines are: \"%s\". Default one is used instead (first one in the list).",
01485 name.c_str(), availablePipelines.c_str());
01486 }
01487
01488
01489 #if defined(LIBFREENECT2_WITH_OPENGL_SUPPORT)
01490 UINFO("Using 'gl' pipeline.");
01491 return new libfreenect2::OpenGLPacketPipeline();
01492 #elif defined(LIBFREENECT2_WITH_CUDA_SUPPORT)
01493 UINFO("Using 'cuda' pipeline.");
01494 return new libfreenect2::CudaPacketPipeline();
01495 #elif defined(LIBFREENECT2_WITH_OPENCL_SUPPORT)
01496 UINFO("Using 'cl' pipeline.");
01497 return new libfreenect2::OpenCLPacketPipeline();
01498 #else
01499 UINFO("Using 'cpu' pipeline.");
01500 return new libfreenect2::CpuPacketPipeline();
01501 #endif
01502 }
01503 #endif
01504
01505 bool CameraFreenect2::init(const std::string & calibrationFolder, const std::string & cameraName)
01506 {
01507 #ifdef RTABMAP_FREENECT2
01508 if(dev_)
01509 {
01510 dev_->stop();
01511 dev_->close();
01512 dev_ = 0;
01513 }
01514
01515 if(reg_)
01516 {
01517 delete reg_;
01518 reg_ = 0;
01519 }
01520
01521 libfreenect2::PacketPipeline * pipeline = createPacketPipelineByName(pipelineName_);
01522
01523 if(deviceId_ <= 0)
01524 {
01525 UDEBUG("Opening default device...");
01526 dev_ = freenect2_->openDefaultDevice(pipeline);
01527 pipeline = 0;
01528 }
01529 else
01530 {
01531 UDEBUG("Opening device ID=%d...", deviceId_);
01532 dev_ = freenect2_->openDevice(deviceId_, pipeline);
01533 pipeline = 0;
01534 }
01535
01536 if(dev_)
01537 {
01538
01539
01540
01541
01542
01543 libfreenect2::Freenect2Device::Config config;
01544 config.EnableBilateralFilter = bilateralFiltering_;
01545 config.EnableEdgeAwareFilter = edgeAwareFiltering_;
01546 config.MinDepth = minKinect2Depth_;
01547 config.MaxDepth = maxKinect2Depth_;
01548 dev_->setConfiguration(config);
01549
01550 dev_->setColorFrameListener(listener_);
01551 dev_->setIrAndDepthFrameListener(listener_);
01552
01553 dev_->start();
01554
01555 UINFO("CameraFreenect2: device serial: %s", dev_->getSerialNumber().c_str());
01556 UINFO("CameraFreenect2: device firmware: %s", dev_->getFirmwareVersion().c_str());
01557
01558
01559 libfreenect2::Freenect2Device::IrCameraParams depthParams = dev_->getIrCameraParams();
01560 libfreenect2::Freenect2Device::ColorCameraParams colorParams = dev_->getColorCameraParams();
01561 reg_ = new libfreenect2::Registration(depthParams, colorParams);
01562
01563
01564 stereoModel_ = StereoCameraModel();
01565 if(!calibrationFolder.empty())
01566 {
01567 std::string calibrationName = dev_->getSerialNumber();
01568 if(!cameraName.empty())
01569 {
01570 calibrationName = cameraName;
01571 }
01572 stereoModel_.setName(calibrationName, "depth", "rgb");
01573 if(!stereoModel_.load(calibrationFolder, calibrationName, false))
01574 {
01575 UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration "
01576 "is used. Note that from version 0.11.10, calibration suffixes for Freenect2 driver have "
01577 "changed from \"_left\"->\"_depth\" and \"_right\"->\"_rgb\". You can safely rename "
01578 "the calibration files to avoid recalibrating.",
01579 calibrationName.c_str(), calibrationFolder.c_str());
01580 }
01581 else
01582 {
01583 UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
01584 "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
01585
01586 if(type_==kTypeColor2DepthSD)
01587 {
01588 UWARN("Freenect2: When using custom calibration file, type "
01589 "kTypeColor2DepthSD is not supported. kTypeDepth2ColorSD is used instead...");
01590 type_ = kTypeDepth2ColorSD;
01591 }
01592
01593
01594 cv::Mat colorP = stereoModel_.right().P();
01595 cv::Size colorSize = stereoModel_.right().imageSize();
01596 if(type_ == kTypeDepth2ColorSD)
01597 {
01598 colorP.at<double>(0,0)/=2.0f;
01599 colorP.at<double>(1,1)/=2.0f;
01600 colorP.at<double>(0,2)/=2.0f;
01601 colorP.at<double>(1,2)/=2.0f;
01602 colorSize.width/=2;
01603 colorSize.height/=2;
01604 }
01605 cv::Mat depthP = stereoModel_.left().P();
01606 cv::Size depthSize = stereoModel_.left().imageSize();
01607 float ratioY = float(colorSize.height)/float(depthSize.height);
01608 float ratioX = float(colorSize.width)/float(depthSize.width);
01609 depthP.at<double>(0,0)*=ratioX;
01610 depthP.at<double>(1,1)*=ratioY;
01611 depthP.at<double>(0,2)*=ratioX;
01612 depthP.at<double>(1,2)*=ratioY;
01613 depthSize.width*=ratioX;
01614 depthSize.height*=ratioY;
01615 const CameraModel & l = stereoModel_.left();
01616 const CameraModel & r = stereoModel_.right();
01617 stereoModel_ = StereoCameraModel(stereoModel_.name(),
01618 depthSize, l.K_raw(), l.D_raw(), l.R(), depthP,
01619 colorSize, r.K_raw(), r.D_raw(), r.R(), colorP,
01620 stereoModel_.R(), stereoModel_.T(), stereoModel_.E(), stereoModel_.F());
01621 stereoModel_.initRectificationMap();
01622 }
01623 }
01624
01625 return true;
01626 }
01627 else
01628 {
01629 UERROR("CameraFreenect2: no device connected or failure opening the default one! Note that rtabmap should link on libusb of libfreenect2. "
01630 "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
01631 }
01632 #else
01633 UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
01634 #endif
01635 return false;
01636 }
01637
01638 bool CameraFreenect2::isCalibrated() const
01639 {
01640 return true;
01641 }
01642
01643 std::string CameraFreenect2::getSerial() const
01644 {
01645 #ifdef RTABMAP_FREENECT2
01646 if(dev_)
01647 {
01648 return dev_->getSerialNumber();
01649 }
01650 #endif
01651 return "";
01652 }
01653
01654 SensorData CameraFreenect2::captureImage(CameraInfo * info)
01655 {
01656 SensorData data;
01657 #ifdef RTABMAP_FREENECT2
01658 if(dev_ && listener_)
01659 {
01660 libfreenect2::FrameMap frames;
01661 #ifndef LIBFREENECT2_THREADING_STDLIB
01662 UDEBUG("Waiting for new frames... If it is stalled here, rtabmap should link on libusb of libfreenect2. "
01663 "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
01664 listener_->waitForNewFrame(frames);
01665 #else
01666 if(!listener_->waitForNewFrame(frames, 1000))
01667 {
01668 UWARN("CameraFreenect2: Failed to get frames! rtabmap should link on libusb of libfreenect2. "
01669 "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
01670 }
01671 else
01672 #endif
01673 {
01674 double stamp = UTimer::now();
01675 libfreenect2::Frame *rgbFrame = 0;
01676 libfreenect2::Frame *irFrame = 0;
01677 libfreenect2::Frame *depthFrame = 0;
01678
01679 switch(type_)
01680 {
01681 case kTypeColorIR:
01682 rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
01683 irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
01684 break;
01685 case kTypeIRDepth:
01686 irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
01687 depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
01688 break;
01689 case kTypeColor2DepthSD:
01690 case kTypeDepth2ColorSD:
01691 case kTypeDepth2ColorHD:
01692 case kTypeDepth2ColorHD2:
01693 default:
01694 rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
01695 depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
01696 break;
01697 }
01698
01699 cv::Mat rgb, depth;
01700 float fx=0,fy=0,cx=0,cy=0;
01701 if(irFrame && depthFrame)
01702 {
01703 cv::Mat irMat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data);
01704
01705 float maxIr_ = 0x7FFF;
01706 float minIr_ = 0x0;
01707 const float factor = 255.0f / float((maxIr_ - minIr_));
01708 rgb = cv::Mat(irMat.rows, irMat.cols, CV_8UC1);
01709 for(int i=0; i<irMat.rows; ++i)
01710 {
01711 for(int j=0; j<irMat.cols; ++j)
01712 {
01713 rgb.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(irMat.at<float>(i,j) - minIr_, 0.0f)) * factor, 255.0f);
01714 }
01715 }
01716
01717 cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
01718
01719 cv::flip(rgb, rgb, 1);
01720 cv::flip(depth, depth, 1);
01721
01722 if(stereoModel_.isValidForRectification())
01723 {
01724
01725 rgb = stereoModel_.left().rectifyImage(rgb);
01726 depth = stereoModel_.left().rectifyDepth(depth);
01727 fx = stereoModel_.left().fx();
01728 fy = stereoModel_.left().fy();
01729 cx = stereoModel_.left().cx();
01730 cy = stereoModel_.left().cy();
01731 }
01732 else
01733 {
01734 libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
01735 fx = params.fx;
01736 fy = params.fy;
01737 cx = params.cx;
01738 cy = params.cy;
01739 }
01740 }
01741 else
01742 {
01743
01744 if(stereoModel_.isValidForRectification())
01745 {
01746 cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
01747 cv::Mat rgbMat;
01748 #ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
01749
01750 cv::cvtColor(rgbMatC4, rgbMat, CV_RGBA2BGR);
01751
01752 #else
01753
01754 cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
01755
01756 #endif
01757 cv::flip(rgbMat, rgb, 1);
01758
01759
01760 rgb = stereoModel_.right().rectifyImage(rgb);
01761 if(irFrame)
01762 {
01763
01764 cv::Mat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
01765 cv::flip(depth, depth, 1);
01766 depth = stereoModel_.left().rectifyImage(depth);
01767 }
01768 else
01769 {
01770
01771 cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
01772 cv::flip(depth, depth, 1);
01773
01774
01775 depth = stereoModel_.left().rectifyDepth(depth);
01776
01777 bool registered = true;
01778 if(registered)
01779 {
01780 depth = util2d::registerDepth(
01781 depth,
01782 stereoModel_.left().P().colRange(0,3).rowRange(0,3),
01783 depth.size(),
01784 stereoModel_.right().P().colRange(0,3).rowRange(0,3),
01785 stereoModel_.stereoTransform());
01786 util2d::fillRegisteredDepthHoles(depth, true, false);
01787 fx = stereoModel_.right().fx();
01788 fy = stereoModel_.right().fy();
01789 cx = stereoModel_.right().cx();
01790 cy = stereoModel_.right().cy();
01791 }
01792 else
01793 {
01794 fx = stereoModel_.left().fx();
01795 fy = stereoModel_.left().fy();
01796 cx = stereoModel_.left().cx();
01797 cy = stereoModel_.left().cy();
01798 }
01799 }
01800 }
01801 else
01802 {
01803
01804 if(irFrame)
01805 {
01806 cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
01807 cv::Mat rgbMat;
01808 #ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
01809
01810 cv::cvtColor(rgbMatC4, rgbMat, CV_RGB2BGR);
01811
01812 #else
01813
01814 cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
01815
01816 #endif
01817 cv::flip(rgbMat, rgb, 1);
01818
01819 cv::Mat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
01820 cv::flip(depth, depth, 1);
01821 }
01822 else
01823 {
01824
01825 UASSERT(reg_!=0);
01826
01827 float maxDepth = maxKinect2Depth_*1000.0f;
01828 float minDepth = minKinect2Depth_*1000.0f;
01829 if(type_ == kTypeColor2DepthSD || type_ == kTypeDepth2ColorHD)
01830 {
01831 cv::Mat rgbMatBGRA;
01832 libfreenect2::Frame depthUndistorted(512, 424, 4);
01833 libfreenect2::Frame rgbRegistered(512, 424, 4);
01834
01835
01836 if(noiseFiltering_)
01837 {
01838 cv::Mat depthMat = cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data);
01839 for(int dx=0; dx<depthMat.cols; ++dx)
01840 {
01841 bool onEdgeX = dx==depthMat.cols-1;
01842 for(int dy=0; dy<depthMat.rows; ++dy)
01843 {
01844 bool onEdge = onEdgeX || dy==depthMat.rows-1;
01845 float z = 0.0f;
01846 float & dz = depthMat.at<float>(dy,dx);
01847 if(dz>=minDepth && dz <= maxDepth)
01848 {
01849 z = dz;
01850 if(noiseFiltering_ && !onEdge)
01851 {
01852 z=0;
01853 const float & dz1 = depthMat.at<float>(dy,dx+1);
01854 const float & dz2 = depthMat.at<float>(dy+1,dx);
01855 const float & dz3 = depthMat.at<float>(dy+1,dx+1);
01856 if( dz1>=minDepth && dz1 <= maxDepth &&
01857 dz2>=minDepth && dz2 <= maxDepth &&
01858 dz3>=minDepth && dz3 <= maxDepth)
01859 {
01860 float avg = (dz + dz1 + dz2 + dz3) / 4.0f;
01861 float thres = 0.01f*avg;
01862
01863 if( fabs(dz-avg) < thres &&
01864 fabs(dz1-avg) < thres &&
01865 fabs(dz2-avg) < thres &&
01866 fabs(dz3-avg) < thres)
01867 {
01868 z = dz;
01869 }
01870 }
01871 }
01872 }
01873 dz = z;
01874 }
01875 }
01876 }
01877
01878 libfreenect2::Frame bidDepth(1920, 1082, 4);
01879 reg_->apply(rgbFrame, depthFrame, &depthUndistorted, &rgbRegistered, true, &bidDepth);
01880
01881 cv::Mat depthMat;
01882 if(type_ == kTypeColor2DepthSD)
01883 {
01884 rgbMatBGRA = cv::Mat((int)rgbRegistered.height, (int)rgbRegistered.width, CV_8UC4, rgbRegistered.data);
01885 depthMat = cv::Mat((int)depthUndistorted.height, (int)depthUndistorted.width, CV_32FC1, depthUndistorted.data);
01886
01887
01888 libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
01889 fx = params.fx;
01890 fy = params.fy;
01891 cx = params.cx;
01892 cy = params.cy;
01893 }
01894 else
01895 {
01896 rgbMatBGRA = cv::Mat((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
01897 depthMat = cv::Mat((int)bidDepth.height, (int)bidDepth.width, CV_32FC1, bidDepth.data);
01898 depthMat = depthMat(cv::Range(1, 1081), cv::Range::all());
01899
01900
01901 libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
01902 fx = params.fx;
01903 fy = params.fy;
01904 cx = params.cx;
01905 cy = params.cy;
01906 }
01907
01908
01909 depth = cv::Mat(depthMat.size(), CV_16UC1);
01910 for(int dx=0; dx<depthMat.cols; ++dx)
01911 {
01912 for(int dy=0; dy<depthMat.rows; ++dy)
01913 {
01914 unsigned short z = 0;
01915 const float & dz = depthMat.at<float>(dy,dx);
01916 if(dz>=minDepth && dz <= maxDepth)
01917 {
01918 z = (unsigned short)dz;
01919 }
01920 depth.at<unsigned short>(dy,(depthMat.cols-1)-dx) = z;
01921 }
01922 }
01923
01924
01925 #ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
01926
01927 cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR);
01928
01929 #else
01930
01931 cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
01932
01933 #endif
01934 cv::flip(rgb, rgb, 1);
01935 }
01936 else
01937 {
01938 UASSERT(type_ == kTypeDepth2ColorSD || type_ == kTypeDepth2ColorHD2);
01939 cv::Mat rgbMatBGRA = cv::Mat((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
01940 if(type_ == kTypeDepth2ColorSD)
01941 {
01942 cv::Mat tmp;
01943 cv::resize(rgbMatBGRA, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
01944 rgbMatBGRA = tmp;
01945 }
01946
01947 #ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
01948
01949 cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR);
01950
01951 #else
01952
01953 cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
01954
01955 #endif
01956 cv::flip(rgb, rgb, 1);
01957
01958 cv::Mat depthFrameMat = cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data);
01959 depth = cv::Mat::zeros(rgbMatBGRA.rows, rgbMatBGRA.cols, CV_16U);
01960 for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
01961 {
01962 for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
01963 {
01964 float dz = depthFrameMat.at<float>(dy,dx);
01965 if(dz>=minDepth && dz<=maxDepth)
01966 {
01967 bool goodDepth = true;
01968 if(noiseFiltering_)
01969 {
01970 goodDepth = false;
01971 float dz1 = depthFrameMat.at<float>(dy,dx+1);
01972 float dz2 = depthFrameMat.at<float>(dy+1,dx);
01973 float dz3 = depthFrameMat.at<float>(dy+1,dx+1);
01974 if(dz1>=minDepth && dz1 <= maxDepth &&
01975 dz2>=minDepth && dz2 <= maxDepth &&
01976 dz3>=minDepth && dz3 <= maxDepth)
01977 {
01978 float avg = (dz + dz1 + dz2 + dz3) / 4.0f;
01979 float thres = 0.01 * avg;
01980 if( fabs(dz-avg) < thres &&
01981 fabs(dz1-avg) < thres &&
01982 fabs(dz2-avg) < thres &&
01983 fabs(dz3-avg) < thres)
01984 {
01985 goodDepth = true;
01986 }
01987 }
01988 }
01989 if(goodDepth)
01990 {
01991 float cx=-1,cy=-1;
01992 reg_->apply(dx, dy, dz, cx, cy);
01993 if(type_ == kTypeDepth2ColorSD)
01994 {
01995 cx/=2.0f;
01996 cy/=2.0f;
01997 }
01998 int rcx = cvRound(cx);
01999 int rcy = cvRound(cy);
02000 if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows))
02001 {
02002 unsigned short & zReg = depth.at<unsigned short>(rcy, rcx);
02003 if(zReg == 0 || zReg > (unsigned short)dz)
02004 {
02005 zReg = (unsigned short)dz;
02006 }
02007 }
02008 }
02009 }
02010 }
02011 }
02012 util2d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeDepth2ColorHD2);
02013 util2d::fillRegisteredDepthHoles(depth, type_==kTypeDepth2ColorSD, type_==kTypeDepth2ColorHD2);
02014 cv::flip(depth, depth, 1);
02015 libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
02016 fx = params.fx*(type_==kTypeDepth2ColorSD?0.5:1.0f);
02017 fy = params.fy*(type_==kTypeDepth2ColorSD?0.5:1.0f);
02018 cx = params.cx*(type_==kTypeDepth2ColorSD?0.5:1.0f);
02019 cy = params.cy*(type_==kTypeDepth2ColorSD?0.5:1.0f);
02020 }
02021 }
02022 }
02023 }
02024
02025 CameraModel model;
02026 if(fx && fy)
02027 {
02028 model=CameraModel(
02029 fx,
02030 fy,
02031 cx,
02032 cy,
02033 this->getLocalTransform(),
02034 0,
02035 rgb.size());
02036 }
02037 data = SensorData(rgb, depth, model, this->getNextSeqID(), stamp);
02038
02039 listener_->release(frames);
02040 }
02041 }
02042 #else
02043 UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
02044 #endif
02045 return data;
02046 }
02047
02048
02049
02050
02051
02052 #ifdef RTABMAP_K4W2
02053
02054 template<class Interface>
02055 inline void SafeRelease(Interface *& pInterfaceToRelease)
02056 {
02057 if (pInterfaceToRelease != NULL)
02058 {
02059 pInterfaceToRelease->Release();
02060 pInterfaceToRelease = NULL;
02061 }
02062 }
02063 #endif
02064
02065 bool CameraK4W2::available()
02066 {
02067 #ifdef RTABMAP_K4W2
02068 return true;
02069 #else
02070 return false;
02071 #endif
02072 }
02073
02074 CameraK4W2::CameraK4W2(
02075 int deviceId,
02076 Type type,
02077 float imageRate,
02078 const Transform & localTransform) :
02079 Camera(imageRate, localTransform)
02080 #ifdef RTABMAP_K4W2
02081 ,
02082 type_(type),
02083 pKinectSensor_(NULL),
02084 pCoordinateMapper_(NULL),
02085 pDepthCoordinates_(new DepthSpacePoint[cColorWidth * cColorHeight]),
02086 pColorCoordinates_(new ColorSpacePoint[cDepthWidth * cDepthHeight]),
02087 pMultiSourceFrameReader_(NULL),
02088 pColorRGBX_(new RGBQUAD[cColorWidth * cColorHeight]),
02089 hMSEvent(NULL)
02090 #endif
02091 {
02092 }
02093
02094 CameraK4W2::~CameraK4W2()
02095 {
02096 #ifdef RTABMAP_K4W2
02097 if (pDepthCoordinates_)
02098 {
02099 delete[] pDepthCoordinates_;
02100 pDepthCoordinates_ = NULL;
02101 }
02102
02103 if (pColorCoordinates_)
02104 {
02105 delete[] pColorCoordinates_;
02106 pColorCoordinates_ = NULL;
02107 }
02108
02109 if (pColorRGBX_)
02110 {
02111 delete[] pColorRGBX_;
02112 pColorRGBX_ = NULL;
02113 }
02114
02115 close();
02116 #endif
02117 }
02118
02119 void CameraK4W2::close()
02120 {
02121 #ifdef RTABMAP_K4W2
02122 if (pMultiSourceFrameReader_)
02123 {
02124 pMultiSourceFrameReader_->UnsubscribeMultiSourceFrameArrived(hMSEvent);
02125 CloseHandle((HANDLE)hMSEvent);
02126 hMSEvent = NULL;
02127 }
02128
02129
02130 SafeRelease(pMultiSourceFrameReader_);
02131
02132
02133 SafeRelease(pCoordinateMapper_);
02134
02135
02136 if (pKinectSensor_)
02137 {
02138 pKinectSensor_->Close();
02139 }
02140
02141 SafeRelease(pKinectSensor_);
02142
02143 colorCameraModel_ = CameraModel();
02144 #endif
02145 }
02146
02147 bool CameraK4W2::init(const std::string & calibrationFolder, const std::string & cameraName)
02148 {
02149 #ifdef RTABMAP_K4W2
02150 HRESULT hr;
02151
02152 close();
02153
02154 hr = GetDefaultKinectSensor(&pKinectSensor_);
02155 if (FAILED(hr))
02156 {
02157 return false;
02158 }
02159
02160 if (pKinectSensor_)
02161 {
02162
02163
02164 hr = pKinectSensor_->Open();
02165
02166 if (SUCCEEDED(hr))
02167 {
02168 hr = pKinectSensor_->get_CoordinateMapper(&pCoordinateMapper_);
02169
02170 if (SUCCEEDED(hr))
02171 {
02172 hr = pKinectSensor_->OpenMultiSourceFrameReader(
02173 FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color,
02174 &pMultiSourceFrameReader_);
02175
02176 if (SUCCEEDED(hr))
02177 {
02178 hr = pMultiSourceFrameReader_->SubscribeMultiSourceFrameArrived(&hMSEvent);
02179 }
02180 }
02181 }
02182 }
02183
02184 if (!pKinectSensor_ || FAILED(hr))
02185 {
02186 UERROR("No ready Kinect found!");
02187 close();
02188 return false;
02189 }
02190
02191
02192 uSleep(3000);
02193
02194
02195 CameraIntrinsics intrinsics;
02196 hr = pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
02197 if (SUCCEEDED(hr) && intrinsics.FocalLengthX > 0.0f)
02198 {
02199
02200 CameraModel depthModel(
02201 intrinsics.FocalLengthX,
02202 intrinsics.FocalLengthY,
02203 intrinsics.PrincipalPointX,
02204 intrinsics.PrincipalPointY);
02205
02206 cv::Mat fakeDepth = cv::Mat::ones(cDepthHeight, cDepthWidth, CV_16UC1) * 1000;
02207 hr = pCoordinateMapper_->MapDepthFrameToColorSpace(cDepthWidth * cDepthHeight, (UINT16*)fakeDepth.data, cDepthWidth * cDepthHeight, pColorCoordinates_);
02208 if (SUCCEEDED(hr))
02209 {
02210 int firstIndex = -1;
02211 int lastIndex = -1;
02212 for (int depthIndex = 0; depthIndex < (cDepthWidth*cDepthHeight); ++depthIndex)
02213 {
02214 ColorSpacePoint p = pColorCoordinates_[depthIndex];
02215
02216
02217 if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
02218 {
02219 if (firstIndex == -1)
02220 {
02221 firstIndex = depthIndex;
02222 }
02223 lastIndex = depthIndex;
02224 }
02225 }
02226
02227 UASSERT(firstIndex >= 0 && lastIndex >= 0);
02228 float fx, fy, cx, cy;
02229 float x1, y1, z1, x2, y2, z2;
02230 depthModel.project(firstIndex - (firstIndex / cDepthWidth)*cDepthWidth, firstIndex / cDepthWidth, 1.0f, x1, y1, z1);
02231 depthModel.project(lastIndex - (lastIndex / cDepthWidth)*cDepthWidth, lastIndex / cDepthWidth, 1.0f, x2, y2, z2);
02232 ColorSpacePoint uv1 = pColorCoordinates_[firstIndex];
02233 ColorSpacePoint uv2 = pColorCoordinates_[lastIndex];
02234 fx = ((uv1.X - uv2.X)*z1*z2) / (x1*z2 - x2*z1);
02235 cx = uv1.X - (x1 / z1) * fx;
02236 fy = ((uv1.Y - uv2.Y)*z1*z2) / (y1*z2 - y2*z1);
02237 cy = uv1.Y - (y1 / z1) * fy;
02238
02239 colorCameraModel_ = CameraModel(
02240 fx,
02241 fy,
02242 float(cColorWidth) - cx,
02243 cy,
02244 this->getLocalTransform(),
02245 0,
02246 cv::Size(cColorWidth, cColorHeight));
02247 }
02248 }
02249
02250 if (!colorCameraModel_.isValidForProjection())
02251 {
02252 UERROR("Failed to get camera parameters! Is the camera connected? Try restarting the camera again or use kTypeColor2DepthSD.");
02253 close();
02254 return false;
02255 }
02256
02257 std::string serial = getSerial();
02258 if (!serial.empty())
02259 {
02260 UINFO("Running kinect device \"%s\"", serial.c_str());
02261 }
02262
02263 return true;
02264 #else
02265 UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
02266 return false;
02267 #endif
02268 }
02269
02270 bool CameraK4W2::isCalibrated() const
02271 {
02272 return true;
02273 }
02274
02275 std::string CameraK4W2::getSerial() const
02276 {
02277 #ifdef RTABMAP_K4W2
02278 if (pKinectSensor_)
02279 {
02280 wchar_t uid[255] = { 0 };
02281
02282 HRESULT hr = pKinectSensor_->get_UniqueKinectId(255, uid);
02283 if (SUCCEEDED(hr))
02284 {
02285 std::wstring ws(uid);
02286 return std::string(ws.begin(), ws.end());
02287 }
02288 }
02289 #endif
02290 return "";
02291 }
02292
02293 SensorData CameraK4W2::captureImage(CameraInfo * info)
02294 {
02295 SensorData data;
02296
02297 #ifdef RTABMAP_K4W2
02298
02299 if (!pMultiSourceFrameReader_)
02300 {
02301 return data;
02302 }
02303
02304 HRESULT hr;
02305
02306
02307 HANDLE handles[] = { reinterpret_cast<HANDLE>(hMSEvent) };
02308
02309 double t = UTimer::now();
02310 while((UTimer::now()-t < 5.0) && WaitForMultipleObjects(_countof(handles), handles, false, 5000) == WAIT_OBJECT_0)
02311 {
02312 IMultiSourceFrameArrivedEventArgs* pArgs = NULL;
02313
02314 hr = pMultiSourceFrameReader_->GetMultiSourceFrameArrivedEventData(hMSEvent, &pArgs);
02315 if (SUCCEEDED(hr))
02316 {
02317 IMultiSourceFrameReference * pFrameRef = NULL;
02318 hr = pArgs->get_FrameReference(&pFrameRef);
02319 if (SUCCEEDED(hr))
02320 {
02321 IMultiSourceFrame* pMultiSourceFrame = NULL;
02322 IDepthFrame* pDepthFrame = NULL;
02323 IColorFrame* pColorFrame = NULL;
02324
02325 hr = pFrameRef->AcquireFrame(&pMultiSourceFrame);
02326 if (FAILED(hr))
02327 {
02328 UERROR("Failed getting latest frame.");
02329 }
02330
02331 IDepthFrameReference* pDepthFrameReference = NULL;
02332 hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
02333 if (SUCCEEDED(hr))
02334 {
02335 hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
02336 }
02337 SafeRelease(pDepthFrameReference);
02338
02339 IColorFrameReference* pColorFrameReference = NULL;
02340 hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
02341 if (SUCCEEDED(hr))
02342 {
02343 hr = pColorFrameReference->AcquireFrame(&pColorFrame);
02344 }
02345 SafeRelease(pColorFrameReference);
02346
02347 if (pDepthFrame && pColorFrame)
02348 {
02349 IFrameDescription* pDepthFrameDescription = NULL;
02350 int nDepthWidth = 0;
02351 int nDepthHeight = 0;
02352 UINT nDepthBufferSize = 0;
02353 UINT16 *pDepthBuffer = NULL;
02354
02355 IFrameDescription* pColorFrameDescription = NULL;
02356 int nColorWidth = 0;
02357 int nColorHeight = 0;
02358 ColorImageFormat imageFormat = ColorImageFormat_None;
02359 UINT nColorBufferSize = 0;
02360 RGBQUAD *pColorBuffer = NULL;
02361
02362
02363 if (SUCCEEDED(hr))
02364 hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription);
02365 if (SUCCEEDED(hr))
02366 hr = pDepthFrameDescription->get_Width(&nDepthWidth);
02367 if (SUCCEEDED(hr))
02368 hr = pDepthFrameDescription->get_Height(&nDepthHeight);
02369 if (SUCCEEDED(hr))
02370 hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
02371
02372
02373 if (SUCCEEDED(hr))
02374 hr = pColorFrame->get_FrameDescription(&pColorFrameDescription);
02375 if (SUCCEEDED(hr))
02376 hr = pColorFrameDescription->get_Width(&nColorWidth);
02377 if (SUCCEEDED(hr))
02378 hr = pColorFrameDescription->get_Height(&nColorHeight);
02379 if (SUCCEEDED(hr))
02380 hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
02381 if (SUCCEEDED(hr))
02382 {
02383 if (imageFormat == ColorImageFormat_Bgra)
02384 {
02385 hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
02386 }
02387 else if (pColorRGBX_)
02388 {
02389 pColorBuffer = pColorRGBX_;
02390 nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
02391 hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
02392 }
02393 else
02394 {
02395 hr = E_FAIL;
02396 }
02397 }
02398
02399 if(SUCCEEDED(hr))
02400 {
02401
02402
02403
02404
02405
02406 if (pCoordinateMapper_ &&
02407 pDepthBuffer && (nDepthWidth == cDepthWidth) && (nDepthHeight == cDepthHeight) &&
02408 pColorBuffer && (nColorWidth == cColorWidth) && (nColorHeight == cColorHeight))
02409 {
02410 if (type_ == kTypeColor2DepthSD)
02411 {
02412 HRESULT hr = pCoordinateMapper_->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nColorWidth * nColorHeight, pDepthCoordinates_);
02413 if (SUCCEEDED(hr))
02414 {
02415 cv::Mat depth = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_16UC1);
02416 cv::Mat imageColorRegistered = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_8UC3);
02417
02418 for (int colorIndex = 0; colorIndex < (nColorWidth*nColorHeight); ++colorIndex)
02419 {
02420 DepthSpacePoint p = pDepthCoordinates_[colorIndex];
02421
02422
02423 if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
02424 {
02425
02426
02427 int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
02428 pixel_x_l = nDepthWidth - static_cast<int>(p.X);
02429 pixel_y_l = static_cast<int>(p.Y);
02430 pixel_x_h = pixel_x_l - 1;
02431 pixel_y_h = pixel_y_l + 1;
02432
02433 const RGBQUAD* pSrc = pColorBuffer + colorIndex;
02434 if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
02435 {
02436 unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_l);
02437 ptr[0] = pSrc->rgbBlue;
02438 ptr[1] = pSrc->rgbGreen;
02439 ptr[2] = pSrc->rgbRed;
02440 depth.at<unsigned short>(pixel_y_l, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_l*nDepthWidth);
02441 }
02442 if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
02443 {
02444 unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_l);
02445 ptr[0] = pSrc->rgbBlue;
02446 ptr[1] = pSrc->rgbGreen;
02447 ptr[2] = pSrc->rgbRed;
02448 depth.at<unsigned short>(pixel_y_h, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_h*nDepthWidth);
02449 }
02450 if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
02451 {
02452 unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_h);
02453 ptr[0] = pSrc->rgbBlue;
02454 ptr[1] = pSrc->rgbGreen;
02455 ptr[2] = pSrc->rgbRed;
02456 depth.at<unsigned short>(pixel_y_l, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_l*nDepthWidth);
02457 }
02458 if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
02459 {
02460 unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_h);
02461 ptr[0] = pSrc->rgbBlue;
02462 ptr[1] = pSrc->rgbGreen;
02463 ptr[2] = pSrc->rgbRed;
02464 depth.at<unsigned short>(pixel_y_h, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_h*nDepthWidth);
02465 }
02466 }
02467 }
02468
02469 CameraIntrinsics intrinsics;
02470 pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
02471 CameraModel model(
02472 intrinsics.FocalLengthX,
02473 intrinsics.FocalLengthY,
02474 intrinsics.PrincipalPointX,
02475 intrinsics.PrincipalPointY,
02476 this->getLocalTransform(),
02477 0,
02478 depth.size());
02479 data = SensorData(imageColorRegistered, depth, model, this->getNextSeqID(), UTimer::now());
02480 }
02481 else
02482 {
02483 UERROR("Failed color to depth registration!");
02484 }
02485 }
02486 else
02487 {
02488 HRESULT hr = pCoordinateMapper_->MapDepthFrameToColorSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nDepthWidth * nDepthHeight, pColorCoordinates_);
02489 if (SUCCEEDED(hr))
02490 {
02491 cv::Mat depthSource(nDepthHeight, nDepthWidth, CV_16UC1, pDepthBuffer);
02492 cv::Mat depthRegistered = cv::Mat::zeros(
02493 type_ == kTypeDepth2ColorSD ? nColorHeight/2 : nColorHeight,
02494 type_ == kTypeDepth2ColorSD ? nColorWidth/2 : nColorWidth,
02495 CV_16UC1);
02496 cv::Mat imageColor;
02497 if(type_ == kTypeDepth2ColorSD)
02498 {
02499 cv::Mat tmp;
02500 cv::resize(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
02501 cv::cvtColor(tmp, imageColor, CV_BGRA2BGR);
02502 }
02503 else
02504 {
02505 cv::cvtColor(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), imageColor, CV_BGRA2BGR);
02506 }
02507
02508 for (int depthIndex = 0; depthIndex < (nDepthWidth*nDepthHeight); ++depthIndex)
02509 {
02510 ColorSpacePoint p = pColorCoordinates_[depthIndex];
02511
02512
02513 if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
02514 {
02515 if (type_ == kTypeDepth2ColorSD)
02516 {
02517 p.X /= 2.0f;
02518 p.Y /= 2.0f;
02519 }
02520 const unsigned short & depth_value = depthSource.at<unsigned short>(0, depthIndex);
02521 int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
02522
02523 pixel_x_l = depthRegistered.cols - p.X;
02524 pixel_y_l = p.Y;
02525 pixel_x_h = pixel_x_l - 1;
02526 pixel_y_h = pixel_y_l + 1;
02527
02528 if (pixel_x_l >= 0 && pixel_x_l < depthRegistered.cols &&
02529 pixel_y_l>0 && pixel_y_l < depthRegistered.rows &&
02530 depth_value)
02531 {
02532 unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_l, pixel_x_l);
02533 if (depthPixel == 0 || depthPixel > depth_value)
02534 {
02535 depthPixel = depth_value;
02536 }
02537 }
02538 if (pixel_x_h >= 0 && pixel_x_h < depthRegistered.cols &&
02539 pixel_y_h>0 && pixel_y_h < depthRegistered.rows &&
02540 depth_value)
02541 {
02542 unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_h, pixel_x_h);
02543 if (depthPixel == 0 || depthPixel > depth_value)
02544 {
02545 depthPixel = depth_value;
02546 }
02547 }
02548 }
02549 }
02550
02551 CameraModel model = colorCameraModel_;
02552 if (type_ == kTypeDepth2ColorSD)
02553 {
02554 model = model.scaled(0.5);
02555 }
02556 util2d::fillRegisteredDepthHoles(depthRegistered, true, true, type_ == kTypeDepth2ColorHD);
02557 depthRegistered = rtabmap::util2d::fillDepthHoles(depthRegistered, 1);
02558 cv::flip(imageColor, imageColor, 1);
02559 data = SensorData(imageColor, depthRegistered, model, this->getNextSeqID(), UTimer::now());
02560 }
02561 else
02562 {
02563 UERROR("Failed depth to color registration!");
02564 }
02565 }
02566 }
02567 }
02568
02569 SafeRelease(pDepthFrameDescription);
02570 SafeRelease(pColorFrameDescription);
02571 }
02572
02573 pFrameRef->Release();
02574
02575 SafeRelease(pDepthFrame);
02576 SafeRelease(pColorFrame);
02577 SafeRelease(pMultiSourceFrame);
02578 }
02579 pArgs->Release();
02580 }
02581 if (!data.imageRaw().empty())
02582 {
02583 break;
02584 }
02585 }
02586 #else
02587 UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
02588 #endif
02589 return data;
02590 }
02591
02593
02595 bool CameraRealSense::available()
02596 {
02597 #ifdef RTABMAP_REALSENSE
02598 return true;
02599 #else
02600 return false;
02601 #endif
02602 }
02603
02604 CameraRealSense::CameraRealSense(
02605 int device,
02606 int presetRGB,
02607 int presetDepth,
02608 bool computeOdometry,
02609 float imageRate,
02610 const rtabmap::Transform & localTransform) :
02611 Camera(imageRate, localTransform)
02612 #ifdef RTABMAP_REALSENSE
02613 ,
02614 ctx_(0),
02615 dev_(0),
02616 deviceId_(device),
02617 presetRGB_(presetRGB),
02618 presetDepth_(presetDepth),
02619 computeOdometry_(computeOdometry),
02620 depthScaledToRGBSize_(false),
02621 rgbSource_(kColor),
02622 slam_(0)
02623 #endif
02624 {
02625 UDEBUG("");
02626 }
02627
02628 CameraRealSense::~CameraRealSense()
02629 {
02630 UDEBUG("");
02631 #ifdef RTABMAP_REALSENSE
02632 UDEBUG("");
02633 if(dev_)
02634 {
02635 try
02636 {
02637 if(slam_!=0)
02638 {
02639 dev_->stop(rs::source::all_sources);
02640 }
02641 else
02642 {
02643 dev_->stop();
02644 }
02645 }
02646 catch(const rs::error & error){UWARN("%s", error.what());}
02647 dev_ = 0;
02648 }
02649 UDEBUG("");
02650 if (ctx_)
02651 {
02652 delete ctx_;
02653 }
02654 #ifdef RTABMAP_REALSENSE_SLAM
02655 UDEBUG("");
02656 if(slam_)
02657 {
02658 UScopeMutex lock(slamLock_);
02659 slam_->flush_resources();
02660 delete slam_;
02661 slam_ = 0;
02662 }
02663 #endif
02664 #endif
02665 }
02666
02667 #ifdef RTABMAP_REALSENSE_SLAM
02668 bool setStreamConfigIntrin(
02669 rs::core::stream_type stream,
02670 std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics,
02671 rs::core::video_module_interface::supported_module_config & supported_config,
02672 rs::core::video_module_interface::actual_module_config & actual_config)
02673 {
02674 auto & supported_stream_config = supported_config[stream];
02675 if (!supported_stream_config.is_enabled || supported_stream_config.size.width != intrinsics[stream].width || supported_stream_config.size.height != intrinsics[stream].height)
02676 {
02677 UERROR("size of stream is not supported by slam");
02678 UERROR(" supported: stream %d, width: %d height: %d", (uint32_t) stream, supported_stream_config.size.width, supported_stream_config.size.height);
02679 UERROR(" received: stream %d, width: %d height: %d", (uint32_t) stream, intrinsics[stream].width, intrinsics[stream].height);
02680
02681 return false;
02682 }
02683 rs::core::video_module_interface::actual_image_stream_config &actual_stream_config = actual_config[stream];
02684 actual_config[stream].size.width = intrinsics[stream].width;
02685 actual_config[stream].size.height = intrinsics[stream].height;
02686 actual_stream_config.frame_rate = supported_stream_config.frame_rate;
02687 actual_stream_config.intrinsics = intrinsics[stream];
02688 actual_stream_config.is_enabled = true;
02689 return true;
02690 }
02691 #endif
02692
02693 void CameraRealSense::setDepthScaledToRGBSize(bool enabled) {
02694 #ifdef RTABMAP_REALSENSE
02695 depthScaledToRGBSize_ = enabled;
02696 #endif
02697 }
02698 void CameraRealSense::setRGBSource(RGBSource source)
02699 {
02700 #ifdef RTABMAP_REALSENSE
02701 rgbSource_ = source;
02702 #endif
02703 }
02704
02705 #ifdef RTABMAP_REALSENSE
02706 template<class GET_DEPTH, class TRANSFER_PIXEL> void align_images(const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
02707 {
02708
02709 #pragma omp parallel for schedule(dynamic)
02710 for(int depth_y = 0; depth_y < depth_intrin.height; ++depth_y)
02711 {
02712 int depth_pixel_index = depth_y * depth_intrin.width;
02713 for(int depth_x = 0; depth_x < depth_intrin.width; ++depth_x, ++depth_pixel_index)
02714 {
02715
02716 if(float depth = get_depth(depth_pixel_index))
02717 {
02718
02719 float depth_pixel[2] = {depth_x-0.5f, depth_y-0.5f}, depth_point[3], other_point[3], other_pixel[2];
02720 rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
02721 rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
02722 rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
02723 const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
02724 const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);
02725
02726
02727 depth_pixel[0] = depth_x+0.5f; depth_pixel[1] = depth_y+0.5f;
02728 rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
02729 rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
02730 rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
02731 const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
02732 const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);
02733
02734 if(other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height) continue;
02735
02736
02737 for(int y=other_y0; y<=other_y1; ++y) for(int x=other_x0; x<=other_x1; ++x) transfer_pixel(depth_pixel_index, y * other_intrin.width + x);
02738 }
02739 }
02740 }
02741 }
02742 typedef uint8_t byte;
02743
02744 void align_z_to_other(byte * z_aligned_to_other, const uint16_t * z_pixels, float z_scale, const rs_intrinsics & z_intrin, const rs_extrinsics & z_to_other, const rs_intrinsics & other_intrin)
02745 {
02746 auto out_z = (uint16_t *)(z_aligned_to_other);
02747 align_images(z_intrin, z_to_other, other_intrin,
02748 [z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
02749 [out_z, z_pixels](int z_pixel_index, int other_pixel_index) { out_z[other_pixel_index] = out_z[other_pixel_index] ? std::min(out_z[other_pixel_index],z_pixels[z_pixel_index]) : z_pixels[z_pixel_index]; });
02750 }
02751
02752 void align_disparity_to_other(byte * disparity_aligned_to_other, const uint16_t * disparity_pixels, float disparity_scale, const rs_intrinsics & disparity_intrin, const rs_extrinsics & disparity_to_other, const rs_intrinsics & other_intrin)
02753 {
02754 auto out_disparity = (uint16_t *)(disparity_aligned_to_other);
02755 align_images(disparity_intrin, disparity_to_other, other_intrin,
02756 [disparity_pixels, disparity_scale](int disparity_pixel_index) { return disparity_scale / disparity_pixels[disparity_pixel_index]; },
02757 [out_disparity, disparity_pixels](int disparity_pixel_index, int other_pixel_index) { out_disparity[other_pixel_index] = disparity_pixels[disparity_pixel_index]; });
02758 }
02759
02760 template<int N> struct bytes { char b[N]; };
02761 template<int N, class GET_DEPTH> void align_other_to_depth_bytes(byte * other_aligned_to_depth, GET_DEPTH get_depth, const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, const byte * other_pixels)
02762 {
02763 auto in_other = (const bytes<N> *)(other_pixels);
02764 auto out_other = (bytes<N> *)(other_aligned_to_depth);
02765 align_images(depth_intrin, depth_to_other, other_intrin, get_depth,
02766 [out_other, in_other](int depth_pixel_index, int other_pixel_index) { out_other[depth_pixel_index] = in_other[other_pixel_index]; });
02767 }
02768
02770
02772
02773 std::vector<int> compute_rectification_table(const rs_intrinsics & rect_intrin, const rs_extrinsics & rect_to_unrect, const rs_intrinsics & unrect_intrin)
02774 {
02775 std::vector<int> rectification_table;
02776 rectification_table.resize(rect_intrin.width * rect_intrin.height);
02777 align_images(rect_intrin, rect_to_unrect, unrect_intrin, [](int) { return 1.0f; },
02778 [&rectification_table](int rect_pixel_index, int unrect_pixel_index) { rectification_table[rect_pixel_index] = unrect_pixel_index; });
02779 return rectification_table;
02780 }
02781
02782 template<class T> void rectify_image_pixels(T * rect_pixels, const std::vector<int> & rectification_table, const T * unrect_pixels)
02783 {
02784 for(auto entry : rectification_table) *rect_pixels++ = unrect_pixels[entry];
02785 }
02786
02787 void rectify_image(uint8_t * rect_pixels, const std::vector<int> & rectification_table, const uint8_t * unrect_pixels, rs_format format)
02788 {
02789 switch(format)
02790 {
02791 case RS_FORMAT_Y8:
02792 return rectify_image_pixels((bytes<1> *)rect_pixels, rectification_table, (const bytes<1> *)unrect_pixels);
02793 case RS_FORMAT_Y16: case RS_FORMAT_Z16:
02794 return rectify_image_pixels((bytes<2> *)rect_pixels, rectification_table, (const bytes<2> *)unrect_pixels);
02795 case RS_FORMAT_RGB8: case RS_FORMAT_BGR8:
02796 return rectify_image_pixels((bytes<3> *)rect_pixels, rectification_table, (const bytes<3> *)unrect_pixels);
02797 case RS_FORMAT_RGBA8: case RS_FORMAT_BGRA8:
02798 return rectify_image_pixels((bytes<4> *)rect_pixels, rectification_table, (const bytes<4> *)unrect_pixels);
02799 default:
02800 assert(false);
02801 }
02802 }
02803 #endif
02804
02805 bool CameraRealSense::init(const std::string & calibrationFolder, const std::string & cameraName)
02806 {
02807 UDEBUG("");
02808 #ifdef RTABMAP_REALSENSE
02809
02810 if(dev_)
02811 {
02812 try
02813 {
02814 if(slam_!=0)
02815 {
02816 dev_->stop(rs::source::all_sources);
02817 }
02818 else
02819 {
02820 dev_->stop();
02821 }
02822 }
02823 catch(const rs::error & error){UWARN("%s", error.what());}
02824 dev_ = 0;
02825 }
02826 bufferedFrames_.clear();
02827 rsRectificationTable_.clear();
02828
02829 #ifdef RTABMAP_REALSENSE_SLAM
02830 motionSeq_[0] = motionSeq_[1] = 0;
02831 if(slam_)
02832 {
02833 UScopeMutex lock(slamLock_);
02834 UDEBUG("Flush slam");
02835 slam_->flush_resources();
02836 delete slam_;
02837 slam_ = 0;
02838 }
02839 #endif
02840
02841 if (ctx_ == 0)
02842 {
02843 ctx_ = new rs::context();
02844 }
02845
02846 UDEBUG("");
02847 if (ctx_->get_device_count() == 0)
02848 {
02849 UERROR("No RealSense device detected!");
02850 return false;
02851 }
02852
02853 UDEBUG("");
02854 dev_ = ctx_->get_device(deviceId_);
02855 if (dev_ == 0)
02856 {
02857 UERROR("Cannot connect to device %d", deviceId_);
02858 return false;
02859 }
02860 std::string name = dev_->get_name();
02861 UINFO("Using device %d, an %s", deviceId_, name.c_str());
02862 UINFO(" Serial number: %s", dev_->get_serial());
02863 UINFO(" Firmware version: %s", dev_->get_firmware_version());
02864 UINFO(" Preset RGB: %d", presetRGB_);
02865 UINFO(" Preset Depth: %d", presetDepth_);
02866
02867 #ifndef RTABMAP_REALSENSE_SLAM
02868 computeOdometry_ = false;
02869 #endif
02870
02871 if (name.find("ZR300") == std::string::npos)
02872 {
02873
02874
02875 computeOdometry_ = false;
02876
02877 if(rgbSource_ == kFishEye)
02878 {
02879 UWARN("Fisheye cannot be used with %s camera, using color instead...", name.c_str());
02880 rgbSource_ = kColor;
02881 }
02882 }
02883
02884 rs::intrinsics depth_intrin;
02885 rs::intrinsics fisheye_intrin;
02886 rs::intrinsics color_intrin;
02887
02888 UINFO("Enabling streams...");
02889
02890
02891
02892
02893 try {
02894
02895
02896 if(rgbSource_==kFishEye || computeOdometry_)
02897 {
02898 dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
02899 if(computeOdometry_)
02900 {
02901
02902 dev_->set_option(rs::option::fisheye_strobe, 1);
02903 }
02904
02905 dev_->set_option(rs::option::fisheye_external_trigger, 1);
02906 dev_->set_option(rs::option::fisheye_color_auto_exposure, 1);
02907 fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
02908 UINFO(" Fisheye: %dx%d", fisheye_intrin.width, fisheye_intrin.height);
02909 if(rgbSource_==kFishEye)
02910 {
02911 color_intrin = fisheye_intrin;
02912 }
02913 }
02914 if(rgbSource_!=kFishEye)
02915 {
02916 dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_);
02917 color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color);
02918 UINFO(" RGB: %dx%d", color_intrin.width, color_intrin.height);
02919
02920 if(rgbSource_==kInfrared)
02921 {
02922 dev_->enable_stream(rs::stream::infrared, (rs::preset)presetRGB_);
02923 color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared);
02924 UINFO(" IR left: %dx%d", color_intrin.width, color_intrin.height);
02925 }
02926 }
02927
02928 dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_);
02929 depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
02930 UINFO(" Depth: %dx%d", depth_intrin.width, depth_intrin.height);
02931 }
02932 catch(const rs::error & error)
02933 {
02934 UERROR("Failed starting the streams: %s", error.what());
02935 return false;
02936 }
02937
02938 Transform imu2Camera = Transform::getIdentity();
02939
02940 #ifdef RTABMAP_REALSENSE_SLAM
02941 UDEBUG("Setup frame callback");
02942
02943 std::function<void(rs::frame)> frameCallback = [this](rs::frame frame)
02944 {
02945 if(slam_ != 0)
02946 {
02947 const auto timestampDomain = frame.get_frame_timestamp_domain();
02948 if (rs::timestamp_domain::microcontroller != timestampDomain)
02949 {
02950 UERROR("error: Junk time stamp in stream: %d\twith frame counter: %d",
02951 (int)(frame.get_stream_type()), frame.get_frame_number());
02952 return ;
02953 }
02954 }
02955
02956 int width = frame.get_width();
02957 int height = frame.get_height();
02958 rs::core::correlated_sample_set sample_set = {};
02959
02960 rs::core::image_info info =
02961 {
02962 width,
02963 height,
02964 rs::utils::convert_pixel_format(frame.get_format()),
02965 frame.get_stride()
02966 };
02967 cv::Mat image;
02968 if(frame.get_format() == rs::format::raw8 || frame.get_format() == rs::format::y8)
02969 {
02970 image = cv::Mat(height, width, CV_8UC1, (unsigned char*)frame.get_data());
02971 if(frame.get_stream_type() == rs::stream::fisheye)
02972 {
02973
02974 if(bufferedFrames_.size())
02975 {
02976 bufferedFrames_.rbegin()->second.first = image.clone();
02977 UScopeMutex lock(dataMutex_);
02978 bool notify = lastSyncFrames_.first.empty();
02979 lastSyncFrames_ = bufferedFrames_.rbegin()->second;
02980 if(notify)
02981 {
02982 dataReady_.release();
02983 }
02984 bufferedFrames_.clear();
02985 }
02986 }
02987 else if(frame.get_stream_type() == rs::stream::infrared)
02988 {
02989 if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
02990 {
02991 bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
02992 UScopeMutex lock(dataMutex_);
02993 bool notify = lastSyncFrames_.first.empty();
02994 lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
02995 if(notify)
02996 {
02997 dataReady_.release();
02998 }
02999 bufferedFrames_.erase(frame.get_timestamp());
03000 }
03001 else
03002 {
03003 bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(image.clone(), cv::Mat())));
03004 }
03005 if(bufferedFrames_.size()>5)
03006 {
03007 UWARN("Frames cannot be synchronized!");
03008 bufferedFrames_.clear();
03009 }
03010 return;
03011 }
03012 }
03013 else if(frame.get_format() == rs::format::z16)
03014 {
03015 image = cv::Mat(height, width, CV_16UC1, (unsigned char*)frame.get_data());
03016 if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
03017 {
03018 bufferedFrames_.find(frame.get_timestamp())->second.second = image.clone();
03019 UScopeMutex lock(dataMutex_);
03020 bool notify = lastSyncFrames_.first.empty();
03021 lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
03022 if(notify)
03023 {
03024 dataReady_.release();
03025 }
03026 bufferedFrames_.erase(frame.get_timestamp());
03027 }
03028 else
03029 {
03030 bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(cv::Mat(), image.clone())));
03031 }
03032 if(bufferedFrames_.size()>5)
03033 {
03034 UWARN("Frames cannot be synchronized!");
03035 bufferedFrames_.clear();
03036 }
03037 }
03038 else if(frame.get_format() == rs::format::rgb8)
03039 {
03040 if(rsRectificationTable_.size())
03041 {
03042 image = cv::Mat(height, width, CV_8UC3);
03043 rectify_image(image.data, rsRectificationTable_, (unsigned char*)frame.get_data(), (rs_format)frame.get_format());
03044 }
03045 else
03046 {
03047 image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data());
03048 }
03049 if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
03050 {
03051 bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
03052 UScopeMutex lock(dataMutex_);
03053 bool notify = lastSyncFrames_.first.empty();
03054 lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
03055 if(notify)
03056 {
03057 dataReady_.release();
03058 }
03059 bufferedFrames_.erase(frame.get_timestamp());
03060 }
03061 else
03062 {
03063 bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(image.clone(), cv::Mat())));
03064 }
03065 if(bufferedFrames_.size()>5)
03066 {
03067 UWARN("Frames cannot be synchronized!");
03068 bufferedFrames_.clear();
03069 }
03070 return;
03071 }
03072 else
03073 {
03074 return;
03075 }
03076
03077 if(slam_ != 0)
03078 {
03079 rs::core::stream_type stream = rs::utils::convert_stream_type(frame.get_stream_type());
03080 sample_set[stream] = rs::core::image_interface::create_instance_from_raw_data(
03081 & info,
03082 image.data,
03083 stream,
03084 rs::core::image_interface::flag::any,
03085 frame.get_timestamp(),
03086 (uint64_t)frame.get_frame_number(),
03087 rs::core::timestamp_domain::microcontroller);
03088
03089 UScopeMutex lock(slamLock_);
03090 if (slam_->process_sample_set(sample_set) < rs::core::status_no_error)
03091 {
03092 UERROR("error: failed to process sample");
03093 }
03094 sample_set[stream]->release();
03095 }
03096 };
03097
03098 UDEBUG("");
03099
03100 if(computeOdometry_ || rgbSource_ == kFishEye)
03101 {
03102 dev_->set_frame_callback(rs::stream::fisheye, frameCallback);
03103 }
03104 if(rgbSource_ == kInfrared)
03105 {
03106 dev_->set_frame_callback(rs::stream::infrared, frameCallback);
03107 }
03108 else if(rgbSource_ == kColor)
03109 {
03110 dev_->set_frame_callback(rs::stream::color, frameCallback);
03111 }
03112
03113 dev_->set_frame_callback(rs::stream::depth, frameCallback);
03114
03115 UDEBUG("");
03116
03117 if (computeOdometry_)
03118 {
03119 UDEBUG("Setup motion callback");
03120
03121 std::function<void(rs::motion_data)> motion_callback;
03122 motion_callback = [this](rs::motion_data entry)
03123 {
03124 if ((entry.timestamp_data.source_id != RS_EVENT_IMU_GYRO) &&
03125 (entry.timestamp_data.source_id != RS_EVENT_IMU_ACCEL))
03126 return;
03127
03128 rs_event_source motionType = entry.timestamp_data.source_id;
03129
03130 rs::core::correlated_sample_set sample_set = {};
03131 if (motionType == RS_EVENT_IMU_ACCEL)
03132 {
03133 sample_set[rs::core::motion_type::accel].timestamp = entry.timestamp_data.timestamp;
03134 sample_set[rs::core::motion_type::accel].data[0] = (float)entry.axes[0];
03135 sample_set[rs::core::motion_type::accel].data[1] = (float)entry.axes[1];
03136 sample_set[rs::core::motion_type::accel].data[2] = (float)entry.axes[2];
03137 sample_set[rs::core::motion_type::accel].type = rs::core::motion_type::accel;
03138 ++motionSeq_[0];
03139 sample_set[rs::core::motion_type::accel].frame_number = motionSeq_[0];
03140 }
03141 else if (motionType == RS_EVENT_IMU_GYRO)
03142 {
03143 sample_set[rs::core::motion_type::gyro].timestamp = entry.timestamp_data.timestamp;
03144 sample_set[rs::core::motion_type::gyro].data[0] = (float)entry.axes[0];
03145 sample_set[rs::core::motion_type::gyro].data[1] = (float)entry.axes[1];
03146 sample_set[rs::core::motion_type::gyro].data[2] = (float)entry.axes[2];
03147 sample_set[rs::core::motion_type::gyro].type = rs::core::motion_type::gyro;
03148 ++motionSeq_[1];
03149 sample_set[rs::core::motion_type::gyro].frame_number = motionSeq_[1];
03150 }
03151
03152 UScopeMutex lock(slamLock_);
03153 if (slam_->process_sample_set(sample_set) < rs::core::status_no_error)
03154 {
03155 UERROR("error: failed to process sample");
03156 }
03157 };
03158
03159 std::function<void(rs::timestamp_data)> timestamp_callback;
03160 timestamp_callback = [](rs::timestamp_data entry) {};
03161
03162 dev_->enable_motion_tracking(motion_callback, timestamp_callback);
03163 UINFO(" enabled accel and gyro stream");
03164
03165 rs::motion_intrinsics imuIntrinsics;
03166 rs::extrinsics fisheye2ImuExtrinsics;
03167 rs::extrinsics fisheye2DepthExtrinsics;
03168 try
03169 {
03170 imuIntrinsics = dev_->get_motion_intrinsics();
03171 fisheye2ImuExtrinsics = dev_->get_motion_extrinsics_from(rs::stream::fisheye);
03172 fisheye2DepthExtrinsics = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye);
03173 }
03174 catch (const rs::error & e) {
03175 UERROR("Exception: %s (try to unplug/plug the camera)", e.what());
03176 return false;
03177 }
03178
03179 UDEBUG("Setup SLAM");
03180 UScopeMutex lock(slamLock_);
03181 slam_ = new rs::slam::slam();
03182 slam_->set_auto_occupancy_map_building(false);
03183 slam_->force_relocalization_pose(false);
03184
03185 rs::core::video_module_interface::supported_module_config supported_config = {};
03186 if (slam_->query_supported_module_config(0, supported_config) < rs::core::status_no_error)
03187 {
03188 UERROR("Failed to query the first supported module configuration");
03189 return false;
03190 }
03191
03192 rs::core::video_module_interface::actual_module_config actual_config = {};
03193
03194
03195 std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics;
03196 intrinsics[rs::core::stream_type::fisheye] = rs::utils::convert_intrinsics(fisheye_intrin);
03197 intrinsics[rs::core::stream_type::depth] = rs::utils::convert_intrinsics(depth_intrin);
03198
03199 if(!setStreamConfigIntrin(rs::core::stream_type::fisheye, intrinsics, supported_config, actual_config))
03200 {
03201 return false;
03202 }
03203 if(!setStreamConfigIntrin(rs::core::stream_type::depth, intrinsics, supported_config, actual_config))
03204 {
03205 return false;
03206 }
03207
03208
03209 actual_config[rs::core::motion_type::accel].is_enabled = true;
03210 actual_config[rs::core::motion_type::gyro].is_enabled = true;
03211 actual_config[rs::core::motion_type::gyro].intrinsics = rs::utils::convert_motion_device_intrinsics(imuIntrinsics.gyro);
03212 actual_config[rs::core::motion_type::accel].intrinsics = rs::utils::convert_motion_device_intrinsics(imuIntrinsics.acc);
03213
03214
03215 actual_config[rs::core::stream_type::fisheye].extrinsics_motion = rs::utils::convert_extrinsics(fisheye2ImuExtrinsics);
03216 actual_config[rs::core::stream_type::fisheye].extrinsics = rs::utils::convert_extrinsics(fisheye2DepthExtrinsics);
03217
03218 UDEBUG("Set SLAM config");
03219
03220 if (slam_->set_module_config(actual_config) < rs::core::status_no_error)
03221 {
03222 UERROR("error : failed to set the enabled module configuration");
03223 return false;
03224 }
03225
03226 rs::extrinsics fisheye2imu = dev_->get_motion_extrinsics_from(rs::stream::fisheye);
03227 imu2Camera = Transform(
03228 fisheye2imu.rotation[0], fisheye2imu.rotation[1], fisheye2imu.rotation[2], fisheye2imu.translation[0],
03229 fisheye2imu.rotation[3], fisheye2imu.rotation[4], fisheye2imu.rotation[5], fisheye2imu.translation[1],
03230 fisheye2imu.rotation[6], fisheye2imu.rotation[7], fisheye2imu.rotation[8], fisheye2imu.translation[2]).inverse();
03231
03232 if(rgbSource_ == kInfrared)
03233 {
03234 rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::infrared);
03235 Transform fisheye2Color = Transform(
03236 color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
03237 color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
03238 color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
03239 imu2Camera *= fisheye2Color;
03240 }
03241 else if(rgbSource_ == kColor)
03242 {
03243 rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::rectified_color);
03244 Transform fisheye2Color = Transform(
03245 color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
03246 color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
03247 color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
03248 imu2Camera *= fisheye2Color;
03249 }
03250
03251 UDEBUG("start device!");
03252 try
03253 {
03254 dev_->start(rs::source::all_sources);
03255 }
03256 catch(const rs::error & error)
03257 {
03258 UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
03259 return false;
03260 }
03261 }
03262 else
03263 {
03264 UDEBUG("start device!");
03265 try
03266 {
03267 dev_->start();
03268 }
03269 catch(const rs::error & error)
03270 {
03271 UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
03272 return false;
03273 }
03274 }
03275 #else
03276 try {
03277 dev_->start();
03278 dev_->wait_for_frames();
03279 }
03280 catch (const rs::error & e)
03281 {
03282 UERROR("Exception: %s (try to unplug/plug the camera)", e.what());
03283 }
03284 #endif
03285
03286 cv::Mat D;
03287 if(rgbSource_ == kFishEye)
03288 {
03289
03290 D = cv::Mat::zeros(1,6,CV_64FC1);
03291 D.at<double>(0,0) = color_intrin.coeffs[0];
03292 D.at<double>(0,1) = color_intrin.coeffs[1];
03293 D.at<double>(0,4) = color_intrin.coeffs[2];
03294 D.at<double>(0,5) = color_intrin.coeffs[3];
03295 }
03296 else
03297 {
03298
03299 D = cv::Mat::zeros(1,5,CV_64FC1);
03300 D.at<double>(0,0) = color_intrin.coeffs[0];
03301 D.at<double>(0,1) = color_intrin.coeffs[1];
03302 D.at<double>(0,2) = color_intrin.coeffs[2];
03303 D.at<double>(0,3) = color_intrin.coeffs[3];
03304 D.at<double>(0,4) = color_intrin.coeffs[4];
03305 }
03306 cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
03307 K.at<double>(0,0) = color_intrin.fx;
03308 K.at<double>(1,1) = color_intrin.fy;
03309 K.at<double>(0,2) = color_intrin.ppx;
03310 K.at<double>(1,2) = color_intrin.ppy;
03311 cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
03312 cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
03313 K(cv::Range(0,2), cv::Range(0,3)).copyTo(P(cv::Range(0,2), cv::Range(0,3)));
03314 cameraModel_ = CameraModel(
03315 dev_->get_name(),
03316 cv::Size(color_intrin.width, color_intrin.height),
03317 K,
03318 D,
03319 R,
03320 P,
03321 this->getLocalTransform()*imu2Camera);
03322
03323 UDEBUG("");
03324
03325 if(rgbSource_ == kColor)
03326 {
03327 rs::extrinsics rect_to_unrect = dev_->get_extrinsics(rs::stream::rectified_color, rs::stream::color);
03328 rs::intrinsics unrect_intrin = dev_->get_stream_intrinsics(rs::stream::color);
03329 rsRectificationTable_ = compute_rectification_table(color_intrin, rect_to_unrect, unrect_intrin);
03330 }
03331 else if(rgbSource_ == kFishEye)
03332 {
03333 UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
03334 if(!calibrationFolder.empty() && !cameraName.empty())
03335 {
03336 CameraModel model;
03337 if(!model.load(calibrationFolder, cameraName))
03338 {
03339 UWARN("Failed to load calibration \"%s\" in \"%s\" folder, you should calibrate the camera!",
03340 cameraName.c_str(), calibrationFolder.c_str());
03341 }
03342 else
03343 {
03344 UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
03345 model.fx(),
03346 model.fy(),
03347 model.cx(),
03348 model.cy());
03349 cameraModel_ = model;
03350 cameraModel_.setName(cameraName);
03351 cameraModel_.initRectificationMap();
03352 cameraModel_.setLocalTransform(this->getLocalTransform()*imu2Camera);
03353 }
03354 }
03355 }
03356
03357 uSleep(1000);
03358 UINFO("Enabling streams...done!");
03359
03360 return true;
03361
03362 #else
03363 UERROR("CameraRealSense: RTAB-Map is not built with RealSense support!");
03364 return false;
03365 #endif
03366 }
03367
03368 bool CameraRealSense::isCalibrated() const
03369 {
03370 return true;
03371 }
03372
03373 std::string CameraRealSense::getSerial() const
03374 {
03375 #ifdef RTABMAP_REALSENSE
03376 if (dev_)
03377 {
03378 return dev_->get_serial();
03379 }
03380 else
03381 {
03382 UERROR("Cannot get a serial before initialization. Call init() before.");
03383 }
03384 #endif
03385 return "NA";
03386 }
03387
03388 bool CameraRealSense::odomProvided() const
03389 {
03390 #ifdef RTABMAP_REALSENSE_SLAM
03391 return slam_!=0;
03392 #else
03393 return false;
03394 #endif
03395 }
03396
03397 #ifdef RTABMAP_REALSENSE_SLAM
03398 Transform rsPoseToTransform(const rs::slam::PoseMatrix4f & pose)
03399 {
03400 return Transform(
03401 pose.m_data[0], pose.m_data[1], pose.m_data[2], pose.m_data[3],
03402 pose.m_data[4], pose.m_data[5], pose.m_data[6], pose.m_data[7],
03403 pose.m_data[8], pose.m_data[9], pose.m_data[10], pose.m_data[11]);
03404 }
03405 #endif
03406
03407 SensorData CameraRealSense::captureImage(CameraInfo * info)
03408 {
03409 SensorData data;
03410 #ifdef RTABMAP_REALSENSE
03411 if (dev_)
03412 {
03413 cv::Mat rgb;
03414 cv::Mat depthIn;
03415
03416
03417 rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
03418 rs::extrinsics depth_to_color;
03419 rs::intrinsics color_intrin;
03420 if(rgbSource_ == kFishEye)
03421 {
03422 depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye);
03423 color_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
03424 }
03425 else if(rgbSource_ == kInfrared)
03426 {
03427 depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::infrared);
03428 color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared);
03429 }
03430 else
03431 {
03432 depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::rectified_color);
03433 color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color);
03434 }
03435
03436 #ifdef RTABMAP_REALSENSE_SLAM
03437 if(!dataReady_.acquire(1, 5000))
03438 {
03439 UWARN("Not received new frames since 5 seconds, end of stream reached!");
03440 return data;
03441 }
03442
03443 {
03444 UScopeMutex lock(dataMutex_);
03445 rgb = lastSyncFrames_.first;
03446 depthIn = lastSyncFrames_.second;
03447 lastSyncFrames_.first = cv::Mat();
03448 lastSyncFrames_.second = cv::Mat();
03449 }
03450
03451 if(rgb.empty() || depthIn.empty())
03452 {
03453 return data;
03454 }
03455 #else
03456 try {
03457 dev_->wait_for_frames();
03458 }
03459 catch (const rs::error & e)
03460 {
03461 UERROR("Exception: %s", e.what());
03462 return data;
03463 }
03464
03465
03466 depthIn = cv::Mat(depth_intrin.height, depth_intrin.width, CV_16UC1, (unsigned char*)dev_->get_frame_data(rs::stream::depth));
03467 if(rgbSource_ == kFishEye)
03468 {
03469 rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::fisheye));
03470 }
03471 else if(rgbSource_ == kInfrared)
03472 {
03473 rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::infrared));
03474 }
03475 else
03476 {
03477 rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, (unsigned char*)dev_->get_frame_data(rs::stream::color));
03478 }
03479 #endif
03480
03481
03482 cv::Mat bgr;
03483 if(rgbSource_ != kColor)
03484 {
03485 bgr = rgb;
03486 }
03487 else
03488 {
03489 cv::cvtColor(rgb, bgr, CV_RGB2BGR);
03490 }
03491
03492 bool rectified = false;
03493 if(rgbSource_ == kFishEye && cameraModel_.isRectificationMapInitialized())
03494 {
03495 bgr = cameraModel_.rectifyImage(bgr);
03496 rectified = true;
03497 color_intrin.fx = cameraModel_.fx();
03498 color_intrin.fy = cameraModel_.fy();
03499 color_intrin.ppx = cameraModel_.cx();
03500 color_intrin.ppy = cameraModel_.cy();
03501 UASSERT_MSG(color_intrin.width == cameraModel_.imageWidth() && color_intrin.height == cameraModel_.imageHeight(),
03502 uFormat("color_intrin=%dx%d cameraModel_=%dx%d",
03503 color_intrin.width, color_intrin.height, cameraModel_.imageWidth(), cameraModel_.imageHeight()).c_str());
03504 ((rs_intrinsics*)&color_intrin)->model = RS_DISTORTION_NONE;
03505 }
03506 #ifndef RTABMAP_REALSENSE_SLAM
03507 else if(rgbSource_ != kColor)
03508 {
03509 bgr = bgr.clone();
03510 }
03511 #endif
03512
03513 cv::Mat depth;
03514 if(rgbSource_ != kFishEye || rectified)
03515 {
03516 if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
03517 depth_intrin.width < color_intrin.width &&
03518 depth_intrin.height < color_intrin.height &&
03519 !depthScaledToRGBSize_)
03520 {
03521
03522 depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);
03523 float scaleX = float(depth_intrin.width) / float(color_intrin.width);
03524 float scaleY = float(depth_intrin.height) / float(color_intrin.height);
03525 color_intrin.fx *= scaleX;
03526 color_intrin.fy *= scaleY;
03527 color_intrin.ppx *= scaleX;
03528 color_intrin.ppy *= scaleY;
03529 color_intrin.height = depth_intrin.height;
03530 color_intrin.width = depth_intrin.width;
03531 }
03532 else
03533 {
03534
03535 depth = cv::Mat::zeros(bgr.size(), CV_16UC1);
03536 }
03537
03538 float scale = dev_->get_depth_scale();
03539 for (int dy = 0; dy < depth_intrin.height; ++dy)
03540 {
03541 for (int dx = 0; dx < depth_intrin.width; ++dx)
03542 {
03543
03544 uint16_t depth_value = depthIn.at<unsigned short>(dy,dx);
03545 float depth_in_meters = depth_value * scale;
03546
03547
03548 if (depth_value == 0 || depth_in_meters>10.0f) continue;
03549
03550
03551 int pdx = dx;
03552 int pdy = dy;
03553 if(rgbSource_ == kColor || rgbSource_ == kFishEye)
03554 {
03555 rs::float2 depth_pixel = { (float)dx, (float)dy };
03556 rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
03557 rs::float3 color_point = depth_to_color.transform(depth_point);
03558 rs::float2 color_pixel = color_intrin.project(color_point);
03559
03560 pdx = color_pixel.x;
03561 pdy = color_pixel.y;
03562 }
03563
03564
03565 if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows))
03566 {
03567 depth.at<unsigned short>(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f);
03568 }
03569 }
03570 }
03571
03572 if (color_intrin.width > depth_intrin.width)
03573 {
03574
03575 UTimer time;
03576 util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
03577 util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
03578 UDEBUG("Filling depth holes: %fs", time.ticks());
03579 }
03580 }
03581
03582 if (!bgr.empty() && ((rgbSource_==kFishEye && !rectified) || !depth.empty()))
03583 {
03584 data = SensorData(bgr, depth, cameraModel_, this->getNextSeqID(), UTimer::now());
03585 #ifdef RTABMAP_REALSENSE_SLAM
03586 if(info && slam_)
03587 {
03588 UScopeMutex lock(slamLock_);
03589 rs::slam::PoseMatrix4f pose;
03590 if(slam_->get_camera_pose(pose) == rs::core::status_no_error)
03591 {
03592
03593
03594
03595
03596 {
03597
03598 Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
03599 info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
03600 info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
03601 }
03602
03603
03604
03605
03606 }
03607 else
03608 {
03609 UERROR("Failed getting odometry pose");
03610 }
03611 }
03612 #endif
03613 }
03614 }
03615 else
03616 {
03617 ULOGGER_WARN("The camera must be initialized before requesting an image.");
03618 }
03619 #else
03620 UERROR("CameraRealSense: RTAB-Map is not built with RealSense support!");
03621 #endif
03622 return data;
03623 }
03624
03626
03628 bool CameraRealSense2::available()
03629 {
03630 #ifdef RTABMAP_REALSENSE2
03631 return true;
03632 #else
03633 return false;
03634 #endif
03635 }
03636
03637 CameraRealSense2::CameraRealSense2(
03638 const std::string & device,
03639 float imageRate,
03640 const rtabmap::Transform & localTransform) :
03641 Camera(imageRate, localTransform)
03642 #ifdef RTABMAP_REALSENSE2
03643 ,
03644 ctx_(new rs2::context),
03645 dev_(new rs2::device),
03646 deviceId_(device),
03647 syncer_(new rs2::syncer),
03648 depth_scale_meters_(1.0f),
03649 depthIntrinsics_(new rs2_intrinsics),
03650 rgbIntrinsics_(new rs2_intrinsics),
03651 depthToRGBExtrinsics_(new rs2_extrinsics),
03652 emitterEnabled_(true),
03653 irDepth_(false)
03654 #endif
03655 {
03656 UDEBUG("");
03657 }
03658
03659 CameraRealSense2::~CameraRealSense2()
03660 {
03661 #ifdef RTABMAP_REALSENSE2
03662 delete ctx_;
03663 delete dev_;
03664 delete syncer_;
03665 delete depthIntrinsics_;
03666 delete rgbIntrinsics_;
03667 delete depthToRGBExtrinsics_;
03668 #endif
03669 UDEBUG("");
03670 }
03671
03672 #ifdef RTABMAP_REALSENSE2
03673 void alignFrame(const rs2_intrinsics& from_intrin,
03674 const rs2_intrinsics& other_intrin,
03675 rs2::frame from_image,
03676 uint32_t output_image_bytes_per_pixel,
03677 const rs2_extrinsics& from_to_other,
03678 cv::Mat & registeredDepth,
03679 float depth_scale_meters)
03680 {
03681 static const auto meter_to_mm = 0.001f;
03682 uint8_t* p_out_frame = registeredDepth.data;
03683 auto from_vid_frame = from_image.as<rs2::video_frame>();
03684 auto from_bytes_per_pixel = from_vid_frame.get_bytes_per_pixel();
03685
03686 static const auto blank_color = 0x00;
03687 UASSERT(registeredDepth.total()*registeredDepth.channels()*registeredDepth.depth() == other_intrin.height * other_intrin.width * output_image_bytes_per_pixel);
03688 memset(p_out_frame, blank_color, other_intrin.height * other_intrin.width * output_image_bytes_per_pixel);
03689
03690 auto p_from_frame = reinterpret_cast<const uint8_t*>(from_image.get_data());
03691 auto from_stream_type = from_image.get_profile().stream_type();
03692 float depth_units = ((from_stream_type == RS2_STREAM_DEPTH)? depth_scale_meters:1.f);
03693 UASSERT(from_stream_type == RS2_STREAM_DEPTH);
03694 UASSERT_MSG(depth_units > 0.0f, uFormat("depth_scale_meters=%f", depth_scale_meters).c_str());
03695 #pragma omp parallel for schedule(dynamic)
03696 for (int from_y = 0; from_y < from_intrin.height; ++from_y)
03697 {
03698 int from_pixel_index = from_y * from_intrin.width;
03699 for (int from_x = 0; from_x < from_intrin.width; ++from_x, ++from_pixel_index)
03700 {
03701
03702 float depth = (from_stream_type == RS2_STREAM_DEPTH)?(depth_units * ((const uint16_t*)p_from_frame)[from_pixel_index]): 1.f;
03703 if (depth)
03704 {
03705
03706 float from_pixel[2] = { from_x - 0.5f, from_y - 0.5f }, from_point[3], other_point[3], other_pixel[2];
03707 rs2_deproject_pixel_to_point(from_point, &from_intrin, from_pixel, depth);
03708 rs2_transform_point_to_point(other_point, &from_to_other, from_point);
03709 rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
03710 const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
03711 const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);
03712
03713
03714 from_pixel[0] = from_x + 0.5f; from_pixel[1] = from_y + 0.5f;
03715 rs2_deproject_pixel_to_point(from_point, &from_intrin, from_pixel, depth);
03716 rs2_transform_point_to_point(other_point, &from_to_other, from_point);
03717 rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
03718 const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
03719 const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);
03720
03721 if (other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height)
03722 continue;
03723
03724 for (int y = other_y0; y <= other_y1; ++y)
03725 {
03726 for (int x = other_x0; x <= other_x1; ++x)
03727 {
03728 int out_pixel_index = y * other_intrin.width + x;
03729
03730 for (int i = 0; i < from_bytes_per_pixel; i++)
03731 {
03732 const auto out_offset = out_pixel_index * output_image_bytes_per_pixel + i;
03733 const auto from_offset = from_pixel_index * output_image_bytes_per_pixel + i;
03734 p_out_frame[out_offset] = p_from_frame[from_offset] * (depth_units / meter_to_mm);
03735 }
03736 }
03737 }
03738 }
03739 }
03740 }
03741 }
03742 #endif
03743
03744 bool CameraRealSense2::init(const std::string & calibrationFolder, const std::string & cameraName)
03745 {
03746 UDEBUG("");
03747 #ifdef RTABMAP_REALSENSE2
03748
03749 UINFO("setupDevice...");
03750
03751 auto list = ctx_->query_devices();
03752 if (0 == list.size())
03753 {
03754 UERROR("No RealSense2 devices were found!");
03755 return false;
03756 }
03757
03758 bool found=false;
03759 for (auto&& dev : list)
03760 {
03761 auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
03762 auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
03763 uint16_t pid;
03764 std::stringstream ss;
03765 ss << std::hex << pid_str;
03766 ss >> pid;
03767 UINFO("Device with serial number %s was found with product ID=%d.", sn, (int)pid);
03768 if (deviceId_.empty() || deviceId_ == sn)
03769 {
03770 *dev_ = dev;
03771 found=true;
03772 break;
03773 }
03774 }
03775
03776 if (!found)
03777 {
03778 UERROR("The requested device %s is NOT found!", deviceId_.c_str());
03779 return false;
03780 }
03781
03782 ctx_->set_devices_changed_callback([this](rs2::event_information& info)
03783 {
03784 if (info.was_removed(*dev_))
03785 {
03786 UERROR("The device has been disconnected!");
03787 }
03788 });
03789
03790
03791 auto camera_name = dev_->get_info(RS2_CAMERA_INFO_NAME);
03792 UINFO("Device Name: %s", camera_name);
03793
03794 auto sn = dev_->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
03795 UINFO("Device Serial No: %s", sn);
03796
03797 auto fw_ver = dev_->get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION);
03798 UINFO("Device FW version: %s", fw_ver);
03799
03800 auto pid = dev_->get_info(RS2_CAMERA_INFO_PRODUCT_ID);
03801 UINFO("Device Product ID: 0x%s", pid);
03802
03803 auto dev_sensors = dev_->query_sensors();
03804
03805 UINFO("Device Sensors: ");
03806 std::vector<rs2::sensor> sensors(2);
03807 for(auto&& elem : dev_sensors)
03808 {
03809 std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME);
03810 if ("Stereo Module" == module_name)
03811 {
03812 sensors[1] = elem;
03813 sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
03814 if(irDepth_)
03815 {
03816 sensors[0] = elem;
03817 }
03818 }
03819 else if ("Coded-Light Depth Sensor" == module_name)
03820 {
03821 }
03822 else if ("RGB Camera" == module_name)
03823 {
03824 if(!irDepth_)
03825 {
03826 sensors[0] = elem;
03827 }
03828 }
03829 else if ("Wide FOV Camera" == module_name)
03830 {
03831 }
03832 else if ("Motion Module" == module_name)
03833 {
03834 }
03835 else
03836 {
03837 UERROR("Module Name \"%s\" isn't supported by LibRealSense!", module_name.c_str());
03838 return false;
03839 }
03840 UINFO("%s was found.", elem.get_info(RS2_CAMERA_INFO_NAME));
03841 }
03842
03843 UDEBUG("");
03844
03845 model_ = CameraModel();
03846 rs2::stream_profile depthStreamProfile;
03847 rs2::stream_profile rgbStreamProfile;
03848 std::vector<std::vector<rs2::stream_profile> > profilesPerSensor(2);
03849 for (unsigned int i=0; i<sensors.size(); ++i)
03850 {
03851 UDEBUG("i=%d", (int)i);
03852 auto profiles = sensors[i].get_stream_profiles();
03853 bool added = false;
03854 UDEBUG("profiles=%d", (int)profiles.size());
03855 for (auto& profile : profiles)
03856 {
03857 auto video_profile = profile.as<rs2::video_stream_profile>();
03858 if (video_profile.format() == (i==1?RS2_FORMAT_Z16:irDepth_?RS2_FORMAT_Y8:RS2_FORMAT_RGB8) &&
03859 video_profile.width() == 640 &&
03860 video_profile.height() == 480 &&
03861 video_profile.fps() == 30)
03862 {
03863 profilesPerSensor[irDepth_?1:i].push_back(profile);
03864 auto intrinsic = video_profile.get_intrinsics();
03865 if(i==1)
03866 {
03867 depthBuffer_ = cv::Mat(cv::Size(640, 480), CV_16UC1, cv::Scalar(0));
03868 depthStreamProfile = profile;
03869 *depthIntrinsics_ = intrinsic;
03870 }
03871 else
03872 {
03873 rgbBuffer_ = cv::Mat(cv::Size(640, 480), irDepth_?CV_8UC1:CV_8UC3, irDepth_?cv::Scalar(0):cv::Scalar(0, 0, 0));
03874 model_ = CameraModel(camera_name, intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy, this->getLocalTransform(), 0, cv::Size(intrinsic.width, intrinsic.height));
03875 rgbStreamProfile = profile;
03876 *rgbIntrinsics_ = intrinsic;
03877 }
03878 added = true;
03879 break;
03880 }
03881 }
03882 if (!added)
03883 {
03884 UERROR("Given stream configuration is not supported by the device! "
03885 "Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, 640, 480, 30);
03886 return false;
03887 }
03888 }
03889
03890 if(!model_.isValidForProjection())
03891 {
03892 UERROR("Calibration info not valid!");
03893 return false;
03894 }
03895 *depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
03896
03897 for (unsigned int i=0; i<sensors.size(); ++i)
03898 {
03899 if(profilesPerSensor[i].size())
03900 {
03901 UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
03902 sensors[i].open(profilesPerSensor[i]);
03903 if(i ==1)
03904 {
03905 auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
03906 depth_scale_meters_ = depth_sensor.get_depth_scale();
03907 }
03908 sensors[i].start(*syncer_);
03909 }
03910 }
03911
03912 uSleep(1000);
03913 UINFO("Enabling streams...done!");
03914
03915 return true;
03916
03917 #else
03918 UERROR("CameraRealSense: RTAB-Map is not built with RealSense2 support!");
03919 return false;
03920 #endif
03921 }
03922
03923 bool CameraRealSense2::isCalibrated() const
03924 {
03925 return true;
03926 }
03927
03928 std::string CameraRealSense2::getSerial() const
03929 {
03930 #ifdef RTABMAP_REALSENSE2
03931 return dev_->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
03932 #endif
03933 return "NA";
03934 }
03935
03936 void CameraRealSense2::setEmitterEnabled(bool enabled)
03937 {
03938 #ifdef RTABMAP_REALSENSE2
03939 emitterEnabled_ = enabled;
03940 #endif
03941 }
03942
03943 void CameraRealSense2::setIRDepthFormat(bool enabled)
03944 {
03945 #ifdef RTABMAP_REALSENSE2
03946 irDepth_ = enabled;
03947 #endif
03948 }
03949
03950 SensorData CameraRealSense2::captureImage(CameraInfo * info)
03951 {
03952 SensorData data;
03953 #ifdef RTABMAP_REALSENSE2
03954
03955 try{
03956 auto frameset = syncer_->wait_for_frames(5000);
03957 UTimer timer;
03958 while (frameset.size() != 2 && timer.elapsed() < 2.0)
03959 {
03960
03961 frameset = syncer_->wait_for_frames(100);
03962 }
03963 if (frameset.size() == 2)
03964 {
03965 double stamp = UTimer::now();
03966 UDEBUG("Frameset arrived.");
03967 bool is_rgb_arrived = false;
03968 bool is_depth_arrived = false;
03969 rs2::frame rgb_frame;
03970 rs2::frame depth_frame;
03971 for (auto it = frameset.begin(); it != frameset.end(); ++it)
03972 {
03973 auto f = (*it);
03974 auto stream_type = f.get_profile().stream_type();
03975 if (stream_type == RS2_STREAM_COLOR || stream_type == RS2_STREAM_INFRARED)
03976 {
03977 rgb_frame = f;
03978 is_rgb_arrived = true;
03979 }
03980 else if (stream_type == RS2_STREAM_DEPTH)
03981 {
03982 depth_frame = f;
03983 is_depth_arrived = true;
03984 }
03985 }
03986
03987 if(is_rgb_arrived && is_depth_arrived)
03988 {
03989 auto from_image_frame = depth_frame.as<rs2::video_frame>();
03990 cv::Mat depth;
03991 if(irDepth_)
03992 {
03993 depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
03994 }
03995 else
03996 {
03997 depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type());
03998 alignFrame(*depthIntrinsics_, *rgbIntrinsics_,
03999 depth_frame, from_image_frame.get_bytes_per_pixel(),
04000 *depthToRGBExtrinsics_, depth, depth_scale_meters_);
04001 }
04002
04003 cv::Mat rgb = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data());
04004 cv::Mat bgr;
04005 if(rgb.channels() == 3)
04006 {
04007 cv::cvtColor(rgb, bgr, CV_RGB2BGR);
04008 }
04009 else
04010 {
04011 bgr = rgb.clone();
04012 }
04013
04014 data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
04015 }
04016 else
04017 {
04018 UERROR("Not received depth and rgb");
04019 }
04020 }
04021 else
04022 {
04023 UERROR("Missing frames (received %d)", (int)frameset.size());
04024 }
04025 }
04026 catch(const std::exception& ex)
04027 {
04028 UERROR("An error has occurred during frame callback: %s", ex.what());
04029 }
04030 #else
04031 UERROR("CameraRealSense2: RTAB-Map is not built with RealSense2 support!");
04032 #endif
04033 return data;
04034 }
04035
04036
04037
04038
04039 bool CameraRGBDImages::available()
04040 {
04041 return true;
04042 }
04043
04044 CameraRGBDImages::CameraRGBDImages(
04045 const std::string & pathRGBImages,
04046 const std::string & pathDepthImages,
04047 float depthScaleFactor,
04048 float imageRate,
04049 const Transform & localTransform) :
04050 CameraImages(pathRGBImages, imageRate, localTransform)
04051 {
04052 UASSERT(depthScaleFactor >= 1.0);
04053 cameraDepth_.setPath(pathDepthImages);
04054 cameraDepth_.setDepth(true, depthScaleFactor);
04055 }
04056
04057 CameraRGBDImages::~CameraRGBDImages()
04058 {
04059 }
04060
04061 bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::string & cameraName)
04062 {
04063 bool success = false;
04064 if(CameraImages::init(calibrationFolder, cameraName) && cameraDepth_.init())
04065 {
04066 if(this->imagesCount() == cameraDepth_.imagesCount())
04067 {
04068 success = true;
04069 }
04070 else
04071 {
04072 UERROR("Cameras don't have the same number of images (%d vs %d)",
04073 this->imagesCount(), cameraDepth_.imagesCount());
04074 }
04075 }
04076
04077 return success;
04078 }
04079
04080 bool CameraRGBDImages::isCalibrated() const
04081 {
04082 return this->cameraModel().isValidForProjection();
04083 }
04084
04085 std::string CameraRGBDImages::getSerial() const
04086 {
04087 return this->cameraModel().name();
04088 }
04089
04090 SensorData CameraRGBDImages::captureImage(CameraInfo * info)
04091 {
04092 SensorData data;
04093
04094 SensorData rgb, depth;
04095 rgb = CameraImages::captureImage(info);
04096 if(!rgb.imageRaw().empty())
04097 {
04098 depth = cameraDepth_.takeImage();
04099 if(!depth.depthRaw().empty())
04100 {
04101 data = SensorData(rgb.imageRaw(), depth.depthRaw(), rgb.cameraModels(), rgb.id(), rgb.stamp());
04102 data.setGroundTruth(rgb.groundTruth());
04103 }
04104 }
04105 return data;
04106 }
04107
04108
04109 }