Registration and Geometry

/**
 * Combine frames of depth and color camera.
 * 
 * This class uses a reverse engineered formula that uses factory
 * preset extrinsic parameters. We do not have a clear understanding of these
 * particular extrinsic parameters and do not know how to calibrate them by hand.
 * 
 * If you want to perform registration with standard camera extrinsic matrix,
 * you probably need something else.
 */
class Registration {
    /**
     * Constructor with camera parameters
     * @param depth_p Depth camera parameters (can use factory values or custom)
     * @param rgb_p Color camera parameters (recommend using factory values)
     */
    Registration(@ByVal Freenect2Device.IrCameraParams depth_p, 
                @ByVal Freenect2Device.ColorCameraParams rgb_p);
    
    /**
     * Undistort and register a single depth point to color camera
     * @param dx Distorted depth coordinate x (pixel)
     * @param dy Distorted depth coordinate y (pixel)  
     * @param dz Depth value (millimeter)
     * @param cx Output: Undistorted color coordinate x (normalized)
     * @param cy Output: Undistorted color coordinate y (normalized)
     */
    void apply(int dx, int dy, float dz, @ByRef FloatPointer cx, @ByRef FloatPointer cy);
    void apply(int dx, int dy, float dz, @ByRef FloatBuffer cx, @ByRef FloatBuffer cy);
    void apply(int dx, int dy, float dz, @ByRef float[] cx, @ByRef float[] cy);
    
    /**
     * Map color images onto depth images (full registration)
     * @param rgb Color image (1920x1080 BGRX)
     * @param depth Depth image (512x424 float)
     * @param undistorted Output: Undistorted depth image
     * @param registered Output: Color image for the depth image (512x424)
     */
    void apply(@Const Frame rgb, @Const Frame depth, Frame undistorted, Frame registered);
    
    /**
     * Full registration with additional options
     * @param rgb Color image (1920x1080 BGRX)
     * @param depth Depth image (512x424 float)
     * @param undistorted Output: Undistorted depth image
     * @param registered Output: Color image for depth image (512x424)
     * @param enable_filter Filter out pixels not visible to both cameras
     * @param bigdepth Output: Mapping of depth onto colors (1920x1082 float, optional)
     * @param color_depth_map Output: Index of mapped color pixel for each depth pixel (optional)
     */
    void apply(@Const Frame rgb, @Const Frame depth, Frame undistorted, Frame registered,
              @Cast("const bool") boolean enable_filter, Frame bigdepth, IntPointer color_depth_map);
    void apply(@Const Frame rgb, @Const Frame depth, Frame undistorted, Frame registered,
              @Cast("const bool") boolean enable_filter, Frame bigdepth, IntBuffer color_depth_map);
    void apply(@Const Frame rgb, @Const Frame depth, Frame undistorted, Frame registered,
              @Cast("const bool") boolean enable_filter, Frame bigdepth, int[] color_depth_map);
    
    /**
     * Undistort depth image only
     * @param depth Depth image (512x424 float)
     * @param undistorted Output: Undistorted depth image
     */
    void undistortDepth(@Const Frame depth, Frame undistorted);
    
    /**
     * Construct a 3-D point with color in a point cloud
     * @param undistorted Undistorted depth frame from apply()
     * @param registered Registered color frame from apply()
     * @param r Row (y) index in depth image
     * @param c Column (x) index in depth image
     * @param x Output: X coordinate of the 3-D point (meter)
     * @param y Output: Y coordinate of the 3-D point (meter)
     * @param z Output: Z coordinate of the 3-D point (meter)
     * @param rgb Output: Color of the 3-D point (BGRX format)
     */
    void getPointXYZRGB(@Const Frame undistorted, @Const Frame registered, int r, int c,
                       @ByRef FloatPointer x, @ByRef FloatPointer y, @ByRef FloatPointer z, @ByRef FloatPointer rgb);
    void getPointXYZRGB(@Const Frame undistorted, @Const Frame registered, int r, int c,
                       @ByRef FloatBuffer x, @ByRef FloatBuffer y, @ByRef FloatBuffer z, @ByRef FloatBuffer rgb);
    void getPointXYZRGB(@Const Frame undistorted, @Const Frame registered, int r, int c,
                       @ByRef float[] x, @ByRef float[] y, @ByRef float[] z, @ByRef float[] rgb);
    
    /**
     * Construct a 3-D point in a point cloud (without color)
     * @param undistorted Undistorted depth frame from apply()
     * @param r Row (y) index in depth image
     * @param c Column (x) index in depth image
     * @param x Output: X coordinate of the 3-D point (meter)
     * @param y Output: Y coordinate of the 3-D point (meter)
     * @param z Output: Z coordinate of the 3-D point (meter)
     */
    void getPointXYZ(@Const Frame undistorted, int r, int c,
                    @ByRef FloatPointer x, @ByRef FloatPointer y, @ByRef FloatPointer z);
    void getPointXYZ(@Const Frame undistorted, int r, int c,
                    @ByRef FloatBuffer x, @ByRef FloatBuffer y, @ByRef FloatBuffer z);
    void getPointXYZ(@Const Frame undistorted, int r, int c,
                    @ByRef float[] x, @ByRef float[] y, @ByRef float[] z);
}

// Initialize registration with device parameters
Freenect2Device.IrCameraParams irParams = device.getIrCameraParams();
Freenect2Device.ColorCameraParams colorParams = device.getColorCameraParams();
Registration registration = new Registration(irParams, colorParams);

// Capture synchronized frames
FrameMap frames = new FrameMap();
if (listener.waitForNewFrame(frames, 10000)) {
    Frame color = frames.get(Frame.Color);
    Frame depth = frames.get(Frame.Depth);
    
    // Create output frames
    Frame undistorted = new Frame(512, 424, 4); // float depth
    Frame registered = new Frame(512, 424, 4);  // BGRX color
    
    // Perform registration
    registration.apply(color, depth, undistorted, registered);
    
    System.out.println("Registration complete");
    
    listener.release(frames);
}

// Single point registration
float[] cx = new float[1];
float[] cy = new float[1];
registration.apply(256, 212, 1000.0f, cx, cy); // Center pixel at 1m depth
System.out.println("Depth point maps to color coordinates: " + cx[0] + ", " + cy[0]);

// Generate point cloud from registered frames
Frame undistorted = new Frame(512, 424, 4);
Frame registered = new Frame(512, 424, 4);
registration.apply(color, depth, undistorted, registered);

// Create point cloud data structures
int width = (int)undistorted.width();
int height = (int)undistorted.height();
float[] pointCloud = new float[width * height * 3]; // x,y,z per point
int[] colorData = new int[width * height];          // BGRX per point

// Extract point cloud
float[] x = new float[1], y = new float[1], z = new float[1];
float[] rgb = new float[1];

int pointIndex = 0;
for (int r = 0; r < height; r++) {
    for (int c = 0; c < width; c++) {
        registration.getPointXYZRGB(undistorted, registered, r, c, x, y, z, rgb);
        
        // Store 3D coordinates (in meters)
        pointCloud[pointIndex * 3] = x[0];
        pointCloud[pointIndex * 3 + 1] = y[0];
        pointCloud[pointIndex * 3 + 2] = z[0];
        
        // Store color (unpack BGRX)
        int rgbInt = Float.floatToIntBits(rgb[0]);
        colorData[pointIndex] = rgbInt;
        
        pointIndex++;
    }
}

// Filter valid points (remove NaN and infinite values)
List<float[]> validPoints = new ArrayList<>();
List<Integer> validColors = new ArrayList<>();

for (int i = 0; i < pointIndex; i++) {
    float px = pointCloud[i * 3];
    float py = pointCloud[i * 3 + 1];
    float pz = pointCloud[i * 3 + 2];
    
    if (Float.isFinite(px) && Float.isFinite(py) && Float.isFinite(pz) && pz > 0) {
        validPoints.add(new float[]{px, py, pz});
        validColors.add(colorData[i]);
    }
}

System.out.println("Generated " + validPoints.size() + " valid 3D points");

// Full registration with filtering and additional outputs
Frame bigdepth = new Frame(1920, 1082, 4); // Note: 1082 not 1080
int[] colorDepthMap = new int[512 * 424];

registration.apply(
    color, depth,           // Input frames
    undistorted, registered, // Standard outputs
    true,                   // Enable filtering
    bigdepth,              // Depth mapped to color resolution
    colorDepthMap          // Per-depth-pixel color mapping
);

// Process big depth image (depth data at color resolution)
System.out.println("Big depth dimensions: " + bigdepth.width() + "x" + bigdepth.height());

// Use color-depth mapping for efficient lookups
BytePointer colorData = registered.data();
for (int depthPixel = 0; depthPixel < colorDepthMap.length; depthPixel++) {
    int colorPixel = colorDepthMap[depthPixel];
    if (colorPixel >= 0) {
        // Valid mapping from depth pixel to color pixel
        int colorOffset = colorPixel * 4; // BGRX format
        // Access color data at colorData + colorOffset
    }
}

/**
 * Implementation details for registration
 */
class RegistrationImpl {
    // Internal implementation - not for direct use
}

// Examine factory calibration parameters
Freenect2Device.IrCameraParams irParams = device.getIrCameraParams();
System.out.println("IR Camera Intrinsics:");
System.out.println("  fx: " + irParams.fx() + ", fy: " + irParams.fy());
System.out.println("  cx: " + irParams.cx() + ", cy: " + irParams.cy());
System.out.println("  Distortion - k1: " + irParams.k1() + ", k2: " + irParams.k2());

Freenect2Device.ColorCameraParams colorParams = device.getColorCameraParams();
System.out.println("Color Camera Intrinsics:");
System.out.println("  fx: " + colorParams.fx() + ", fy: " + colorParams.fy());
System.out.println("  cx: " + colorParams.cx() + ", cy: " + colorParams.cy());
System.out.println("  Extrinsic shift: " + colorParams.shift_d() + ", " + colorParams.shift_m());

// Use custom calibrated parameters (advanced users)
irParams.fx(365.456f);  // Custom focal length
irParams.fy(365.456f);
irParams.cx(257.608f);  // Custom principal point
irParams.cy(210.040f);
device.setIrCameraParams(irParams);

// Create registration with custom parameters
Registration customRegistration = new Registration(irParams, colorParams);

// Unpack BGRX color data
float[] rgbFloat = new float[1];
registration.getPointXYZRGB(undistorted, registered, r, c, x, y, z, rgbFloat);

int rgbPacked = Float.floatToIntBits(rgbFloat[0]);
int blue = rgbPacked & 0xFF;
int green = (rgbPacked >> 8) & 0xFF;
int red = (rgbPacked >> 16) & 0xFF;
// Alpha/unused byte is (rgbPacked >> 24) & 0xFF

System.out.println("Point color - R: " + red + ", G: " + green + ", B: " + blue);