diff --git a/CMakeLists.txt b/CMakeLists.txt index 580c11fe..38344f17 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,6 +134,7 @@ IF("${VTK_MAJOR_VERSION}" EQUAL 5) ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5) FIND_PACKAGE(ZLIB REQUIRED) FIND_PACKAGE(Freenect) +FIND_PACKAGE(freenect2 QUIET) FIND_PACKAGE(OpenNI2) FIND_PACKAGE(DC1394) FIND_PACKAGE(G2O) @@ -344,6 +345,12 @@ ELSE() MESSAGE(STATUS " With OpenNI2 = NO (OpenNI2 not found)") ENDIF() +IF(freenect2_FOUND) +MESSAGE(STATUS " With Freenect2 = YES") +ELSE() +MESSAGE(STATUS " With Freenect2 = NO (libfreenect2 not found)") +ENDIF() + IF(DC1394_FOUND) MESSAGE(STATUS " With dc1394 = YES") ELSE() diff --git a/corelib/include/rtabmap/core/CameraRGBD.h b/corelib/include/rtabmap/core/CameraRGBD.h index 1a71547e..c71a7d7f 100644 --- a/corelib/include/rtabmap/core/CameraRGBD.h +++ b/corelib/include/rtabmap/core/CameraRGBD.h @@ -54,7 +54,14 @@ class VideoStream; namespace pcl { - class Grabber; +class Grabber; +} + +namespace libfreenect2 +{ +class Freenect2; +class Freenect2Device; +class SyncMultiFrameListener; } typedef struct _freenect_context freenect_context; @@ -265,4 +272,37 @@ private: FreenectDevice * freenectDevice_; }; +///////////////////////// +// CameraFreenect2 +///////////////////////// + +class RTABMAP_EXP CameraFreenect2 : + public CameraRGBD +{ +public: + static bool available(); + +public: + // default local transform z in, x right, y down)); + CameraFreenect2(int deviceId= 0, + float imageRate=0.0f, + const Transform & localTransform = Transform::getIdentity(), + float fx = 0.0f, + float fy = 0.0f, + float cx = 0.0f, + float cy = 0.0f); + virtual ~CameraFreenect2(); + + bool init(); + +protected: + virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy); + +private: + int deviceId_; + libfreenect2::Freenect2 * freenect2_; + libfreenect2::Freenect2Device *dev_; + libfreenect2::SyncMultiFrameListener * listener_; +}; + } // namespace rtabmap diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 8f12ae10..0f13286e 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -83,6 +83,18 @@ IF(OpenNI2_FOUND) ) ENDIF(OpenNI2_FOUND) +IF(freenect2_FOUND) + ADD_DEFINITIONS("-DWITH_FREENECT2") + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${freenect2_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + ${freenect2_LIBRARIES} + ) +ENDIF(freenect2_FOUND) + IF(DC1394_FOUND) ADD_DEFINITIONS("-DWITH_DC1394") SET(INCLUDE_DIRS diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index dab81806..0a3f224d 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -51,6 +51,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #endif #endif +#ifdef WITH_FREENECT2 +#include +#include +#endif + #ifdef WITH_OPENNI2 #include #include @@ -940,20 +945,146 @@ void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, fl delete freenectDevice_; freenectDevice_ = 0; } - - if(depth.empty() || rgb.empty()) - { - rgb = cv::Mat(); - depth = cv::Mat(); - fx = 0.0f; - fy = 0.0f; - cx = 0.0f; - cy = 0.0f; - } + } + if(depth.empty() || rgb.empty()) + { + rgb = cv::Mat(); + depth = cv::Mat(); + fx = 0.0f; + fy = 0.0f; + cx = 0.0f; + cy = 0.0f; } #else UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!"); #endif } +// +// CameraFreenect2 +// +bool CameraFreenect2::available() +{ +#ifdef WITH_FREENECT2 + return true; +#else + return false; +#endif +} + +CameraFreenect2::CameraFreenect2(int deviceId, float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) : + CameraRGBD(imageRate, localTransform, fx, fy, cx, cy), + deviceId_(deviceId), + freenect2_(0), + dev_(0) +{ +#ifdef WITH_FREENECT2 + freenect2_ = new libfreenect2::Freenect2(); + //listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth); + listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth); + UWARN("CameraFreenect2: Images are not yet registered!"); +#endif +} + +CameraFreenect2::~CameraFreenect2() +{ +#ifdef WITH_FREENECT2 + if(dev_) + { + dev_->stop(); + dev_->close(); + } + if(listener_) + { + delete listener_; + } + if(freenect2_) + { + delete freenect2_; + } +#endif +} + +bool CameraFreenect2::init() +{ +#ifdef WITH_FREENECT2 + if(dev_) + { + dev_->stop(); + dev_->close(); + dev_ = 0; + } + + if(deviceId_ <= 0) + { + dev_ = freenect2_->openDefaultDevice(); + } + else + { + dev_ = freenect2_->openDevice(deviceId_); + } + + if(dev_) + { + dev_->setColorFrameListener(listener_); + dev_->setIrAndDepthFrameListener(listener_); + dev_->start(); + UINFO("CameraFreenect2: device serial: %s", dev_->getSerialNumber().c_str()); + UINFO("CameraFreenect2: device firmware: %s", dev_->getFirmwareVersion().c_str()); + return true; + } + else + { + UERROR("CameraFreenect2: no device connected or failure opening the default one!"); + } +#else + UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!"); +#endif + return false; +} + +void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) +{ +#ifdef WITH_FREENECT2 + rgb = cv::Mat(); + depth = cv::Mat(); + fx = 0.0f; + fy = 0.0f; + cx = 0.0f; + cy = 0.0f; + if(dev_ && listener_) + { + libfreenect2::FrameMap frames; + if(listener_->waitForNewFrame(frames, 1000)) + { + libfreenect2::Frame *rgbFrame = frames[libfreenect2::Frame::Color]; + //libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir]; + libfreenect2::Frame *depthFrame = frames[libfreenect2::Frame::Depth]; + + if(rgbFrame && depthFrame) + { + cv::flip(cv::Mat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data), rgb, 1); + cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1); + cv::flip(depth, depth, 1); + libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams(); + fx = params.fx; + fy = params.fy; + cx = params.cx; + cy = params.cy; + } + + listener_->release(frames); + } + else + { + UWARN("CameraFreenect2: Failed to get frames! rtabmap should link on libusb of " + "libfreenect2, this can be done by setting LD_LIBRARY_PATH to " + "\"libfreenect2/depends/libusb/lib\""); + } + } +#else + UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!"); +#endif +} + } // namespace rtabmap diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index cd42c506..e4f74817 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -977,7 +977,7 @@ bool Rtabmap::process(const SensorData & data) { std::string rejectedMsg; UDEBUG("Check local transform between %d and %d", signature->id(), *iter); - double variance = -1.0; + double variance = 1.0; int inliers = -1; Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg, &inliers, &variance); if(!transform.isNull() && _globalLoopClosureIcpType > 0) @@ -1407,7 +1407,7 @@ bool Rtabmap::process(const SensorData & data) { //Compute transform if metric data are present Transform transform; - double variance = -1; + double variance = 1; if(_rgbdSlamMode) { std::string rejectedMsg; diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index e62cf7e5..f73b4895 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -36,7 +36,12 @@ void showUsage() { printf("\nUsage:\n" "rtabmap-rgbd_camera driver\n" - " driver Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n"); + " driver Driver number to use: 0=OpenNI-PCL\n" + " 1=OpenNI2\n" + " 2=Freenect\n" + " 3=OpenNI-CV\n" + " 4=OpenNI-CV-ASUS\n" + " 5=Freenect2\n\n"); exit(1); } @@ -53,9 +58,9 @@ int main(int argc, char * argv[]) else { driver = atoi(argv[argc-1]); - if(driver < 0 || driver > 4) + if(driver < 0 || driver > 5) { - UERROR("driver should be between 0 and 4."); + UERROR("driver should be between 0 and 5."); showUsage(); } } @@ -64,7 +69,7 @@ int main(int argc, char * argv[]) rtabmap::CameraRGBD * camera = 0; if(driver == 0) { - camera = new rtabmap::CameraOpenni("", 0); + camera = new rtabmap::CameraOpenni(); } else if(driver == 1) { @@ -73,7 +78,7 @@ int main(int argc, char * argv[]) UERROR("Not built with OpenNI2 support..."); exit(-1); } - camera = new rtabmap::CameraOpenNI2(0); + camera = new rtabmap::CameraOpenNI2(); } else if(driver == 2) { @@ -82,7 +87,7 @@ int main(int argc, char * argv[]) UERROR("Not built with Freenect support..."); exit(-1); } - camera = new rtabmap::CameraFreenect(0, 0); + camera = new rtabmap::CameraFreenect(); } else if(driver == 3) { @@ -91,7 +96,7 @@ int main(int argc, char * argv[]) UERROR("Not built with OpenNI from OpenCV support..."); exit(-1); } - camera = new rtabmap::CameraOpenNICV(false, 0); + camera = new rtabmap::CameraOpenNICV(false); } else if(driver == 4) { @@ -100,7 +105,16 @@ int main(int argc, char * argv[]) UERROR("Not built with OpenNI from OpenCV support..."); exit(-1); } - camera = new rtabmap::CameraOpenNICV(true, 0); + camera = new rtabmap::CameraOpenNICV(true); + } + else if(driver == 5) + { + if(!rtabmap::CameraFreenect2::available()) + { + UERROR("Not built with Freenect2 support..."); + exit(-1); + } + camera = new rtabmap::CameraFreenect2(); } else { @@ -113,25 +127,36 @@ int main(int argc, char * argv[]) delete camera; exit(1); } - cv::Mat rgb, depth; float fx, fy, cx, cy; camera->takeImage(rgb, depth, fx, fy, cx, cy); + if(rgb.cols != depth.cols || rgb.rows != depth.rows) + { + UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.", + rgb.cols, rgb.rows, depth.cols, depth.rows); + } cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window cv::namedWindow("Depth", CV_WINDOW_AUTOSIZE); // create window pcl::visualization::CloudViewer viewer("cloud"); rtabmap::Transform opticalTransform(0,0,1,0, -1,0,0,0, 0,-1,0,0); while(!rgb.empty() && !viewer.wasStopped()) { + if(depth.type() == CV_32FC1) + { + depth = rtabmap::util3d::cvtDepthFromFloat(depth); + } cv::Mat tmp; depth.convertTo(tmp, CV_8UC1, 255.0/2048.0); cv::imshow("Video", rgb); // show frame - cv::imshow("Depth",tmp); + cv::imshow("Depth", tmp); - pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy); - cloud = rtabmap::util3d::transformPointCloud(cloud, opticalTransform); - viewer.showCloud(cloud, "cloud"); + if(rgb.cols == depth.cols && rgb.rows == depth.rows) + { + pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy); + cloud = rtabmap::util3d::transformPointCloud(cloud, opticalTransform); + viewer.showCloud(cloud, "cloud"); + } int c = cv::waitKey(10); // wait 10 ms or for key stroke if(c == 27)