primesense_depth.hpp

#ifndef __fovis_primesense_depth_hpp__
#define __fovis_primesense_depth_hpp__

#include <inttypes.h>
#include <assert.h>
#include "feature_match.hpp"
#include "camera_intrinsics.hpp"
#include "rectification.hpp"
#include "depth_source.hpp"

namespace fovis
{

struct PrimeSenseCalibrationParameters
{
  int width;
  int height;

  double shift_offset;
  double projector_depth_baseline;

  double depth_to_rgb_translation[3];
  double depth_to_rgb_quaternion[4];

  CameraIntrinsicsParameters depth_params;
  CameraIntrinsicsParameters rgb_params;
};

class PrimeSenseCalibration
{
  public:
    PrimeSenseCalibration(const PrimeSenseCalibrationParameters& params);
    ~PrimeSenseCalibration();

    Eigen::Matrix4d getDepthUvdToDepthXyz() const {
      double fx_inv = 1 / params.depth_params.fx;
      double pdb_inv = 1 / params.projector_depth_baseline;
      double a = -0.125 * fx_inv * pdb_inv;
      double b = 0.125 * params.shift_offset * fx_inv * pdb_inv;
      double cx = params.depth_params.cx;
      double cy = params.depth_params.cy;
      Eigen::Matrix4d result;
      result <<
        fx_inv, 0, 0, -cx * fx_inv,
        0, fx_inv, 0, -cy * fx_inv,
        0, 0, 0, 1,
        0, 0, a, b;
      return result;
    }

    Eigen::Isometry3d getDepthXyzToRgbXyz() const {
      Eigen::Isometry3d result = Eigen::Isometry3d::Identity();
      result.translate(Eigen::Vector3d(params.depth_to_rgb_translation));
      const double *q = params.depth_to_rgb_quaternion;
      result.rotate(Eigen::Quaterniond(q[0], q[1], q[2], q[3]));
      return result;
    }

    Eigen::Matrix<double, 3, 4> getRgbXyzToRgbUvw() const {
      return rgb_rectification->getInputCameraParameters().toProjectionMatrix();
    }

    // ============= convenience functions =================

    Eigen::Matrix<double, 3, 4> getDepthUvdToRgbUvw() const {
      return getRgbXyzToRgbUvw() * getDepthXyzToRgbXyz().matrix() * getDepthUvdToDepthXyz();
    }

    Eigen::Matrix4d getDepthUvdToRgbXyz() const {
      return getDepthXyzToRgbXyz().matrix() * getDepthUvdToDepthXyz();
    }

    const PrimeSenseCalibrationParameters& getParameters() const {
      return params;
    }

    const Rectification* getRgbRectification() const {
      return rgb_rectification;
    }

  private:
    PrimeSenseCalibrationParameters params;

    Rectification* rgb_rectification;
};

class PrimeSenseDepth : public DepthSource
{
  public:
    PrimeSenseDepth(const PrimeSenseCalibration* calib);
    ~PrimeSenseDepth();

    void setDisparityData(const uint16_t* disparity);

    bool getXyz(int u, int v, Eigen::Vector3d & xyz);
    void getXyz(OdometryFrame * frame);
    void refineXyz(FeatureMatch * matches,
                   int num_matches,
                   OdometryFrame * frame);

    inline uint16_t getDisparity(int rgb_u, int rgb_v) const;

    inline uint16_t getDisparityAtIRPixel(int ir_u, int ir_v) const;

    inline double getBaseline() const;

    inline bool haveXyz(int u, int v);

  private:
    bool haveXyz(float u_f, float v_f);

    bool getXyzFast(KeypointData* kpdata);
    bool getXyzInterp(KeypointData* kpdata);

    int _ir_width;
    int _ir_height;

    const PrimeSenseCalibration* _calib;

    uint16_t* _disparity;

    // for each RGB camera pixel, stores the index of the corresponding
    // disparity image pixel (v*width + u)
    uint32_t* _rgb_to_disparity_map;

    uint32_t _disparity_map_max_index;

    Eigen::Matrix4d* _uvd1_d_to_xyz_c;
};

uint16_t
PrimeSenseDepth::getDisparity(int u, int v) const
{
  int rgb_index = v * _ir_width + u;
  uint32_t disparity_index = _rgb_to_disparity_map[rgb_index];
  if(disparity_index < _disparity_map_max_index) {
    return _disparity[disparity_index];
  } else {
    return 0;
  }
}

uint16_t
PrimeSenseDepth::getDisparityAtIRPixel(int ir_u, int ir_v) const
{
  assert(ir_u >= 0 && ir_v >= 0 && ir_u < _ir_width && ir_v < _ir_height);
  return _disparity[ir_v * _ir_width + ir_u];
}

double
PrimeSenseDepth::getBaseline() const
{
  return _calib->getParameters().projector_depth_baseline;
}

bool
PrimeSenseDepth::haveXyz(int u, int v)
{
  int rgb_index = v * _ir_width + u;
  uint32_t disparity_index = _rgb_to_disparity_map[rgb_index];
  return disparity_index < _disparity_map_max_index;
}

}
#endif