rectification.hpp

#ifndef __fovis_rectification_hpp__
#define __fovis_rectification_hpp__

#include <inttypes.h>

#include <Eigen/Core>

#include "camera_intrinsics.hpp"

namespace fovis
{

class Rectification
{
  public:

    Rectification(const CameraIntrinsicsParameters& input_camera_params);

    Rectification(const CameraIntrinsicsParameters& input_camera_params,
            const Eigen::Matrix3d& rotation,
            const CameraIntrinsicsParameters& rectified_camera_params);

    ~Rectification();

    const CameraIntrinsicsParameters& getInputCameraParameters() const {
      return _input_camera;
    }

    const Eigen::Matrix3d& getRectificationRotation() const {
      return *_rotation;
    }

    const CameraIntrinsicsParameters& getRectifiedCameraParameters() const {
      return _rectified_camera;
    }

    void rectifyLookup(int dist_u, int dist_v,
                       Eigen::Vector2d* rect_uv) const
    {
      assert(dist_u >= 0 && dist_v >= 0 && dist_u < _input_camera.width && dist_v < _input_camera.height);
      int pixel_index = dist_v * _input_camera.width + dist_u;
      rect_uv->x() = _map_x[pixel_index];
      rect_uv->y() = _map_y[pixel_index];
    }

    void rectifyLookupByIndex(int pixel_index,
                              Eigen::Vector2d* rect_uv) const
    {
      rect_uv->x() = _map_x[pixel_index];
      rect_uv->y() = _map_y[pixel_index];
    }

    void rectifyBilinearLookup(const Eigen::Vector2d& dist_uv,
                               Eigen::Vector2d* rect_uv) const {
      int u = (int)dist_uv.x();
      int v = (int)dist_uv.y();

      assert(u >= 0 && v >= 0 && u < _input_camera.width && v < _input_camera.height);

      // weights
      float wright  = (dist_uv.x() - u);
      float wbottom = (dist_uv.y() - v);
      float w[4] = {
        (1 - wright) * (1 - wbottom),
        wright * (1 - wbottom),
        (1 - wright) * wbottom,
        wright * wbottom
      };

      int ra_index = v * _input_camera.width + u;
      int neighbor_indices[4] = {
        ra_index,
        ra_index + 1,
        ra_index + _input_camera.width,
        ra_index + _input_camera.width + 1
      };

      rect_uv->x() = 0;
      rect_uv->y() = 0;
      for(int i = 0; i < 4; ++i) {
        rect_uv->x() += w[i] * _map_x[neighbor_indices[i]];
        rect_uv->y() += w[i] * _map_y[neighbor_indices[i]];
      }
    }

    Rectification* makeCopy() const;

  private:
    Rectification() {}

    void populateMap();

    CameraIntrinsicsParameters _input_camera;

    Eigen::Matrix3d* _rotation;
    // use a pointer above to avoid EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    CameraIntrinsicsParameters _rectified_camera;

    float* _map_x;
    float* _map_y;
};

} /*  */

#endif