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,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)
 

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

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",

corelib/src/CameraFreenect.cpp

@@ -15,6 +15,7 @@
 
 #ifdef WITH_FREENECT
 #include <libfreenect.h>
+#include <libfreenect-registration.h>
 #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(&reg);
+
+		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;
 			}
-
+			UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
-			float constant = 0.001905f;
+			float constant = 1.0f/freenectDevice_->getDepthFocal();
 			this->post(new CameraEvent(rgb, depth, constant, localTransform_, ++seq_));
 		}
 	}

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_));
 }