Don't like ads? PRO users don't see any ads ;-)
Public Pastes
Guest

Untitled

By: a guest on Jul 4th, 2012  |  syntax: None  |  size: 2.05 KB  |  hits: 9  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1. Using the CL NUI Kinect drivers, convert depth color to actual relative distance
  2. float RawDepthToMeters(int depthValue)
  3. {
  4.     if (depthValue < 2047)
  5.     {
  6.         return float(1.0 / (double(depthValue) * -0.0030711016 + 3.3309495161));
  7.     }
  8.     return 0.0f;
  9. }
  10.  
  11. Vec3f DepthToWorld(int x, int y, int depthValue)
  12. {
  13.     static const double fx_d = 1.0 / 5.9421434211923247e+02;
  14.     static const double fy_d = 1.0 / 5.9104053696870778e+02;
  15.     static const double cx_d = 3.3930780975300314e+02;
  16.     static const double cy_d = 2.4273913761751615e+02;
  17.  
  18.     Vec3f result;
  19.     const double depth = RawDepthToMeters(depthValue);
  20.     result.x = float((x - cx_d) * depth * fx_d);
  21.     result.y = float((y - cy_d) * depth * fy_d);
  22.     result.z = float(depth);
  23.     return result;
  24. }
  25.  
  26. Vec2i WorldToColor(const Vec3f &pt)
  27. {
  28.     static const Matrix4 rotationMatrix(
  29.                             Vec3f(9.9984628826577793e-01f, 1.2635359098409581e-03f, -1.7487233004436643e-02f),
  30.                             Vec3f(-1.4779096108364480e-03f, 9.9992385683542895e-01f, -1.2251380107679535e-02f),
  31.                             Vec3f(1.7470421412464927e-02f, 1.2275341476520762e-02f, 9.9977202419716948e-01f));
  32.     static const Vec3f translation(1.9985242312092553e-02f, -7.4423738761617583e-04f, -1.0916736334336222e-02f);
  33.     static const Matrix4 finalMatrix = rotationMatrix.Transpose() * Matrix4::Translation(-translation);
  34.  
  35.     static const double fx_rgb = 5.2921508098293293e+02;
  36.     static const double fy_rgb = 5.2556393630057437e+02;
  37.     static const double cx_rgb = 3.2894272028759258e+02;
  38.     static const double cy_rgb = 2.6748068171871557e+02;
  39.  
  40.     const Vec3f transformedPos = finalMatrix.TransformPoint(pt);
  41.     const float invZ = 1.0f / transformedPos.z;
  42.  
  43.     Vec2i result;
  44.     result.x = Utility::Bound(Math::Round((transformedPos.x * fx_rgb * invZ) + cx_rgb), 0, 639);
  45.     result.y = Utility::Bound(Math::Round((transformedPos.y * fy_rgb * invZ) + cy_rgb), 0, 479);
  46.     return result;
  47. }
  48.        
  49. private int colorToDepth(Color c) {
  50.         int colorInt = c.ToArgb();
  51.  
  52.         return (colorInt << 16) >> 16;
  53.     }
create a new version of this paste RAW Paste Data