From 899e0baeb4e162f4e491a7ca484c66d792697abb Mon Sep 17 00:00:00 2001 From: Lionel Heng Date: Tue, 7 Dec 2010 01:00:25 +0100 Subject: [PATCH] Kinect parameters are now read from a calibration file. --- data/kinect.cal | 35 +++++++++++++ src/input/Freenect.cc | 111 +++++++++++++++++++++++------------------- src/input/Freenect.h | 2 + 3 files changed, 99 insertions(+), 49 deletions(-) create mode 100644 data/kinect.cal diff --git a/data/kinect.cal b/data/kinect.cal new file mode 100644 index 000000000..432c70091 --- /dev/null +++ b/data/kinect.cal @@ -0,0 +1,35 @@ +[rgb] +principal_point\x=313.67245 +principal_point\y=264.06175 +focal_length\x=527.44654 +focal_length\y=527.40652 +distortion\k1=0.20780 +distortion\k2=-0.34320 +distortion\k3=0.00139 +distortion\k4=0.00061 +distortion\k5=0.00000 + +[depth] +principal_point\x=313.23400 +principal_point\y=246.13447 +focal_length\x=587.62150 +focal_length\y=587.51184 +distortion\k1=0.01063 +distortion\k2=-0.04479 +distortion\k3=-0.00073 +distortion\k4=0.00081 +distortion\k5=0.00000 + +[transform] +R11=0.99994 +R12=0.00098102 +R13=0.010900 +R21=-0.00097894 +R22=1.0 +R23=-0.00019534 +R33=-0.010900 +R32=0.00018466 +R33=0.99994 +Tx=-0.02581986 +Ty=-0.0130948 +Tz=-0.0047681 diff --git a/src/input/Freenect.cc b/src/input/Freenect.cc index 90cb5e06b..6ad5c1f27 100644 --- a/src/input/Freenect.cc +++ b/src/input/Freenect.cc @@ -33,45 +33,34 @@ This file is part of the QGROUNDCONTROL project #include #include +#include Freenect::Freenect() : context(NULL) , device(NULL) , tiltAngle(0) { - // default rgb camera parameters - rgbCameraParameters.cx = 3.2894272028759258e+02; - rgbCameraParameters.cy = 2.6748068171871557e+02; - rgbCameraParameters.fx = 5.2921508098293293e+02; - rgbCameraParameters.fy = 5.2556393630057437e+02; - rgbCameraParameters.k[0] = 2.6451622333009589e-01; - rgbCameraParameters.k[1] = -8.3990749424620825e-01; - rgbCameraParameters.k[2] = -1.9922302173693159e-03; - rgbCameraParameters.k[3] = 1.4371995932897616e-03; - rgbCameraParameters.k[4] = 9.1192465078713847e-01; - - // default depth camera parameters - depthCameraParameters.cx = 3.3930780975300314e+02; - depthCameraParameters.cy = 2.4273913761751615e+02; - depthCameraParameters.fx = 5.9421434211923247e+02; - depthCameraParameters.fy = 5.9104053696870778e+02; - depthCameraParameters.k[0] = -2.6386489753128833e-01; - depthCameraParameters.k[1] = 9.9966832163729757e-01; - depthCameraParameters.k[2] = -7.6275862143610667e-04; - depthCameraParameters.k[3] = 5.0350940090814270e-03; - depthCameraParameters.k[4] = -1.3053628089976321e+00; - - // relative rotation/translation between cameras with depth camera as reference - transformMatrix = QMatrix4x4(9.9984628826577793e-01, 1.2635359098409581e-03, - -1.7487233004436643e-02, 1.9985242312092553e-02, - -1.4779096108364480e-03, 9.9992385683542895e-01, - -1.2251380107679535e-02, -7.4423738761617583e-04, - 1.7470421412464927e-02, 1.2275341476520762e-02, - 9.9977202419716948e-01, -1.0916736334336222e-02, - 0.0, 0.0, 0.0, 1.0); - // relative rotation/translation between cameras with rgb camera as reference - transformMatrix = transformMatrix.transposed(); +} + +Freenect::~Freenect() +{ + if (device != NULL) + { + freenect_stop_depth(device); + freenect_stop_video(device); + } + + freenect_close_device(device); + + freenect_shutdown(context); +} + +bool +Freenect::init(int userDeviceNumber) +{ + // read in settings + readConfigFile(); // populate gamma lookup table for (int i = 0; i < 2048; ++i) @@ -99,24 +88,7 @@ Freenect::Freenect() rgbRectificationMap[i * FREENECT_FRAME_W + j] = rectifiedPoint; } } -} -Freenect::~Freenect() -{ - if (device != NULL) - { - freenect_stop_depth(device); - freenect_stop_video(device); - } - - freenect_close_device(device); - - freenect_shutdown(context); -} - -bool -Freenect::init(int userDeviceNumber) -{ if (freenect_init(&context, NULL) < 0) { return false; @@ -321,6 +293,47 @@ Freenect::FreenectThread::run(void) } } +void +Freenect::readConfigFile(void) +{ + QSettings settings("data/kinect.cal", QSettings::IniFormat, 0); + + rgbCameraParameters.cx = settings.value("rgb/principal_point/x").toDouble(); + rgbCameraParameters.cy = settings.value("rgb/principal_point/y").toDouble(); + rgbCameraParameters.fx = settings.value("rgb/focal_length/x").toDouble(); + rgbCameraParameters.fy = settings.value("rgb/focal_length/y").toDouble(); + rgbCameraParameters.k[0] = settings.value("rgb/distortion/k1").toDouble(); + rgbCameraParameters.k[1] = settings.value("rgb/distortion/k2").toDouble(); + rgbCameraParameters.k[2] = settings.value("rgb/distortion/k3").toDouble(); + rgbCameraParameters.k[3] = settings.value("rgb/distortion/k4").toDouble(); + rgbCameraParameters.k[4] = settings.value("rgb/distortion/k5").toDouble(); + + depthCameraParameters.cx = settings.value("depth/principal_point/x").toDouble(); + depthCameraParameters.cy = settings.value("depth/principal_point/y").toDouble(); + depthCameraParameters.fx = settings.value("depth/focal_length/x").toDouble(); + depthCameraParameters.fy = settings.value("depth/focal_length/y").toDouble(); + depthCameraParameters.k[0] = settings.value("depth/distortion/k1").toDouble(); + depthCameraParameters.k[1] = settings.value("depth/distortion/k2").toDouble(); + depthCameraParameters.k[2] = settings.value("depth/distortion/k3").toDouble(); + depthCameraParameters.k[3] = settings.value("depth/distortion/k4").toDouble(); + depthCameraParameters.k[4] = settings.value("depth/distortion/k5").toDouble(); + + transformMatrix = QMatrix4x4(settings.value("transform/R11").toDouble(), + settings.value("transform/R12").toDouble(), + settings.value("transform/R13").toDouble(), + settings.value("transform/Tx").toDouble(), + settings.value("transform/R21").toDouble(), + settings.value("transform/R22").toDouble(), + settings.value("transform/R23").toDouble(), + settings.value("transform/Ty").toDouble(), + settings.value("transform/R31").toDouble(), + settings.value("transform/R32").toDouble(), + settings.value("transform/R33").toDouble(), + settings.value("transform/Tz").toDouble(), + 0.0, 0.0, 0.0, 1.0); + transformMatrix = transformMatrix.transposed(); +} + void Freenect::rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint, diff --git a/src/input/Freenect.h b/src/input/Freenect.h index 88b915502..6f8f39123 100644 --- a/src/input/Freenect.h +++ b/src/input/Freenect.h @@ -86,6 +86,8 @@ private: } IntrinsicCameraParameters; + void readConfigFile(void); + void rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint, const IntrinsicCameraParameters& params); -- 2.22.0