i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed focal computation when images are registered (freenect), Updated FindOpenNI2.cmake for unix

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1364 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-06-14 16:09:44 +00:00
parent a943083e0b
commit c2f6439567
5 changed files with 48 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/CameraFreenect.h
View File

@@ -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

14
corelib/src/Camera.cpp
View File

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

32
corelib/src/CameraFreenect.cpp
View File

@@ -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;
}
float constant = 0.001905f;
UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
float constant = 1.0f/freenectDevice_->getDepthFocal();
this->post(new CameraEvent(rgb, depth, constant, localTransform_, ++seq_));
}
}

2
corelib/src/CameraOpenni.cpp
View File

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