00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/core/CameraRGBD.h"
00029 #include "rtabmap/core/util3d.h"
00030
00031 #include <rtabmap/utilite/UEventsManager.h>
00032 #include <rtabmap/utilite/UConversion.h>
00033 #include <rtabmap/utilite/UStl.h>
00034 #include <rtabmap/utilite/UConversion.h>
00035 #include <rtabmap/utilite/UFile.h>
00036 #include <rtabmap/utilite/UDirectory.h>
00037 #include <rtabmap/utilite/UTimer.h>
00038 #include <rtabmap/utilite/UMath.h>
00039
00040 #include <pcl/io/openni_grabber.h>
00041 #include <pcl/io/oni_grabber.h>
00042
00043 #ifdef WITH_FREENECT
00044 #include <libfreenect.h>
00045 #ifdef FREENECT_DASH_INCLUDES
00046 #include <libfreenect-registration.h>
00047 #else
00048 #include <libfreenect_registration.h>
00049 #endif
00050 #endif
00051
00052 #ifdef WITH_FREENECT2
00053 #include <libfreenect2/libfreenect2.hpp>
00054 #include <libfreenect2/frame_listener_impl.h>
00055 #include <libfreenect2/registration.h>
00056 #include <libfreenect2/packet_pipeline.h>
00057 #endif
00058
00059 #ifdef WITH_DC1394
00060 #include <dc1394/dc1394.h>
00061 #endif
00062
00063 #ifdef WITH_OPENNI2
00064 #include <OniVersion.h>
00065 #include <OpenNI.h>
00066 #endif
00067
00068 #ifdef WITH_FLYCAPTURE2
00069 #include <triclops.h>
00070 #include <fc2triclops.h>
00071 #endif
00072
00073 namespace rtabmap
00074 {
00075
00076 CameraRGBD::CameraRGBD(float imageRate, const Transform & localTransform) :
00077 _imageRate(imageRate),
00078 _localTransform(localTransform),
00079 _mirroring(false),
00080 _colorOnly(false),
00081 _frameRateTimer(new UTimer())
00082 {
00083 }
00084
00085 CameraRGBD::~CameraRGBD()
00086 {
00087 if(_frameRateTimer)
00088 {
00089 delete _frameRateTimer;
00090 }
00091 }
00092
00093 void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
00094 {
00095 bool warnFrameRateTooHigh = false;
00096 float actualFrameRate = 0;
00097 if(_imageRate>0)
00098 {
00099 int sleepTime = (1000.0f/_imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
00100 if(sleepTime > 2)
00101 {
00102 uSleep(sleepTime-2);
00103 }
00104 else if(sleepTime < 0)
00105 {
00106 warnFrameRateTooHigh = true;
00107 actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime());
00108 }
00109
00110
00111 while(_frameRateTimer->getElapsedTime() < 1.0/double(_imageRate)-0.000001)
00112 {
00113
00114 }
00115
00116 double slept = _frameRateTimer->getElapsedTime();
00117 _frameRateTimer->start();
00118 UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_imageRate));
00119 }
00120
00121 UTimer timer;
00122 this->captureImage(rgb, depth, fx, fy, cx, cy);
00123 if(_colorOnly)
00124 {
00125 depth = cv::Mat();
00126 }
00127 if(_mirroring)
00128 {
00129 if(!rgb.empty())
00130 {
00131 cv::flip(rgb,rgb,1);
00132 if(cx != 0.0f)
00133 {
00134 cx = float(rgb.cols) - cx;
00135 }
00136 }
00137 if(!depth.empty())
00138 {
00139 cv::flip(depth,depth,1);
00140 }
00141 }
00142 if(warnFrameRateTooHigh)
00143 {
00144 UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
00145 _imageRate, actualFrameRate, timer.ticks());
00146 }
00147 else
00148 {
00149 UDEBUG("Time capturing image = %fs", timer.ticks());
00150 }
00151 }
00152
00154
00156 CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform) :
00157 CameraRGBD(imageRate, localTransform),
00158 interface_(0),
00159 deviceId_(deviceId),
00160 depthConstant_(0.0f)
00161 {
00162 }
00163
00164 CameraOpenni::~CameraOpenni()
00165 {
00166 UDEBUG("");
00167 if(connection_.connected())
00168 {
00169 connection_.disconnect();
00170 }
00171
00172 if(interface_)
00173 {
00174 interface_->stop();
00175 uSleep(1000);
00176 delete interface_;
00177 interface_ = 0;
00178 }
00179 }
00180
00181 void CameraOpenni::image_cb (
00182 const boost::shared_ptr<openni_wrapper::Image>& rgb,
00183 const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
00184 float constant)
00185 {
00186 UScopeMutex s(dataMutex_);
00187
00188 bool notify = rgb_.empty();
00189
00190 cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
00191 rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
00192 cv::cvtColor(rgbFrame, rgb_, CV_RGB2BGR);
00193
00194 depth_ = cv::Mat(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
00195 depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depth_.data);
00196
00197 depthConstant_ = constant;
00198
00199 if(notify)
00200 {
00201 dataReady_.release();
00202 }
00203 }
00204
00205 bool CameraOpenni::init(const std::string & calibrationFolder)
00206 {
00207 if(interface_)
00208 {
00209 interface_->stop();
00210 uSleep(100);
00211 delete interface_;
00212 interface_ = 0;
00213 }
00214
00215 try
00216 {
00217 if(UFile::getExtension(deviceId_).compare("oni") == 0)
00218 {
00219 interface_ = new pcl::ONIGrabber(deviceId_, false, true);
00220 }
00221 else
00222 {
00223 interface_ = new pcl::OpenNIGrabber(deviceId_);
00224 }
00225
00226 boost::function<void (
00227 const boost::shared_ptr<openni_wrapper::Image>&,
00228 const boost::shared_ptr<openni_wrapper::DepthImage>&,
00229 float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
00230 connection_ = interface_->registerCallback (f);
00231
00232 interface_->start ();
00233 }
00234 catch(const pcl::IOException& ex)
00235 {
00236 UERROR("OpenNI exception: %s", ex.what());
00237 if(interface_)
00238 {
00239 delete interface_;
00240 interface_ = 0;
00241 }
00242 return false;
00243 }
00244 return true;
00245 }
00246
00247 bool CameraOpenni::isCalibrated() const
00248 {
00249 return true;
00250 }
00251
00252 std::string CameraOpenni::getSerial() const
00253 {
00254 if(interface_)
00255 {
00256 return interface_->getName();
00257 }
00258 return "";
00259 }
00260
00261 void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
00262 {
00263 rgb = cv::Mat();
00264 depth = cv::Mat();
00265 fx=0.0f;
00266 fy=0.0f;
00267 cx=0.0f;
00268 cy=0.0f;
00269 if(interface_ && interface_->isRunning())
00270 {
00271 if(!dataReady_.acquire(1, 2000))
00272 {
00273 UWARN("Not received new frames since 2 seconds, end of stream reached!");
00274 }
00275 else
00276 {
00277 UScopeMutex s(dataMutex_);
00278 if(depthConstant_)
00279 {
00280 depth = depth_;
00281 rgb = rgb_;
00282 fx = 1.0f/depthConstant_;
00283 fy = 1.0f/depthConstant_;
00284 cx = float(depth_.cols/2) - 0.5f;
00285 cy = float(depth_.rows/2) - 0.5f;
00286 }
00287
00288 depth_ = cv::Mat();
00289 rgb_ = cv::Mat();
00290 depthConstant_ = 0.0f;
00291 }
00292 }
00293 }
00294
00295
00296
00298
00300 bool CameraOpenNICV::available()
00301 {
00302 return cv::getBuildInformation().find("OpenNI: YES") != std::string::npos;
00303 }
00304
00305 CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform) :
00306 CameraRGBD(imageRate, localTransform),
00307 _asus(asus),
00308 _depthFocal(0.0f)
00309 {
00310
00311 }
00312
00313 CameraOpenNICV::~CameraOpenNICV()
00314 {
00315 _capture.release();
00316 }
00317
00318 bool CameraOpenNICV::init(const std::string & calibrationFolder)
00319 {
00320 if(_capture.isOpened())
00321 {
00322 _capture.release();
00323 }
00324
00325 ULOGGER_DEBUG("CameraRGBD::init()");
00326 _capture.open( _asus?CV_CAP_OPENNI_ASUS:CV_CAP_OPENNI );
00327 if(_capture.isOpened())
00328 {
00329 _capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ );
00330 _depthFocal = _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH );
00331
00332 UINFO("Depth generator output mode:");
00333 UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
00334 UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
00335 UINFO("FRAME_MAX_DEPTH %f mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
00336 UINFO("BASELINE %f mm", _capture.get( CV_CAP_PROP_OPENNI_BASELINE ));
00337 UINFO("FPS %f", _capture.get( CV_CAP_PROP_FPS ));
00338 UINFO("Focal %f", _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH ));
00339 UINFO("REGISTRATION %f", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
00340 if(_capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) == 0.0)
00341 {
00342 UERROR("Depth registration is not activated on this device!");
00343 }
00344 if( _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
00345 {
00346 UINFO("Image generator output mode:");
00347 UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ));
00348 UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ));
00349 UINFO("FPS %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FPS ));
00350 }
00351 else
00352 {
00353 UERROR("CameraRGBD: Device doesn't contain image generator.");
00354 _capture.release();
00355 return false;
00356 }
00357 }
00358 else
00359 {
00360 ULOGGER_ERROR("CameraRGBD: Failed to create a capture object!");
00361 _capture.release();
00362 return false;
00363 }
00364 return true;
00365 }
00366
00367 bool CameraOpenNICV::isCalibrated() const
00368 {
00369 return true;
00370 }
00371
00372 void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
00373 {
00374 if(_capture.isOpened())
00375 {
00376 _capture.grab();
00377 _capture.retrieve( depth, CV_CAP_OPENNI_DEPTH_MAP );
00378 _capture.retrieve( rgb, CV_CAP_OPENNI_BGR_IMAGE );
00379
00380 depth = depth.clone();
00381 rgb = rgb.clone();
00382 UASSERT(_depthFocal > 0.0f);
00383 fx = _depthFocal;
00384 fy = _depthFocal;
00385 cx = float(depth.cols/2) - 0.5f;
00386 cy = float(depth.rows/2) - 0.5f;
00387 }
00388 else
00389 {
00390 ULOGGER_WARN("The camera must be initialized before requesting an image.");
00391 }
00392 }
00393
00394
00396
00398 bool CameraOpenNI2::available()
00399 {
00400 #ifdef WITH_OPENNI2
00401 return true;
00402 #else
00403 return false;
00404 #endif
00405 }
00406
00407 bool CameraOpenNI2::exposureGainAvailable()
00408 {
00409 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
00410 return true;
00411 #else
00412 return false;
00413 #endif
00414 }
00415
00416 CameraOpenNI2::CameraOpenNI2(
00417 const std::string & deviceId,
00418 float imageRate,
00419 const rtabmap::Transform & localTransform) :
00420 CameraRGBD(imageRate, localTransform),
00421 #ifdef WITH_OPENNI2
00422 _device(new openni::Device()),
00423 _color(new openni::VideoStream()),
00424 _depth(new openni::VideoStream()),
00425 #else
00426 _device(0),
00427 _color(0),
00428 _depth(0),
00429 #endif
00430 _depthFx(0.0f),
00431 _depthFy(0.0f),
00432 _deviceId(deviceId)
00433 {
00434 }
00435
00436 CameraOpenNI2::~CameraOpenNI2()
00437 {
00438 #ifdef WITH_OPENNI2
00439 _color->stop();
00440 _color->destroy();
00441 _depth->stop();
00442 _depth->destroy();
00443 _device->close();
00444 openni::OpenNI::shutdown();
00445
00446 delete _device;
00447 delete _color;
00448 delete _depth;
00449 #endif
00450 }
00451
00452 bool CameraOpenNI2::setAutoWhiteBalance(bool enabled)
00453 {
00454 #ifdef WITH_OPENNI2
00455 if(_color && _color->getCameraSettings())
00456 {
00457 return _color->getCameraSettings()->setAutoWhiteBalanceEnabled(enabled) == openni::STATUS_OK;
00458 }
00459 #else
00460 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00461 #endif
00462 return false;
00463 }
00464
00465 bool CameraOpenNI2::setAutoExposure(bool enabled)
00466 {
00467 #ifdef WITH_OPENNI2
00468 if(_color && _color->getCameraSettings())
00469 {
00470 return _color->getCameraSettings()->setAutoExposureEnabled(enabled) == openni::STATUS_OK;
00471 }
00472 #else
00473 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00474 #endif
00475 return false;
00476 }
00477
00478 bool CameraOpenNI2::setExposure(int value)
00479 {
00480 #ifdef WITH_OPENNI2
00481 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
00482 if(_color && _color->getCameraSettings())
00483 {
00484 return _color->getCameraSettings()->setExposure(value) == openni::STATUS_OK;
00485 }
00486 #else
00487 UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
00488 #endif
00489 #else
00490 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00491 #endif
00492 return false;
00493 }
00494
00495 bool CameraOpenNI2::setGain(int value)
00496 {
00497 #ifdef WITH_OPENNI2
00498 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
00499 if(_color && _color->getCameraSettings())
00500 {
00501 return _color->getCameraSettings()->setGain(value) == openni::STATUS_OK;
00502 }
00503 #else
00504 UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
00505 #endif
00506 #else
00507 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00508 #endif
00509 return false;
00510 }
00511
00512 bool CameraOpenNI2::setMirroring(bool enabled)
00513 {
00514 #ifdef WITH_OPENNI2
00515 if(_color->isValid() && _depth->isValid())
00516 {
00517 return _depth->setMirroringEnabled(enabled) == openni::STATUS_OK &&
00518 _color->setMirroringEnabled(enabled) == openni::STATUS_OK;
00519 }
00520 #endif
00521 return false;
00522 }
00523
00524 bool CameraOpenNI2::init(const std::string & calibrationFolder)
00525 {
00526 #ifdef WITH_OPENNI2
00527 openni::OpenNI::initialize();
00528
00529 if(_device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str()) != openni::STATUS_OK)
00530 {
00531 if(!_deviceId.empty())
00532 {
00533 UERROR("CameraOpenNI2: Cannot open device \"%s\".", _deviceId.c_str());
00534 }
00535 else
00536 {
00537 UERROR("CameraOpenNI2: Cannot open device.");
00538 }
00539 _device->close();
00540 openni::OpenNI::shutdown();
00541 return false;
00542 }
00543
00544 if(UFile::getExtension(_deviceId).compare("oni")==0)
00545 {
00546 if(_device->getPlaybackControl() &&
00547 _device->getPlaybackControl()->setRepeatEnabled(false) != openni::STATUS_OK)
00548 {
00549 UERROR("CameraOpenNI2: Cannot set repeat mode to false.");
00550 _device->close();
00551 openni::OpenNI::shutdown();
00552 return false;
00553 }
00554 }
00555 else if(!_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
00556 {
00557 UERROR("CameraOpenNI2: Device doesn't support depth/color registration.");
00558 _device->close();
00559 openni::OpenNI::shutdown();
00560 return false;
00561 }
00562
00563 if(_device->getSensorInfo(openni::SENSOR_DEPTH) == NULL ||
00564 _device->getSensorInfo(openni::SENSOR_COLOR) == NULL)
00565 {
00566 UERROR("CameraOpenNI2: Cannot get sensor info for depth and color.");
00567 _device->close();
00568 openni::OpenNI::shutdown();
00569 return false;
00570 }
00571
00572 if(_depth->create(*_device, openni::SENSOR_DEPTH) != openni::STATUS_OK)
00573 {
00574 UERROR("CameraOpenNI2: Cannot create depth stream.");
00575 _device->close();
00576 openni::OpenNI::shutdown();
00577 return false;
00578 }
00579
00580 if(_color->create(*_device, openni::SENSOR_COLOR) != openni::STATUS_OK)
00581 {
00582 UERROR("CameraOpenNI2: Cannot create color stream.");
00583 _depth->destroy();
00584 _device->close();
00585 openni::OpenNI::shutdown();
00586 return false;
00587 }
00588
00589 if(_device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
00590 {
00591 UERROR("CameraOpenNI2: Failed to set depth/color registration.");
00592 }
00593
00594 if (_device->setDepthColorSyncEnabled(true) != openni::STATUS_OK)
00595 {
00596 UERROR("CameraOpenNI2: Failed to set depth color sync");
00597 }
00598
00599 _depth->setMirroringEnabled(false);
00600 _color->setMirroringEnabled(false);
00601
00602 const openni::Array<openni::VideoMode>& depthVideoModes = _depth->getSensorInfo().getSupportedVideoModes();
00603 for(int i=0; i<depthVideoModes.getSize(); ++i)
00604 {
00605 UINFO("CameraOpenNI2: Depth video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
00606 i,
00607 depthVideoModes[i].getFps(),
00608 depthVideoModes[i].getPixelFormat(),
00609 depthVideoModes[i].getResolutionX(),
00610 depthVideoModes[i].getResolutionY());
00611 }
00612
00613 const openni::Array<openni::VideoMode>& colorVideoModes = _color->getSensorInfo().getSupportedVideoModes();
00614 for(int i=0; i<colorVideoModes.getSize(); ++i)
00615 {
00616 UINFO("CameraOpenNI2: Color video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
00617 i,
00618 colorVideoModes[i].getFps(),
00619 colorVideoModes[i].getPixelFormat(),
00620 colorVideoModes[i].getResolutionX(),
00621 colorVideoModes[i].getResolutionY());
00622 }
00623
00624 openni::VideoMode mMode;
00625 mMode.setFps(30);
00626 mMode.setResolution(640,480);
00627 mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
00628 _depth->setVideoMode(mMode);
00629
00630 openni::VideoMode mModeColor;
00631 mModeColor.setFps(30);
00632 mModeColor.setResolution(640,480);
00633 mModeColor.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
00634 _color->setVideoMode(mModeColor);
00635
00636 UINFO("CameraOpenNI2: Using depth video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
00637 _depth->getVideoMode().getFps(),
00638 _depth->getVideoMode().getPixelFormat(),
00639 _depth->getVideoMode().getResolutionX(),
00640 _depth->getVideoMode().getResolutionY(),
00641 _depth->getHorizontalFieldOfView(),
00642 _depth->getVerticalFieldOfView());
00643
00644 if(_color->getCameraSettings())
00645 {
00646 UINFO("CameraOpenNI2: AutoWhiteBalanceEnabled = %d", _color->getCameraSettings()->getAutoWhiteBalanceEnabled()?1:0);
00647 UINFO("CameraOpenNI2: AutoExposureEnabled = %d", _color->getCameraSettings()->getAutoExposureEnabled()?1:0);
00648 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
00649 UINFO("CameraOpenNI2: Exposure = %d", _color->getCameraSettings()->getExposure());
00650 UINFO("CameraOpenNI2: GAIN = %d", _color->getCameraSettings()->getGain());
00651 #endif
00652 }
00653
00654 bool registered = true;
00655 if(registered)
00656 {
00657 _depthFx = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
00658 _depthFy = float(_color->getVideoMode().getResolutionY()/2) / std::tan(_color->getVerticalFieldOfView()/2.0f);
00659 }
00660 else
00661 {
00662 _depthFx = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
00663 _depthFy = float(_depth->getVideoMode().getResolutionY()/2) / std::tan(_depth->getVerticalFieldOfView()/2.0f);
00664 }
00665 UINFO("depth fx=%f fy=%f", _depthFx, _depthFy);
00666
00667 UINFO("CameraOpenNI2: Using color video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
00668 _color->getVideoMode().getFps(),
00669 _color->getVideoMode().getPixelFormat(),
00670 _color->getVideoMode().getResolutionX(),
00671 _color->getVideoMode().getResolutionY(),
00672 _color->getHorizontalFieldOfView(),
00673 _color->getVerticalFieldOfView());
00674
00675 if(_depth->start() != openni::STATUS_OK ||
00676 _color->start() != openni::STATUS_OK)
00677 {
00678 UERROR("CameraOpenNI2: Cannot start depth and/or color streams.");
00679 _depth->stop();
00680 _color->stop();
00681 _depth->destroy();
00682 _color->destroy();
00683 _device->close();
00684 openni::OpenNI::shutdown();
00685 return false;
00686 }
00687
00688 uSleep(1000);
00689
00690 return true;
00691 #else
00692 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00693 return false;
00694 #endif
00695 }
00696
00697 bool CameraOpenNI2::isCalibrated() const
00698 {
00699 return true;
00700 }
00701
00702 std::string CameraOpenNI2::getSerial() const
00703 {
00704 #ifdef WITH_OPENNI2
00705 if(_device)
00706 {
00707 return _device->getDeviceInfo().getName();
00708 }
00709 #endif
00710 return "";
00711 }
00712
00713 void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
00714 {
00715 #ifdef WITH_OPENNI2
00716 rgb = cv::Mat();
00717 depth = cv::Mat();
00718 fx = 0.0f;
00719 fy = 0.0f;
00720 cx = 0.0f;
00721 cy = 0.0f;
00722
00723 int readyStream = -1;
00724 if(_device->isValid() &&
00725 _depth->isValid() &&
00726 _color->isValid() &&
00727 _device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
00728 _device->getSensorInfo(openni::SENSOR_COLOR) != NULL)
00729 {
00730 openni::VideoStream* depthStream[] = {_depth};
00731 openni::VideoStream* colorStream[] = {_color};
00732 if(openni::OpenNI::waitForAnyStream(depthStream, 1, &readyStream, 2000) != openni::STATUS_OK ||
00733 openni::OpenNI::waitForAnyStream(colorStream, 1, &readyStream, 2000) != openni::STATUS_OK)
00734 {
00735 UWARN("No frames received since the last 2 seconds, end of stream is reached!");
00736 }
00737 else
00738 {
00739 openni::VideoFrameRef depthFrame, colorFrame;
00740 _depth->readFrame(&depthFrame);
00741 _color->readFrame(&colorFrame);
00742 if(depthFrame.isValid() && colorFrame.isValid())
00743 {
00744 int h=depthFrame.getHeight();
00745 int w=depthFrame.getWidth();
00746 depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
00747
00748 h=colorFrame.getHeight();
00749 w=colorFrame.getWidth();
00750 cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
00751 cv::cvtColor(tmp, rgb, CV_RGB2BGR);
00752 }
00753 UASSERT(_depthFx != 0.0f && _depthFy != 0.0f);
00754 fx = _depthFx;
00755 fy = _depthFy;
00756 cx = float(depth.cols/2) - 0.5f;
00757 cy = float(depth.rows/2) - 0.5f;
00758 }
00759 }
00760 else
00761 {
00762 ULOGGER_WARN("The camera must be initialized before requesting an image.");
00763 }
00764 #else
00765 UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
00766 #endif
00767 }
00768
00769 #ifdef WITH_FREENECT
00770
00771
00772
00773 class FreenectDevice : public UThread {
00774 public:
00775 FreenectDevice(freenect_context * ctx, int index) :
00776 index_(index),
00777 ctx_(ctx),
00778 device_(0),
00779 depthFocal_(0.0f)
00780 {
00781 UASSERT(ctx_ != 0);
00782 }
00783
00784 ~FreenectDevice()
00785 {
00786 this->join(true);
00787 if(device_ && freenect_close_device(device_) < 0){}
00788 }
00789
00790 const std::string & getSerial() const {return serial_;}
00791
00792 bool init()
00793 {
00794 if(device_)
00795 {
00796 this->join(true);
00797 freenect_close_device(device_);
00798 device_ = 0;
00799 }
00800 serial_.clear();
00801 std::vector<std::string> deviceSerials;
00802 freenect_device_attributes* attr_list;
00803 freenect_device_attributes* item;
00804 freenect_list_device_attributes(ctx_, &attr_list);
00805 for (item = attr_list; item != NULL; item = item->next) {
00806 deviceSerials.push_back(std::string(item->camera_serial));
00807 }
00808 freenect_free_device_attributes(attr_list);
00809
00810 if(freenect_open_device(ctx_, &device_, index_) < 0)
00811 {
00812 UERROR("FreenectDevice: Cannot open Kinect");
00813 return false;
00814 }
00815
00816 if(index_ >= 0 && index_ < (int)deviceSerials.size())
00817 {
00818 serial_ = deviceSerials[index_];
00819 }
00820 else
00821 {
00822 UERROR("Could not get serial for index %d", index_);
00823 }
00824
00825 freenect_set_user(device_, this);
00826 freenect_set_video_mode(device_, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
00827 freenect_set_depth_mode(device_, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED));
00828 depthBuffer_ = cv::Mat(cv::Size(640,480),CV_16UC1);
00829 rgbBuffer_ = cv::Mat(cv::Size(640,480), CV_8UC3);
00830 freenect_set_depth_buffer(device_, depthBuffer_.data);
00831 freenect_set_video_buffer(device_, rgbBuffer_.data);
00832 freenect_set_depth_callback(device_, freenect_depth_callback);
00833 freenect_set_video_callback(device_, freenect_video_callback);
00834
00835 bool registered = true;
00836 float rgb_focal_length_sxga = 1050.0f;
00837 float width_sxga = 1280.0f;
00838 float width = freenect_get_current_depth_mode(device_).width;
00839 float scale = width / width_sxga;
00840 if(registered)
00841 {
00842 depthFocal_ = rgb_focal_length_sxga * scale;
00843 }
00844 else
00845 {
00846 freenect_registration reg = freenect_copy_registration(device_);
00847 float depth_focal_length_sxga = reg.zero_plane_info.reference_distance / reg.zero_plane_info.reference_pixel_size;
00848 freenect_destroy_registration(®);
00849
00850 depthFocal_ = depth_focal_length_sxga * scale;
00851 }
00852
00853 UINFO("FreenectDevice: Depth focal = %f", depthFocal_);
00854 return true;
00855 }
00856
00857 float getDepthFocal() const {return depthFocal_;}
00858
00859 void getData(cv::Mat & rgb, cv::Mat & depth)
00860 {
00861 if(this->isRunning())
00862 {
00863 if(!dataReady_.acquire(1, 2000))
00864 {
00865 UERROR("Not received any frames since 2 seconds, try to restart the camera again.");
00866 }
00867 else
00868 {
00869 UScopeMutex s(dataMutex_);
00870 rgb = rgbLastFrame_;
00871 depth = depthLastFrame_;
00872 rgbLastFrame_ = cv::Mat();
00873 depthLastFrame_= cv::Mat();
00874 }
00875 }
00876 }
00877
00878 private:
00879
00880 void VideoCallback(void* rgb)
00881 {
00882 UASSERT(rgbBuffer_.data == rgb);
00883 UScopeMutex s(dataMutex_);
00884 bool notify = rgbLastFrame_.empty();
00885 cv::cvtColor(rgbBuffer_, rgbLastFrame_, CV_RGB2BGR);
00886 if(!depthLastFrame_.empty() && notify)
00887 {
00888 dataReady_.release();
00889 }
00890 }
00891
00892
00893 void DepthCallback(void* depth)
00894 {
00895 UASSERT(depthBuffer_.data == depth);
00896 UScopeMutex s(dataMutex_);
00897 bool notify = depthLastFrame_.empty();
00898 depthLastFrame_ = depthBuffer_.clone();
00899 if(!rgbLastFrame_.empty() && notify)
00900 {
00901 dataReady_.release();
00902 }
00903 }
00904
00905 void startVideo() {
00906 if(device_ && freenect_start_video(device_) < 0) UERROR("Cannot start RGB callback");
00907 }
00908 void stopVideo() {
00909 if(device_ && freenect_stop_video(device_) < 0) UERROR("Cannot stop RGB callback");
00910 }
00911 void startDepth() {
00912 if(device_ && freenect_start_depth(device_) < 0) UERROR("Cannot start depth callback");
00913 }
00914 void stopDepth() {
00915 if(device_ && freenect_stop_depth(device_) < 0) UERROR("Cannot stop depth callback");
00916 }
00917
00918 virtual void mainLoopBegin()
00919 {
00920 this->startDepth();
00921 this->startVideo();
00922 }
00923
00924 virtual void mainLoop()
00925 {
00926 timeval t;
00927 t.tv_sec = 0;
00928 t.tv_usec = 10000;
00929 if(freenect_process_events_timeout(ctx_, &t) < 0)
00930 {
00931 UERROR("FreenectDevice: Cannot process freenect events");
00932 this->kill();
00933 }
00934 }
00935
00936 virtual void mainLoopEnd()
00937 {
00938 this->stopDepth();
00939 this->stopVideo();
00940 dataReady_.release();
00941 }
00942
00943 static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {
00944 FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
00945 device->DepthCallback(depth);
00946 }
00947 static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {
00948 FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
00949 device->VideoCallback(video);
00950 }
00951
00952
00953 FreenectDevice( const FreenectDevice& );
00954 const FreenectDevice& operator=( const FreenectDevice& );
00955
00956 private:
00957 int index_;
00958 std::string serial_;
00959 freenect_context * ctx_;
00960 freenect_device * device_;
00961 cv::Mat depthBuffer_;
00962 cv::Mat rgbBuffer_;
00963 UMutex dataMutex_;
00964 cv::Mat depthLastFrame_;
00965 cv::Mat rgbLastFrame_;
00966 float depthFocal_;
00967 USemaphore dataReady_;
00968 };
00969 #endif
00970
00971
00972
00973
00974 bool CameraFreenect::available()
00975 {
00976 #ifdef WITH_FREENECT
00977 return true;
00978 #else
00979 return false;
00980 #endif
00981 }
00982
00983 CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform) :
00984 CameraRGBD(imageRate, localTransform),
00985 deviceId_(deviceId),
00986 ctx_(0),
00987 freenectDevice_(0)
00988 {
00989 #ifdef WITH_FREENECT
00990 if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
00991
00992 freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_CAMERA));
00993 #endif
00994 }
00995
00996 CameraFreenect::~CameraFreenect()
00997 {
00998 #ifdef WITH_FREENECT
00999 if(freenectDevice_)
01000 {
01001 freenectDevice_->join(true);
01002 delete freenectDevice_;
01003 freenectDevice_ = 0;
01004 }
01005 if(ctx_)
01006 {
01007 if(freenect_shutdown(ctx_) < 0){}
01008 }
01009 #endif
01010 }
01011
01012 bool CameraFreenect::init(const std::string & calibrationFolder)
01013 {
01014 #ifdef WITH_FREENECT
01015 if(freenectDevice_)
01016 {
01017 freenectDevice_->join(true);
01018 delete freenectDevice_;
01019 freenectDevice_ = 0;
01020 }
01021
01022 if(ctx_ && freenect_num_devices(ctx_) > 0)
01023 {
01024 freenectDevice_ = new FreenectDevice(ctx_, deviceId_);
01025 if(freenectDevice_->init())
01026 {
01027 freenectDevice_->start();
01028 uSleep(3000);
01029 return true;
01030 }
01031 else
01032 {
01033 UERROR("CameraFreenect: Init failed!");
01034 }
01035 delete freenectDevice_;
01036 freenectDevice_ = 0;
01037 }
01038 else
01039 {
01040 UERROR("CameraFreenect: No devices connected!");
01041 }
01042 #else
01043 UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
01044 #endif
01045 return false;
01046 }
01047
01048 bool CameraFreenect::isCalibrated() const
01049 {
01050 return true;
01051 }
01052
01053 std::string CameraFreenect::getSerial() const
01054 {
01055 #ifdef WITH_FREENECT
01056 if(freenectDevice_)
01057 {
01058 return freenectDevice_->getSerial();
01059 }
01060 #endif
01061 return "";
01062 }
01063
01064 void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
01065 {
01066 #ifdef WITH_FREENECT
01067 rgb = cv::Mat();
01068 depth = cv::Mat();
01069 fx = 0.0f;
01070 fy = 0.0f;
01071 cx = 0.0f;
01072 cy = 0.0f;
01073 if(ctx_ && freenectDevice_)
01074 {
01075 if(freenectDevice_->isRunning())
01076 {
01077 freenectDevice_->getData(rgb, depth);
01078 if(!rgb.empty() && !depth.empty())
01079 {
01080 UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
01081 fx = freenectDevice_->getDepthFocal();
01082 fy = freenectDevice_->getDepthFocal();
01083 cx = float(depth.cols/2) - 0.5f;
01084 cy = float(depth.rows/2) - 0.5f;
01085 }
01086 }
01087 else
01088 {
01089 UERROR("CameraFreenect: Re-initialization needed!");
01090 delete freenectDevice_;
01091 freenectDevice_ = 0;
01092 }
01093 }
01094 #else
01095 UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
01096 #endif
01097 }
01098
01099
01100
01101
01102 bool CameraFreenect2::available()
01103 {
01104 #ifdef WITH_FREENECT2
01105 return true;
01106 #else
01107 return false;
01108 #endif
01109 }
01110
01111 CameraFreenect2::CameraFreenect2(int deviceId, Type type, float imageRate, const Transform & localTransform) :
01112 CameraRGBD(imageRate, localTransform),
01113 deviceId_(deviceId),
01114 type_(type),
01115 freenect2_(0),
01116 dev_(0),
01117 pipeline_(0),
01118 listener_(0),
01119 reg_(0)
01120 {
01121 #ifdef WITH_FREENECT2
01122 freenect2_ = new libfreenect2::Freenect2();
01123 switch(type_)
01124 {
01125 case kTypeRGBIR:
01126 listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
01127 break;
01128 case kTypeIRDepth:
01129 listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
01130 break;
01131 case kTypeRGBDepthSD:
01132 case kTypeRGBDepthHD:
01133 default:
01134 listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
01135 break;
01136 }
01137
01138 #ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
01139 pipeline_ = new libfreenect2::OpenGLPacketPipeline();
01140 #else
01141 #ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
01142 pipeline_ = new libfreenect2::OpenCLPacketPipeline();
01143 #else
01144 pipeline_ = new libfreenect2::CpuPacketPipeline();
01145 #endif
01146 #endif
01147
01148
01149
01150
01151
01152 libfreenect2::DepthPacketProcessor::Config config;
01153 config.EnableBilateralFilter = true;
01154 config.EnableEdgeAwareFilter = true;
01155 config.MinDepth = 0.1;
01156 config.MaxDepth = 12;
01157 pipeline_->getDepthPacketProcessor()->setConfiguration(config);
01158
01159 #endif
01160 }
01161
01162 CameraFreenect2::~CameraFreenect2()
01163 {
01164 #ifdef WITH_FREENECT2
01165 if(dev_)
01166 {
01167 dev_->stop();
01168 dev_->close();
01169 }
01170 if(listener_)
01171 {
01172 delete listener_;
01173 }
01174
01175 if(reg_)
01176 {
01177 delete reg_;
01178 reg_ = 0;
01179 }
01180
01181
01182
01183
01184
01185
01186 if(freenect2_)
01187 {
01188 delete freenect2_;
01189 }
01190 UDEBUG("");
01191 #endif
01192 }
01193
01194 bool CameraFreenect2::init(const std::string & calibrationFolder)
01195 {
01196 #ifdef WITH_FREENECT2
01197 if(dev_)
01198 {
01199 dev_->stop();
01200 dev_->close();
01201 dev_ = 0;
01202 }
01203
01204 if(reg_)
01205 {
01206 delete reg_;
01207 reg_ = 0;
01208 }
01209
01210 if(deviceId_ <= 0)
01211 {
01212 dev_ = freenect2_->openDefaultDevice(pipeline_);
01213 }
01214 else
01215 {
01216 dev_ = freenect2_->openDevice(deviceId_, pipeline_);
01217 }
01218
01219 if(dev_)
01220 {
01221 dev_->setColorFrameListener(listener_);
01222 dev_->setIrAndDepthFrameListener(listener_);
01223
01224 dev_->start();
01225
01226 UINFO("CameraFreenect2: device serial: %s", dev_->getSerialNumber().c_str());
01227 UINFO("CameraFreenect2: device firmware: %s", dev_->getFirmwareVersion().c_str());
01228
01229
01230 libfreenect2::Freenect2Device::IrCameraParams depthParams = dev_->getIrCameraParams();
01231 libfreenect2::Freenect2Device::ColorCameraParams colorParams = dev_->getColorCameraParams();
01232 reg_ = new libfreenect2::Registration(&depthParams, &colorParams);
01233
01234
01235 if(!calibrationFolder.empty())
01236 {
01237 if(!stereoModel_.load(calibrationFolder, dev_->getSerialNumber()))
01238 {
01239 UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
01240 dev_->getSerialNumber().c_str(), calibrationFolder.c_str());
01241 }
01242 else
01243 {
01244
01245 cv::Mat colorP = stereoModel_.right().P();
01246 cv::Size colorSize = stereoModel_.right().imageSize();
01247 if(type_ == kTypeRGBDepthSD)
01248 {
01249 colorP.at<double>(0,0)/=2.0f;
01250 colorP.at<double>(1,1)/=2.0f;
01251 colorP.at<double>(0,2)/=2.0f;
01252 colorP.at<double>(1,2)/=2.0f;
01253 colorSize.width/=2;
01254 colorSize.height/=2;
01255 }
01256 cv::Mat depthP = stereoModel_.left().P();
01257 cv::Size depthSize = stereoModel_.left().imageSize();
01258 float ratioY = float(colorSize.height)/float(depthSize.height);
01259 float ratioX = float(colorSize.width)/float(depthSize.width);
01260 depthP.at<double>(0,0)*=ratioX;
01261 depthP.at<double>(1,1)*=ratioY;
01262 depthP.at<double>(0,2)*=ratioX;
01263 depthP.at<double>(1,2)*=ratioY;
01264 depthSize.width*=ratioX;
01265 depthSize.height*=ratioY;
01266 const CameraModel & l = stereoModel_.left();
01267 const CameraModel & r = stereoModel_.right();
01268 stereoModel_ = StereoCameraModel(stereoModel_.name(),
01269 depthSize, l.K(), l.D(), l.R(), depthP,
01270 colorSize, r.K(), r.D(), r.R(), colorP,
01271 stereoModel_.R(), stereoModel_.T(), stereoModel_.E(), stereoModel_.F());
01272 }
01273 }
01274
01275 return true;
01276 }
01277 else
01278 {
01279 UERROR("CameraFreenect2: no device connected or failure opening the default one!");
01280 }
01281 #else
01282 UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
01283 #endif
01284 return false;
01285 }
01286
01287 bool CameraFreenect2::isCalibrated() const
01288 {
01289 return true;
01290 }
01291
01292 std::string CameraFreenect2::getSerial() const
01293 {
01294 #ifdef WITH_FREENECT2
01295 if(dev_)
01296 {
01297 return dev_->getSerialNumber();
01298 }
01299 #endif
01300 return "";
01301 }
01302
01303 void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
01304 {
01305 #ifdef WITH_FREENECT2
01306 rgb = cv::Mat();
01307 depth = cv::Mat();
01308 fx = 0.0f;
01309 fy = 0.0f;
01310 cx = 0.0f;
01311 cy = 0.0f;
01312 if(dev_ && listener_)
01313 {
01314 libfreenect2::FrameMap frames;
01315 if(listener_->waitForNewFrame(frames, 1000))
01316 {
01317 libfreenect2::Frame *rgbFrame = 0;
01318 libfreenect2::Frame *irFrame = 0;
01319 libfreenect2::Frame *depthFrame = 0;
01320
01321 switch(type_)
01322 {
01323 case kTypeRGBIR:
01324 rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
01325 irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
01326 break;
01327 case kTypeIRDepth:
01328 irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
01329 depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
01330 break;
01331 case kTypeRGBDepthSD:
01332 case kTypeRGBDepthHD:
01333 default:
01334 rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
01335 depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
01336 break;
01337 }
01338
01339 if(irFrame && depthFrame)
01340 {
01341 cv::Mat irMat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data);
01342
01343 float maxIr_ = 0x7FFF;
01344 float minIr_ = 0x0;
01345 const float factor = 255.0f / float((maxIr_ - minIr_));
01346 rgb = cv::Mat(irMat.rows, irMat.cols, CV_8UC1);
01347 for(int i=0; i<irMat.rows; ++i)
01348 {
01349 for(int j=0; j<irMat.cols; ++j)
01350 {
01351 rgb.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(irMat.at<float>(i,j) - minIr_, 0.0f)) * factor, 255.0f);
01352 }
01353 }
01354
01355 cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
01356 cv::flip(rgb, rgb, 1);
01357 cv::flip(depth, depth, 1);
01358 if(stereoModel_.isValid())
01359 {
01360
01361 rgb = stereoModel_.left().rectifyImage(rgb);
01362 depth = stereoModel_.left().rectifyImage(depth);
01363 fx = stereoModel_.left().fx();
01364 fy = stereoModel_.left().fy();
01365 cx = stereoModel_.left().cx();
01366 cy = stereoModel_.left().cy();
01367 }
01368 else
01369 {
01370 libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
01371 fx = params.fx;
01372 fy = params.fy;
01373 cx = params.cx;
01374 cy = params.cy;
01375 }
01376 }
01377 else
01378 {
01379
01380
01381 cv::Mat rgbMat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data);
01382 cv::flip(rgbMat, rgb, 1);
01383
01384 if(stereoModel_.isValid())
01385 {
01386
01387 rgb = stereoModel_.right().rectifyImage(rgb);
01388 if(irFrame)
01389 {
01390
01391 cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
01392 cv::flip(depth, depth, 1);
01393 depth = stereoModel_.left().rectifyImage(depth);
01394 }
01395 else
01396 {
01397
01398 cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
01399 cv::flip(depth, depth, 1);
01400 depth = stereoModel_.left().rectifyDepth(depth);
01401
01402 bool registered = true;
01403 if(registered)
01404 {
01405 depth = util3d::registerDepth(
01406 depth,
01407 stereoModel_.left().P().colRange(0,3).rowRange(0,3),
01408 stereoModel_.right().P().colRange(0,3).rowRange(0,3),
01409 stereoModel_.transform());
01410 util3d::fillRegisteredDepthHoles(depth, true, false);
01411 fx = stereoModel_.right().fx();
01412 fy = stereoModel_.right().fy();
01413 cx = stereoModel_.right().cx();
01414 cy = stereoModel_.right().cy();
01415 }
01416 else
01417 {
01418 fx = stereoModel_.left().fx();
01419 fy = stereoModel_.left().fy();
01420 cx = stereoModel_.left().cx();
01421 cy = stereoModel_.left().cy();
01422 }
01423 }
01424 }
01425 else
01426 {
01427
01428 if(irFrame)
01429 {
01430 cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
01431 }
01432 else
01433 {
01434 cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
01435
01436
01437 if(reg_)
01438 {
01439 if(type_ == kTypeRGBDepthSD)
01440 {
01441 cv::Mat tmp;
01442 cv::resize(rgb, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
01443 rgb = tmp;
01444 }
01445 cv::Mat depthFrameMat = cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data);
01446 depth = cv::Mat::zeros(rgb.rows, rgb.cols, CV_16U);
01447 for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
01448 {
01449 for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
01450 {
01451 float dz = depthFrameMat.at<float>(dy,dx);
01452 float dz1 = depthFrameMat.at<float>(dy,dx+1);
01453 float dz2 = depthFrameMat.at<float>(dy+1,dx);
01454 float dz3 = depthFrameMat.at<float>(dy+1,dx+1);
01455 if(dz && dz1 && dz2 && dz3)
01456 {
01457 float avg = (dz + dz1 + dz2 + dz3) / 4;
01458 float thres = 0.01 * avg;
01459 if( fabs(dz - avg) < thres &&
01460 fabs(dz1 - avg) < thres &&
01461 fabs(dz2 - avg) < thres &&
01462 fabs(dz3 - avg) < thres)
01463 {
01464 float cx=-1,cy=-1;
01465 reg_->apply(dx, dy, dz, cx, cy);
01466 if(type_==kTypeRGBDepthSD)
01467 {
01468 cx/=2.0f;
01469 cy/=2.0f;
01470 }
01471 int rcx = cvRound(cx);
01472 int rcy = cvRound(cy);
01473 if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows))
01474 {
01475 unsigned short & zReg = depth.at<unsigned short>(rcy, rcx);
01476 if(zReg == 0 || zReg > (unsigned short)dz)
01477 {
01478 zReg = (unsigned short)dz;
01479 }
01480 }
01481 }
01482 }
01483 }
01484 }
01485 util3d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeRGBDepthHD);
01486 util3d::fillRegisteredDepthHoles(depth, type_==kTypeRGBDepthSD, type_==kTypeRGBDepthHD);
01487 libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
01488 fx = params.fx*(type_==kTypeRGBDepthSD?0.5:1.0f);
01489 fy = params.fy*(type_==kTypeRGBDepthSD?0.5:1.0f);
01490 cx = params.cx*(type_==kTypeRGBDepthSD?0.5:1.0f);
01491 cy = params.cy*(type_==kTypeRGBDepthSD?0.5:1.0f);
01492 }
01493 else
01494 {
01495 libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
01496 fx = params.fx;
01497 fy = params.fy;
01498 cx = params.cx;
01499 cy = params.cy;
01500 }
01501 }
01502 cv::flip(depth, depth, 1);
01503 }
01504 }
01505 listener_->release(frames);
01506 }
01507 else
01508 {
01509 UWARN("CameraFreenect2: Failed to get frames! rtabmap should link on libusb of "
01510 "libfreenect2, this can be done by setting LD_LIBRARY_PATH to "
01511 "\"libfreenect2/depends/libusb/lib\"");
01512 }
01513 }
01514 #else
01515 UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
01516 #endif
01517 }
01518
01519
01520
01521
01522
01523
01524 #ifdef WITH_DC1394
01525 class DC1394Device
01526 {
01527 public:
01528 DC1394Device() :
01529 camera_(0),
01530 context_(0)
01531 {
01532
01533 }
01534 ~DC1394Device()
01535 {
01536 if (camera_)
01537 {
01538 if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_OFF) ||
01539 DC1394_SUCCESS != dc1394_capture_stop(camera_))
01540 {
01541 UWARN("unable to stop camera");
01542 }
01543
01544
01545 dc1394_capture_stop(camera_);
01546 dc1394_camera_free(camera_);
01547 camera_ = NULL;
01548 }
01549 if(context_)
01550 {
01551 dc1394_free(context_);
01552 context_ = NULL;
01553 }
01554 }
01555
01556 const std::string & guid() const {return guid_;}
01557
01558 bool init()
01559 {
01560 if(camera_)
01561 {
01562
01563 dc1394_capture_stop(camera_);
01564 dc1394_camera_free(camera_);
01565 camera_ = NULL;
01566 }
01567
01568
01569 int err;
01570 if(context_ == NULL)
01571 {
01572 context_ = dc1394_new ();
01573 if (context_ == NULL)
01574 {
01575 UERROR( "Could not initialize dc1394_context.\n"
01576 "Make sure /dev/raw1394 exists, you have access permission,\n"
01577 "and libraw1394 development package is installed.");
01578 return false;
01579 }
01580 }
01581
01582 dc1394camera_list_t *list;
01583 err = dc1394_camera_enumerate(context_, &list);
01584 if (err != DC1394_SUCCESS)
01585 {
01586 UERROR("Could not get camera list");
01587 return false;
01588 }
01589
01590 if (list->num == 0)
01591 {
01592 UERROR("No cameras found");
01593 dc1394_camera_free_list (list);
01594 return false;
01595 }
01596 uint64_t guid = list->ids[0].guid;
01597 dc1394_camera_free_list (list);
01598
01599
01600 camera_ = dc1394_camera_new (context_, guid);
01601 if (!camera_)
01602 {
01603 UERROR("Failed to initialize camera with GUID [%016lx]", guid);
01604 return false;
01605 }
01606
01607 uint32_t value[3];
01608 value[0]= camera_->guid & 0xffffffff;
01609 value[1]= (camera_->guid >>32) & 0x000000ff;
01610 value[2]= (camera_->guid >>40) & 0xfffff;
01611 guid_ = uFormat("%06x%02x%08x", value[2], value[1], value[0]);
01612
01613 UINFO("camera model: %s %s", camera_->vendor, camera_->model);
01614
01615
01616
01617 bool bmode = camera_->bmode_capable;
01618 if (bmode
01619 && (DC1394_SUCCESS !=
01620 dc1394_video_set_operation_mode(camera_,
01621 DC1394_OPERATION_MODE_1394B)))
01622 {
01623 bmode = false;
01624 UWARN("failed to set IEEE1394b mode");
01625 }
01626
01627
01628 dc1394speed_t request = DC1394_ISO_SPEED_3200;
01629 int rate = 3200;
01630 if (!bmode)
01631 {
01632
01633 request = DC1394_ISO_SPEED_400;
01634 rate = 400;
01635 }
01636
01637
01638 while (rate > 400)
01639 {
01640 if (request <= DC1394_ISO_SPEED_MIN)
01641 {
01642
01643 dc1394speed_t curSpeed;
01644 if (DC1394_SUCCESS == dc1394_video_get_iso_speed(camera_, &curSpeed) && curSpeed <= DC1394_ISO_SPEED_MAX)
01645 {
01646
01647
01648
01649 request = curSpeed;
01650 rate = 100 << (curSpeed - DC1394_ISO_SPEED_MIN);
01651 }
01652 else
01653 {
01654 UWARN("Unable to get ISO speed; assuming 400Mb/s");
01655 rate = 400;
01656 request = DC1394_ISO_SPEED_400;
01657 }
01658 break;
01659 }
01660
01661 request = (dc1394speed_t) ((int) request - 1);
01662 rate = rate / 2;
01663 }
01664
01665
01666 if (DC1394_SUCCESS != dc1394_video_set_iso_speed(camera_, request))
01667 {
01668 UERROR("Failed to set iso speed");
01669 return false;
01670 }
01671
01672
01673 dc1394video_modes_t vmodes;
01674 err = dc1394_video_get_supported_modes(camera_, &vmodes);
01675 if (err != DC1394_SUCCESS)
01676 {
01677 UERROR("unable to get supported video modes");
01678 return (dc1394video_mode_t) 0;
01679 }
01680
01681
01682 bool found = false;
01683 dc1394video_mode_t videoMode = DC1394_VIDEO_MODE_FORMAT7_3;
01684 for (uint32_t i = 0; i < vmodes.num; ++i)
01685 {
01686 if (vmodes.modes[i] == videoMode)
01687 {
01688 found = true;
01689 }
01690 }
01691 if(!found)
01692 {
01693 UERROR("unable to get video mode %d", videoMode);
01694 return false;
01695 }
01696
01697 if (DC1394_SUCCESS != dc1394_video_set_mode(camera_, videoMode))
01698 {
01699 UERROR("Failed to set video mode %d", videoMode);
01700 return false;
01701 }
01702
01703
01704 if (dc1394_is_video_mode_scalable(videoMode) == DC1394_TRUE)
01705 {
01706 if (DC1394_SUCCESS != dc1394_format7_set_color_coding(camera_, videoMode, DC1394_COLOR_CODING_RAW16))
01707 {
01708 UERROR("Could not set color coding");
01709 return false;
01710 }
01711 uint32_t packetSize;
01712 if (DC1394_SUCCESS != dc1394_format7_get_recommended_packet_size(camera_, videoMode, &packetSize))
01713 {
01714 UERROR("Could not get default packet size");
01715 return false;
01716 }
01717
01718 if (DC1394_SUCCESS != dc1394_format7_set_packet_size(camera_, videoMode, packetSize))
01719 {
01720 UERROR("Could not set packet size");
01721 return false;
01722 }
01723 }
01724 else
01725 {
01726 UERROR("Video is not in mode scalable");
01727 }
01728
01729
01730
01731 if (DC1394_SUCCESS != dc1394_capture_setup(camera_, 4, DC1394_CAPTURE_FLAGS_DEFAULT))
01732 {
01733 UERROR("Failed to open device!");
01734 return false;
01735 }
01736
01737
01738 if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_ON))
01739 {
01740 UERROR("Failed to start device!");
01741 return false;
01742 }
01743
01744 return true;
01745 }
01746
01747 bool getImages(cv::Mat & left, cv::Mat & right)
01748 {
01749 if(camera_)
01750 {
01751 dc1394video_frame_t * frame = NULL;
01752 UDEBUG("[%016lx] waiting camera", camera_->guid);
01753 dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame);
01754 if (!frame)
01755 {
01756 UERROR("Unable to capture frame");
01757 return false;
01758 }
01759 dc1394video_frame_t frame1 = *frame;
01760
01761 size_t frame1_size = frame->total_bytes;
01762 frame1.image = (unsigned char *) malloc(frame1_size);
01763 frame1.allocated_image_bytes = frame1_size;
01764 frame1.color_coding = DC1394_COLOR_CODING_RAW8;
01765 int err = dc1394_deinterlace_stereo_frames(frame, &frame1, DC1394_STEREO_METHOD_INTERLACED);
01766 if (err != DC1394_SUCCESS)
01767 {
01768 free(frame1.image);
01769 dc1394_capture_enqueue(camera_, frame);
01770 UERROR("Could not extract stereo frames");
01771 return false;
01772 }
01773
01774 uint8_t* capture_buffer = reinterpret_cast<uint8_t *>(frame1.image);
01775 UASSERT(capture_buffer);
01776
01777 cv::Mat image(frame->size[1], frame->size[0], CV_8UC3);
01778 cv::Mat image2 = image.clone();
01779
01780
01781
01782 cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer), left, CV_BayerRG2BGR);
01783 cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer+image.total()), right, CV_BayerRG2GRAY);
01784
01785 dc1394_capture_enqueue(camera_, frame);
01786
01787 free(frame1.image);
01788
01789 return true;
01790 }
01791 return false;
01792 }
01793
01794 private:
01795 dc1394camera_t *camera_;
01796 dc1394_t *context_;
01797 std::string guid_;
01798 };
01799 #endif
01800
01801 bool CameraStereoDC1394::available()
01802 {
01803 #ifdef WITH_DC1394
01804 return true;
01805 #else
01806 return false;
01807 #endif
01808 }
01809
01810 CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localTransform) :
01811 CameraRGBD(imageRate, localTransform),
01812 device_(0)
01813 {
01814 #ifdef WITH_DC1394
01815 device_ = new DC1394Device();
01816 #endif
01817 }
01818
01819 CameraStereoDC1394::~CameraStereoDC1394()
01820 {
01821 #ifdef WITH_DC1394
01822 if(device_)
01823 {
01824 delete device_;
01825 }
01826 #endif
01827 }
01828
01829 bool CameraStereoDC1394::init(const std::string & calibrationFolder)
01830 {
01831 #ifdef WITH_DC1394
01832 if(device_)
01833 {
01834 bool ok = device_->init();
01835 if(ok)
01836 {
01837
01838 if(!calibrationFolder.empty())
01839 {
01840 if(!stereoModel_.load(calibrationFolder, device_->guid()))
01841 {
01842 UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
01843 device_->guid().c_str(), calibrationFolder.c_str());
01844 }
01845 else
01846 {
01847 UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
01848 stereoModel_.left().fx(),
01849 stereoModel_.left().cx(),
01850 stereoModel_.left().cy(),
01851 stereoModel_.baseline());
01852 }
01853 }
01854 }
01855 return ok;
01856 }
01857 #else
01858 UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
01859 #endif
01860 return false;
01861 }
01862
01863 bool CameraStereoDC1394::isCalibrated() const
01864 {
01865 return stereoModel_.isValid();
01866 }
01867
01868 std::string CameraStereoDC1394::getSerial() const
01869 {
01870 #ifdef WITH_DC1394
01871 if(device_)
01872 {
01873 return device_->guid();
01874 }
01875 #endif
01876 return "";
01877 }
01878
01879 void CameraStereoDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy)
01880 {
01881 #ifdef WITH_DC1394
01882 left = cv::Mat();
01883 right = cv::Mat();
01884 fx = 0.0f;
01885 baseline = 0.0f;
01886 cx = 0.0f;
01887 cy = 0.0f;
01888 if(device_)
01889 {
01890 device_->getImages(left, right);
01891
01892
01893 left = stereoModel_.left().rectifyImage(left);
01894 right = stereoModel_.right().rectifyImage(right);
01895 fx = stereoModel_.left().fx();
01896 cx = stereoModel_.left().cx();
01897 cy = stereoModel_.left().cy();
01898 baseline = stereoModel_.baseline();
01899 }
01900 #else
01901 UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
01902 #endif
01903 }
01904
01905
01906
01907
01908 CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transform & localTransform) :
01909 CameraRGBD(imageRate, localTransform),
01910 camera_(0),
01911 triclopsCtx_(0)
01912 {
01913 #ifdef WITH_FLYCAPTURE2
01914 camera_ = new FlyCapture2::Camera();
01915 #endif
01916 }
01917
01918 CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
01919 {
01920 #ifdef WITH_FLYCAPTURE2
01921
01922 camera_->StopCapture();
01923 camera_->Disconnect();
01924
01925
01926 triclopsDestroyContext( triclopsCtx_ ) ;
01927
01928 delete camera_;
01929 #endif
01930 }
01931
01932 bool CameraStereoFlyCapture2::available()
01933 {
01934 #ifdef WITH_FLYCAPTURE2
01935 return true;
01936 #else
01937 return false;
01938 #endif
01939 }
01940
01941 bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder)
01942 {
01943 #ifdef WITH_FLYCAPTURE2
01944 if(camera_)
01945 {
01946
01947 camera_->StopCapture();
01948 camera_->Disconnect();
01949 }
01950 if(triclopsCtx_)
01951 {
01952 triclopsDestroyContext(triclopsCtx_);
01953 triclopsCtx_ = 0;
01954 }
01955
01956
01957 FlyCapture2::Error fc2Error = camera_->Connect();
01958 if(fc2Error != FlyCapture2::PGRERROR_OK)
01959 {
01960 UERROR("Failed to connect the camera.");
01961 return false;
01962 }
01963
01964
01965 Fc2Triclops::StereoCameraMode mode = Fc2Triclops::TWO_CAMERA_NARROW;
01966 if(Fc2Triclops::setStereoMode(*camera_, mode ))
01967 {
01968 UERROR("Failed to set stereo mode.");
01969 return false;
01970 }
01971
01972
01973 FlyCapture2::CameraInfo camInfo;
01974 if(camera_->GetCameraInfo(&camInfo) != FlyCapture2::PGRERROR_OK)
01975 {
01976 UERROR("Failed to get camera info.");
01977 return false;
01978 }
01979
01980 float dummy;
01981 unsigned packetSz;
01982 FlyCapture2::Format7ImageSettings imageSettings;
01983 int maxWidth = 640;
01984 int maxHeight = 480;
01985 if(camera_->GetFormat7Configuration(&imageSettings, &packetSz, &dummy) == FlyCapture2::PGRERROR_OK)
01986 {
01987 maxHeight = imageSettings.height;
01988 maxWidth = imageSettings.width;
01989 }
01990
01991
01992 if(Fc2Triclops::getContextFromCamera(camInfo.serialNumber, &triclopsCtx_))
01993 {
01994 UERROR("Failed to get calibration from the camera.");
01995 return false;
01996 }
01997
01998 float fx, cx, cy, baseline;
01999 triclopsGetFocalLength(triclopsCtx_, &fx);
02000 triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
02001 triclopsGetBaseline(triclopsCtx_, &baseline);
02002 UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", fx, cx, cy, baseline);
02003
02004 triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW );
02005 UASSERT(triclopsSetResolutionAndPrepare(triclopsCtx_, maxHeight, maxWidth, maxHeight, maxWidth) == Fc2Triclops::ERRORTYPE_OK);
02006
02007 if(camera_->StartCapture() != FlyCapture2::PGRERROR_OK)
02008 {
02009 UERROR("Failed to start capture.");
02010 return false;
02011 }
02012
02013 return true;
02014 #else
02015 UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
02016 #endif
02017 return false;
02018 }
02019
02020 bool CameraStereoFlyCapture2::isCalibrated() const
02021 {
02022 #ifdef WITH_FLYCAPTURE2
02023 if(triclopsCtx_)
02024 {
02025 float fx, cx, cy, baseline;
02026 triclopsGetFocalLength(triclopsCtx_, &fx);
02027 triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
02028 triclopsGetBaseline(triclopsCtx_, &baseline);
02029 return fx > 0.0f && cx > 0.0f && cy > 0.0f && baseline > 0.0f;
02030 }
02031 #endif
02032 return false;
02033 }
02034
02035 std::string CameraStereoFlyCapture2::getSerial() const
02036 {
02037 #ifdef WITH_FLYCAPTURE2
02038 if(camera_ && camera_->IsConnected())
02039 {
02040 FlyCapture2::CameraInfo camInfo;
02041 if(camera_->GetCameraInfo(&camInfo) == FlyCapture2::PGRERROR_OK)
02042 {
02043 return uNumber2Str(camInfo.serialNumber);
02044 }
02045 }
02046 #endif
02047 return "";
02048 }
02049
02050
02051 #ifdef WITH_FLYCAPTURE2
02052 struct ImageContainer
02053 {
02054 FlyCapture2::Image tmp[2];
02055 FlyCapture2::Image unprocessed[2];
02056 } ;
02057 #endif
02058
02059 void CameraStereoFlyCapture2::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy)
02060 {
02061 #ifdef WITH_FLYCAPTURE2
02062 left = cv::Mat();
02063 right = cv::Mat();
02064 fx = 0.0f;
02065 baseline = 0.0f;
02066 cx = 0.0f;
02067 cy = 0.0f;
02068
02069 if(camera_ && triclopsCtx_ && camera_->IsConnected())
02070 {
02071
02072
02073 FlyCapture2::Image grabbedImage;
02074 if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK)
02075 {
02076
02077 ImageContainer imageCont;
02078
02079
02080 FlyCapture2::Image imageRawRight;
02081 FlyCapture2::Image imageRawLeft;
02082 FlyCapture2::Image * unprocessedImage = imageCont.unprocessed;
02083
02084
02085 if(Fc2Triclops::unpackUnprocessedRawOrMono16Image(
02086 grabbedImage,
02087 true ,
02088 imageRawLeft ,
02089 imageRawRight ) == Fc2Triclops::ERRORTYPE_OK)
02090 {
02091
02092 FlyCapture2::Image srcImgRightRef(imageRawRight);
02093 FlyCapture2::Image srcImgLeftRef(imageRawLeft);
02094
02095 bool ok = true;;
02096 if ( srcImgRightRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK ||
02097 srcImgLeftRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK)
02098 {
02099 ok = false;
02100 }
02101
02102 if(ok)
02103 {
02104 FlyCapture2::Image imageColorRight;
02105 FlyCapture2::Image imageColorLeft;
02106 if ( srcImgRightRef.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &imageColorRight) != FlyCapture2::PGRERROR_OK ||
02107 srcImgLeftRef.Convert(FlyCapture2::PIXEL_FORMAT_BGRU, &imageColorLeft) != FlyCapture2::PGRERROR_OK)
02108 {
02109 ok = false;
02110 }
02111
02112 if(ok)
02113 {
02114
02115 TriclopsInput triclopsColorInputs;
02116 triclopsBuildRGBTriclopsInput(
02117 grabbedImage.GetCols(),
02118 grabbedImage.GetRows(),
02119 imageColorRight.GetStride(),
02120 (unsigned long)grabbedImage.GetTimeStamp().seconds,
02121 (unsigned long)grabbedImage.GetTimeStamp().microSeconds,
02122 imageColorRight.GetData(),
02123 imageColorRight.GetData(),
02124 imageColorRight.GetData(),
02125 &triclopsColorInputs);
02126
02127 triclopsRectify(triclopsCtx_, const_cast<TriclopsInput *>(&triclopsColorInputs) );
02128
02129 TriclopsImage rectifiedImage;
02130 triclopsGetImage( triclopsCtx_,
02131 TriImg_RECTIFIED,
02132 TriCam_REFERENCE,
02133 &rectifiedImage );
02134
02135 right = cv::Mat(rectifiedImage.nrows, rectifiedImage.ncols, CV_8UC1, rectifiedImage.data).clone();
02136
02137
02138 triclopsBuildPackedTriclopsInput(
02139 grabbedImage.GetCols(),
02140 grabbedImage.GetRows(),
02141 imageColorLeft.GetStride(),
02142 (unsigned long)grabbedImage.GetTimeStamp().seconds,
02143 (unsigned long)grabbedImage.GetTimeStamp().microSeconds,
02144 imageColorLeft.GetData(),
02145 &triclopsColorInputs );
02146
02147 cv::Mat pixelsLeftBuffer( grabbedImage.GetRows(), grabbedImage.GetCols(), CV_8UC4);
02148 TriclopsPackedColorImage colorImage;
02149 triclopsSetPackedColorImageBuffer(
02150 triclopsCtx_,
02151 TriCam_LEFT,
02152 (TriclopsPackedColorPixel*)pixelsLeftBuffer.data );
02153
02154 triclopsRectifyPackedColorImage(
02155 triclopsCtx_,
02156 TriCam_LEFT,
02157 &triclopsColorInputs,
02158 &colorImage );
02159
02160 cv::cvtColor(pixelsLeftBuffer, left, CV_RGBA2RGB);
02161
02162
02163 triclopsGetFocalLength(triclopsCtx_, &fx);
02164 triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
02165 triclopsGetBaseline(triclopsCtx_, &baseline);
02166 }
02167 }
02168 }
02169 }
02170 }
02171
02172 #else
02173 UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
02174 #endif
02175 }
02176
02177 }