Commits

Cliff Biffle committed bf50f78

Added projective transforms for mapping one frustum onto another.

  • Participants
  • Parent commits 60c7318

Comments (0)

Files changed (2)

 } vec3f_t;
 
 typedef struct {
+  float x, y;
+} vec2f_t;
+
+typedef struct {
   uint16_t samples[480][640];
 } depth_t;
 
   rgb_t pixels[480][640];
 } colors_t;
 
+// Describes how to map color data from one frustum
+// onto depth data from another.
+typedef struct {
+  // Offset of the color frustum's focal point, in
+  // pixels from the depth frustum's focal point.
+  vec3f_t offset;
+  // Ratios of the horizontal and vertical fields of
+  // view of the color camera to the corresponding
+  // parameters of the depth camera.  Corrects for
+  // differences in field-of-view.
+  vec2f_t scale;
+} projective_transform_t;
+
 #endif __types_h__
 	cross_product(&a, &b, out);
 }
 
+// Converts depth image coordinates to color image coordinates
+// using |xform|, given the linearized depth image reading |z|.
+static inline void apply_projective_transform(const projective_transform_t *xform, float z, int *x, int *y) {
+  static const float farScaleZ = 10.F;
+  
+  // Center the coordinates around the origin.
+  vec2f_t centered = { *x - 320, *y - 240 };
+
+  // Shift the origin.
+  vec2f_t displaced = {
+    centered.x + xform->offset.x,// / zRat,
+    centered.y + xform->offset.y,// / zRat,
+  };
+  float shiftedZ = z + xform->offset.z;
+
+  // Correct the field of view
+  vec2f_t scale = {
+    (shiftedZ / farScaleZ) * xform->scale.x,
+    (shiftedZ / farScaleZ) * xform->scale.y,
+  };
+  vec2f_t scaled = {
+    displaced.x * scale.x,
+    displaced.y * scale.y,
+  };
+  
+  *x = ((int) scaled.x) + 320;
+  *y = ((int) scaled.y) + 240;
+}
+
 #endif __utility_h__