Merge branch 'dev' of pixhawk.ethz.ch:qgroundcontrol into dev

data/kinect.cal

 [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
+principal_point\x=314.70228964
+principal_point\y=264.30478827
+focal_length\x=527.91246131
+focal_length\y=527.91246131
+distortion\k1=0.20496745
+distortion\k2=-0.36341243
+distortion\k3=0.00000000
+distortion\k4=0.00000000
+distortion\k5=0.00000000
 
 [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
+principal_point\x=311.88621344
+principal_point\y=247.63447078
+focal_length\x=593.89813561
+focal_length\y=593.89813561
+distortion\k1=0.00000000
+distortion\k2=0.00000000
+distortion\k3=0.00000000
+distortion\k4=0.00000000
+distortion\k5=0.00000000
 
 [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
+R11=0.999982
+R12=0.000556
+R13=0.005927
+R21=-0.000563
+R22=0.999999
+R23=0.001235
+R33=-0.005926
+R32=-0.001239
+R33=0.999982
+Tx=-0.024287
+Ty=0.001018
+Tz=-0.015195
+baseline=0.06061
+disparity_offset=1092.3403

src/input/Freenect.cc

@@ -208,8 +208,9 @@ Freenect::get3DPointCloudData(void)
     {
         if (data[i] > 0 && data[i] <= 2048)
         {
-            // see www.ros.org/wiki/kinect_node for details
-            double range = 1.0f / (-0.00307f * static_cast<float>(data[i]) + 3.33f);
+            double range = baseline * depthCameraParameters.fx
+                           / (1.0 / 8.0 * (disparityOffset
+                                           - static_cast<double>(data[i])));
 
             if (range > 0.0f)
             {
@@ -341,6 +342,9 @@ Freenect::readConfigFile(void)
                                  settings.value("transform/Tz").toDouble(),
                                  0.0, 0.0, 0.0, 1.0);
     transformMatrix = transformMatrix.transposed();
+
+    baseline = settings.value("transform/baseline").toDouble();
+    disparityOffset = settings.value("transform/disparity_offset").toDouble();
 }
 
 void

src/input/Freenect.h

@@ -119,6 +119,8 @@ private:
     IntrinsicCameraParameters depthCameraParameters;
 
     QMatrix4x4 transformMatrix;
+    double baseline;
+    double disparityOffset;
 
     // tilt angle of Kinect camera
     int tiltAngle;