Source

kinect-viewer / KVRayFieldSpaceModel.m

/*
 * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
 * Use of this source code is governed by the Apache License 2.0,
 * which can be found in the LICENSE file.
 */

#import "KVRayFieldSpaceModel.h"
#import "utility.h"

NSString * const KVPolygonMode = @"KVPolygonMode";
NSString * const KVFilledPolygonMode = @"KVFilledPolygonMode";
NSString * const KVWireframePolygonMode = @"KVWireframePolygonMode";
NSString * const KVDottedPolygonMode = @"KVDottedPolygonMode";

NSString * const KVLighting = @"KVLighting";
NSString * const KVNoLighting = @"KVNoLighting";
NSString * const KVDiffuseLighting = @"KVDiffuseLighting";

@interface KVRayFieldSpaceModel ()
- (void) buildLinearizationTable;
- (void) buildUnitRays;

@property(assign, nonatomic) double rayCastingTotal;
@property(assign, nonatomic) NSUInteger rayCastingCount;

@property(assign, nonatomic) double vertexPlacementTotal;
@property(assign, nonatomic) NSUInteger vertexPlacementCount;

- (void) renderAsPointsWithOptions: (NSDictionary *) options;
- (void) renderAsMeshWithOptions: (NSDictionary *) options;
- (void) renderAsSolidWithOptions: (NSDictionary *) options;
@end

@implementation KVRayFieldSpaceModel

@synthesize rayCastingCount, rayCastingTotal, vertexPlacementCount, vertexPlacementTotal;

- init {
  if ((self = [super init])) {
    [self buildUnitRays];
    [self buildLinearizationTable];
  }
  return self;
}

#pragma mark --- Implementation of KVSpaceModel

- (void) setDepthSamples: (NSData *)data {
  NSTimeInterval start = [NSDate timeIntervalSinceReferenceDate];
  
  const uint16_t *sample = [data bytes];
  
  // gcc 4.2.1 is not clever enough to turn raster two-dimensional
  // subscripting (e.g. ray[y][x]) into a single incremented induction
  // variable -- we wind up doing integer multiplications on every
  // use!  So, we do it by hand: thus |vidx|.
  int vidx = 0;
  
  // Cast them rays
  for (int y = 0; y < 480; y++) {
    for (int x = 0; x < 640; x++, vidx++) {
      uint16_t s = sample[vidx];
      float z;
      if (s < 2048) {
        z = linearizationTable[s];
      } else {
        z = 0;
      }
      
      rays[vidx] = (vec3f_t) {
        .x = unitX[x] * z,
        .y = unitY[y] * z,
        .z = z,
      };
    }
  }
  
  // Orient them normals
  for (int y = 0; y < 480 - 1; y++) {
    vidx = 640 * y;
    for (int x = 0; x < 640 - 1; x++, vidx++) {
      normals[vidx] = compute_normal(rays[vidx], rays[vidx + 640], rays[vidx + 1]);
    }
  }
  
  NSTimeInterval end = [NSDate timeIntervalSinceReferenceDate];
  rayCastingCount++;
  rayCastingTotal += end - start;
  
  if ((rayCastingCount & 0xF) == 0) {
    NSLog(@"Raycasting: latest %.03fµs mean %.03fµs",
          (end - start) * 1000000.,
          (rayCastingTotal / rayCastingCount) * 1000000.);
  }
}

- (void) setColorSamples: (NSData *)data {
  memcpy(&colorImage, [data bytes], sizeof colorImage);
}

- (void) drawInCurrentOpenGLContextWithOptions: (NSDictionary *) options {
  NSTimeInterval start = [NSDate timeIntervalSinceReferenceDate];

  id modeOption = [options valueForKey: KVPolygonMode];
  if (modeOption == KVWireframePolygonMode) {
    [self renderAsMeshWithOptions: options];
  } else if (modeOption == KVDottedPolygonMode) {
    [self renderAsPointsWithOptions: options];
  } else {
    [self renderAsSolidWithOptions: options];
  }
  
  NSTimeInterval end = [NSDate timeIntervalSinceReferenceDate];
  vertexPlacementCount++;
  vertexPlacementTotal += end - start;
  
  if ((vertexPlacementCount & 0xF) == 0) {
    NSLog(@"Vertex placement: latest %.03fµs mean %.03fµs",
          (end - start) * 1000000.,
          (vertexPlacementTotal / vertexPlacementCount) * 1000000.);
  }
  
}

#pragma mark --- Internals

- (void) renderAsPointsWithOptions: (NSDictionary *) options {
  glColor3f(1, 1, 1);
  glNormal3f(0, 0, -1);
  glEnableClientState(GL_VERTEX_ARRAY);
  glVertexPointer(3, GL_FLOAT, 0, &rays);
  glDrawArrays(GL_POINTS, 0, 640 * 480);
  glDisableClientState(GL_VERTEX_ARRAY);
}

- (void) renderAsSolidWithOptions: (NSDictionary *) options {
  BOOL lightsOn = ([options valueForKey: KVLighting] == KVDiffuseLighting);
  
  float jump = 0.05f;
  
  glColor3f(1, 1, 1);
  glNormal3f(0, 0, 1);
  
  int vidx = 0;
  
  for (int y = 0; y < 480 - 1; y++) {
    BOOL inStrip = NO;
    float lastDepth = rays[vidx].z;
    for (int x = 0; x < 640; x++, vidx++) {
      for (int step = 0; step < 640 * 2; step += 640) {
        float z = rays[vidx + step].z;
        float delta = z - lastDepth;
        lastDepth = z;
        
        if (delta < -jump || delta > jump || z == 0.f) {
          if (inStrip) {
            glEnd();
            inStrip = NO;
          }
        } else {
          if (!inStrip) {
            glBegin(GL_TRIANGLE_STRIP);
            inStrip = YES;
          }
          if (lightsOn) glNormal3fv(&normals[vidx + step].x);
          glVertex3fv(&rays[vidx + step].x);
        }
        
        if (!inStrip) break;
      }
    }
    if (inStrip) glEnd();
  }
}

- (void) renderAsMeshWithOptions: (NSDictionary *) options {
  float jump = 0.1f;
  
  glBegin(GL_LINES);
  glColor3f(1, 1, 1);
  int vidx = 0;
  for (int y = 0; y < 480 - 1; y++) {
    vidx = y * 640;
    for (int x = 0; x < 640 - 1; x++, vidx++) {
      float cornerZ = rays[vidx].z;
      if (cornerZ == 0.f) continue;
      
      float delta;
      
      delta = cornerZ - rays[vidx + 640].z;
      if (delta > -jump && delta < jump && delta != cornerZ) {
        glVertex3fv(&rays[vidx].x);
        glVertex3fv(&rays[vidx + 640].x);
      } 
      
      delta = cornerZ - rays[vidx + 1].z;
      if (delta > -jump && delta < jump && delta != cornerZ) {
        glVertex3fv(&rays[vidx].x);
        glVertex3fv(&rays[vidx + 1].x);
      } 
    }
  }
  glEnd();
}

// The Kinect produces 11-bit depth samples.  Z-resolution decreases
// with distance, following a curve that looks suspiciously like the
// one used for perspective projection.  The ROS folks at CCNY derived
// the equation below, for converting Kinect depth samples to meters.
// This method memoizes the function over the entire 2048-element
// domain.  This implies replacing a couple of floating-point constant
// loads, an addition, and a division with an indirection to very hot
// memory.  On my machine this eliminates 60% of time spent calculating
// linear depths.
- (void) buildLinearizationTable {
  for (uint16_t depth = 0; depth < 2048; depth++) {
    linearizationTable[depth] = -325.616F / ((GLfloat) depth + -1084.61F);
  }
  
  // Simplification: when the Kinect reports "no data" for a point
  // (typically because the IR projector is occluded) the result is
  // a "very close" reading of nearly zero meters.  All my renderers
  // filter out such readings, so we coerce it to exactly 0.F here
  // to simplify that.
  linearizationTable[0] = 0.F;
  linearizationTable[2047] = 0.F;
}

- (void) buildUnitRays {
  // We assume square pixels, and derive the Y FOV from the X.
  static const float fovXDegrees = 57.F;  // Pretty close to actual?
  
  const float fovX = fovXDegrees / 180.F * (float) M_PI;
  for (int y = 0; y < 240; y++) {
    float theta = ((y + 0.5F) / 320.F) * fovX/2.F;
    unitY[y + 240] = -sinf(theta);
    unitY[239 - y] = sinf(theta);
  }
  for (int x = 0; x < 320; x++) {
    float psi = ((x + 0.5F) / 320.F) * fovX/2.F;
    unitX[x + 320] = -sinf(psi);
    unitX[319 - x] = sinf(psi);
  }
}

@end