From c2f6439567a01cf11623033ac12943726f74e2dd Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 14 Jun 2014 16:09:44 +0000 Subject: [PATCH] Fixed focal computation when images are registered (freenect), Updated FindOpenNI2.cmake for unix git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1364 f169173b-cf89-36c8-b27e-44dbe73f0c83 --- cmake_modules/FindOpenNI2.cmake | 6 ++-- corelib/include/rtabmap/core/CameraFreenect.h | 2 ++ corelib/src/Camera.cpp | 14 ++++++-- corelib/src/CameraFreenect.cpp | 32 ++++++++++++++++--- corelib/src/CameraOpenni.cpp | 2 ++ 5 files changed, 48 insertions(+), 8 deletions(-) diff --git a/cmake_modules/FindOpenNI2.cmake b/cmake_modules/FindOpenNI2.cmake index b0839f88..b62d690b 100644 --- a/cmake_modules/FindOpenNI2.cmake +++ b/cmake_modules/FindOpenNI2.cmake @@ -6,10 +6,12 @@ # OpenNI2_INCLUDE_DIRS - The OpenNI2 include directory. # OpenNI2_LIBRARIES - The OpenNI2 library to link against. -FIND_PATH(OpenNI2_INCLUDE_DIRS OpenNI.h HINTS $ENV{OPENNI2_INCLUDE64} $ENV{OPENNI2_INCLUDE}) +FIND_PATH(OpenNI2_INCLUDE_DIRS OpenNI.h HINTS $ENV{OPENNI2_INCLUDE64} $ENV{OPENNI2_INCLUDE} PATH_SUFFIXES openni2) FIND_LIBRARY(OpenNI2_LIBRARY NAMES OpenNI2 HINTS $ENV{OPENNI2_LIB64} $ENV{OPENNI2_LIB}) +MESSAGE(STATUS "OpenNI2_INCLUDE_DIRS=${OpenNI2_INCLUDE_DIRS} OpenNI2_LIBRARY=${OpenNI2_LIBRARY}") + IF (OpenNI2_INCLUDE_DIRS AND OpenNI2_LIBRARY) SET(OpenNI2_FOUND TRUE) ENDIF (OpenNI2_INCLUDE_DIRS AND OpenNI2_LIBRARY) @@ -23,7 +25,7 @@ IF (OpenNI2_FOUND) ELSE (OpenNI2_FOUND) # fatal error if OpenNI2 is required but not found IF (OpenNI2_FIND_REQUIRED) - MESSAGE(FATAL_ERROR "Could not find OpenNI2") + MESSAGE(FATAL_ERROR "Could not find OpenNI2. Environment variables OPENNI2_INCLUDE (directory containing OpenNI.h) and OPENNI2_LIB (directory containing OpenNI2 library) could bet set.") ENDIF (OpenNI2_FIND_REQUIRED) ENDIF (OpenNI2_FOUND) diff --git a/corelib/include/rtabmap/core/CameraFreenect.h b/corelib/include/rtabmap/core/CameraFreenect.h index a88d94ec..8749018c 100644 --- a/corelib/include/rtabmap/core/CameraFreenect.h +++ b/corelib/include/rtabmap/core/CameraFreenect.h @@ -36,6 +36,7 @@ class RTABMAP_EXP FreenectDevice { cv::Mat getRgb(); cv::Mat getDepth(); + float getDepthFocal() const {return depthFocal_;} // Do not call directly even in child void VideoCallback(void *video, uint32_t timestamp); @@ -59,6 +60,7 @@ class RTABMAP_EXP FreenectDevice { UMutex rgbMutex_; bool depthReady_; bool rgbReady_; + float depthFocal_; }; class RTABMAP_EXP CameraFreenect : public UEventsSender, public UThread diff --git a/corelib/src/Camera.cpp b/corelib/src/Camera.cpp index efaa0eb4..00d569bd 100644 --- a/corelib/src/Camera.cpp +++ b/corelib/src/Camera.cpp @@ -425,8 +425,10 @@ bool CameraRGBD::init() UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_PROP_FRAME_WIDTH )); UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_PROP_FRAME_HEIGHT )); UINFO("FRAME_MAX_DEPTH %f mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH )); + UINFO("BASELINE %f mm", _capture.get( CV_CAP_PROP_OPENNI_BASELINE )); UINFO("FPS %f", _capture.get( CV_CAP_PROP_FPS )); - UINFO("Focal %f", _depthFocal); + UINFO("Focal %f", _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH )); + UINFO("Focal2 %f", _capture.get( CV_CAP_PROP_OPENNI_FOCAL_LENGTH )); UINFO("REGISTRATION %f", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION )); if(_capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) == 0.0) { @@ -606,7 +608,15 @@ bool CameraOpenNI2::init() _depth->getHorizontalFieldOfView(), _depth->getVerticalFieldOfView()); - _depthFocal = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f); + bool registered = true; + if(registered) + { + _depthFocal = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f); + } + else + { + _depthFocal = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f); + } UINFO("depth focal = %f", _depthFocal); UINFO("CameraOpenNI2: Using color video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad", diff --git a/corelib/src/CameraFreenect.cpp b/corelib/src/CameraFreenect.cpp index 0c639ca3..dc9f3b1f 100644 --- a/corelib/src/CameraFreenect.cpp +++ b/corelib/src/CameraFreenect.cpp @@ -15,6 +15,7 @@ #ifdef WITH_FREENECT #include +#include #endif namespace rtabmap { @@ -29,7 +30,8 @@ FreenectDevice::FreenectDevice(freenect_context * ctx, int index) : depthMat_(cv::Size(640,480),CV_16UC1), rgbMat_(cv::Size(640,480), CV_8UC3, cv::Scalar(0)), depthReady_(false), - rgbReady_(false) + rgbReady_(false), + depthFocal_(0.0f) { UASSERT(ctx_ != 0); } @@ -55,7 +57,7 @@ bool FreenectDevice::init() { if(freenect_open_device(ctx_, &device_, index_) < 0) { - UERROR("Cannot open Kinect"); + UERROR("FreenectDevice: Cannot open Kinect"); return false; } freenect_set_user(device_, this); @@ -63,6 +65,28 @@ bool FreenectDevice::init() freenect_set_depth_mode(device_, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED)); freenect_set_depth_callback(device_, freenect_depth_callback); freenect_set_video_callback(device_, freenect_video_callback); + + bool registered = true; + float rgb_focal_length_sxga = 1050.0f; + float width_sxga = 1280.0f; + float width = freenect_get_current_depth_mode(device_).width; + float scale = width / width_sxga; + if(registered) + { + depthFocal_ = rgb_focal_length_sxga * scale; + } + else + { + freenect_registration reg = freenect_copy_registration(device_); + float depth_focal_length_sxga = reg.zero_plane_info.reference_distance / reg.zero_plane_info.reference_pixel_size; + freenect_destroy_registration(®); + + depthFocal_ = depth_focal_length_sxga * scale; + } + + UINFO("FreenectDevice: Depth focal = %f", depthFocal_); + + return true; } @@ -272,8 +296,8 @@ void CameraFreenect::mainLoop() UWARN("CameraFreenect: Rgb not ready! Try to reduce the image rate to avoid this warning..."); return; } - - float constant = 0.001905f; + UASSERT(freenectDevice_->getDepthFocal() != 0.0f); + float constant = 1.0f/freenectDevice_->getDepthFocal(); this->post(new CameraEvent(rgb, depth, constant, localTransform_, ++seq_)); } } diff --git a/corelib/src/CameraOpenni.cpp b/corelib/src/CameraOpenni.cpp index 5f1f01c3..5b6b758a 100644 --- a/corelib/src/CameraOpenni.cpp +++ b/corelib/src/CameraOpenni.cpp @@ -64,6 +64,8 @@ void CameraOpenni::image_cb ( cv::Mat depthFrame(rgb->getHeight(), rgb->getWidth(), CV_16UC1); depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depthFrame.data); + UINFO("constant=%f focal=%f", constant, 1.0f/constant); + this->post(new CameraEvent(bgrFrame, depthFrame, constant, localTransform_, ++seq_)); }