Added additional parameters for Kinect distance estimation.

data/kinect.cal

@@ -1,35 +1,37 @@
 [rgb]
-principal_point\x=313.67245
+principal_point\x=314.70228964
-principal_point\y=264.06175
+principal_point\y=264.30478827
-focal_length\x=527.44654
+focal_length\x=527.91246131
-focal_length\y=527.40652
+focal_length\y=527.91246131
-distortion\k1=0.20780
+distortion\k1=0.20496745
-distortion\k2=-0.34320
+distortion\k2=-0.36341243
-distortion\k3=0.00139
+distortion\k3=0.00000000
-distortion\k4=0.00061
+distortion\k4=0.00000000
-distortion\k5=0.00000
+distortion\k5=0.00000000
 
 [depth]
-principal_point\x=313.23400
+principal_point\x=311.88621344
-principal_point\y=246.13447
+principal_point\y=247.63447078
-focal_length\x=587.62150
+focal_length\x=593.89813561
-focal_length\y=587.51184
+focal_length\y=593.89813561
-distortion\k1=0.01063
+distortion\k1=0.00000000
-distortion\k2=-0.04479
+distortion\k2=0.00000000
-distortion\k3=-0.00073
+distortion\k3=0.00000000
-distortion\k4=0.00081
+distortion\k4=0.00000000
-distortion\k5=0.00000
+distortion\k5=0.00000000
 
 [transform]
-R11=0.99994
+R11=0.999982
-R12=0.00098102
+R12=0.000556
-R13=0.010900
+R13=0.005927
-R21=-0.00097894
+R21=-0.000563
-R22=1.0
+R22=0.999999
-R23=-0.00019534
+R23=0.001235
-R33=-0.010900
+R33=-0.005926
-R32=0.00018466
+R32=-0.001239
-R33=0.99994
+R33=0.999982
-Tx=-0.02581986
+Tx=-0.024287
-Ty=-0.0130948
+Ty=0.001018
-Tz=-0.0047681
+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 = baseline * depthCameraParameters.fx
-            double range = 1.0f / (-0.00307f * static_cast<float>(data[i]) + 3.33f);
+                           / (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;