calibration.hpp

#ifndef _CHRONOPTICS_TOF_CALIBRATION_HPP_
#define _CHRONOPTICS_TOF_CALIBRATION_HPP_
 
#include <chronoptics/tof/calibration.h>
 
#include <chronoptics/tof/data.hpp>
 
namespace chronoptics {
namespace tof {
 
/** 2D point for indexing into array
*/
class Point2d : public detail::Base<tof_point_2d, tof_point_2d_delete> {
 public:
  /** Construct from pointer */
  Point2d(tof_point_2d_t ptr = nullptr) {
    this->ptr_ = ptr;
  }
 
  /** Get the row value
   * @return Row value
   */
  int32_t row() {
    return tof_point_2d_row(this->ptr_, TOF_ERROR_HANDLER{});
  }
 
  /** Get the column value
   * @return Column value
   */
  int32_t col() {
    return tof_point_2d_col(this->ptr_, TOF_ERROR_HANDLER{});
  }
 
};
 
/** This class contains all the calibration information
*/
class Calibration : public detail::Base<tof_calibration, tof_calibration_delete> {
 public:
  /** Construct from pointer */
  Calibration(tof_calibration_t ptr = nullptr) {
    this->ptr_ = ptr;
  }
 
  /** Read calibration from disk
   * @param file_location Location of calibration file
   */
  Calibration(StringView file_location) : Calibration(tof_calibration_new_from_disk(file_location, TOF_ERROR_HANDLER{})) {}
 
  /** Write calibration to disk
   * @param file_location Location to save the calibration file to
   */
  void write(StringView file_location) const {
    return tof_calibration_write(this->ptr_, file_location, TOF_ERROR_HANDLER{});
  }
 
  /** The the calibrated frequencies in the calibration file
   * @return The calibrated modulation frequencies
   */
  std::vector<float> get_calibrated_frequencies() const {
    size_t size = tof_calibration_get_calibrated_frequencies(this->ptr_, nullptr, 0, TOF_ERROR_HANDLER{});
    std::vector<float> vec(size);
    size = tof_calibration_get_calibrated_frequencies(this->ptr_, vec.data(), vec.size(), TOF_ERROR_HANDLER{});
    return vec;
  }
 
  /** Get focal length x
   * @return Focal length
   */
  double get_focal_length_x() const {
    return tof_calibration_get_focal_length_x(this->ptr_, TOF_ERROR_HANDLER{});
  }
 
  /** Get focal length y
   * @return Focal length
   */
  double get_focal_length_y() const {
    return tof_calibration_get_focal_length_y(this->ptr_, TOF_ERROR_HANDLER{});
  }
 
  /** Get principal point x
   * @return Principal point
   */
  double get_principal_point_x() const {
    return tof_calibration_get_principal_point_x(this->ptr_, TOF_ERROR_HANDLER{});
  }
 
  /** Get principal point y
   * @return Principal point
   */
  double get_principal_point_y() const {
    return tof_calibration_get_principal_point_y(this->ptr_, TOF_ERROR_HANDLER{});
  }
 
  /** Get the camera matrix of the depth sensor and lens
   * @return Camera matrix, 3x3 matrix
   */
  std::array<double, 9> get_depth_camera_matrix() const {
    std::array<double, 9> array;
    auto data = tof_calibration_get_depth_camera_matrix(this->ptr_, TOF_ERROR_HANDLER{});
    std::copy(data, data + array.size(), array.data());
    return array;
  }
 
  /** Get the distortion coefficients of the depth lens
   * @return Distortion coefficients
   */
  std::vector<double> get_depth_distortion_coefficients() const {
    size_t size = tof_calibration_get_depth_distortion_coefficients(this->ptr_, nullptr, 0, TOF_ERROR_HANDLER{});
    std::vector<double> vec(size);
    size = tof_calibration_get_depth_distortion_coefficients(this->ptr_, vec.data(), vec.size(), TOF_ERROR_HANDLER{});
    return vec;
  }
 
  /** Get the camera matrix of the rgb sensor and lens
   * @return Camera matrix, 3x3 matrix
   */
  std::array<double, 9> get_rgb_camera_matrix() const {
    std::array<double, 9> array;
    auto data = tof_calibration_get_rgb_camera_matrix(this->ptr_, TOF_ERROR_HANDLER{});
    std::copy(data, data + array.size(), array.data());
    return array;
  }
 
  /** Get the distortion coefficients of the rgb lens
   * @return Distortion coefficients, 5x1 matrix
   */
  std::array<double, 5> get_rgb_distortion_coefficients() const {
    std::array<double, 5> array;
    auto data = tof_calibration_get_rgb_distortion_coefficients(this->ptr_, TOF_ERROR_HANDLER{});
    std::copy(data, data + array.size(), array.data());
    return array;
  }
 
  /** Get the rotation vector between the depth and rgb sensor
   * @return Rotation vector, 3x1 matrix
   */
  std::array<double, 3> get_rotation_vector() const {
    std::array<double, 3> array;
    auto data = tof_calibration_get_rotation_vector(this->ptr_, TOF_ERROR_HANDLER{});
    std::copy(data, data + array.size(), array.data());
    return array;
  }
 
  /** Get the translation vector between the depth and rgb sensor
   * @return Translation vector, 3x1 matrix
   */
  std::array<double, 3> get_translation_vector() const {
    std::array<double, 3> array;
    auto data = tof_calibration_get_translation_vector(this->ptr_, TOF_ERROR_HANDLER{});
    std::copy(data, data + array.size(), array.data());
    return array;
  }
 
};
 
/** Transform a 3D point into a 2D pixel coordinate of the depth camera
 * @param x X value
 * @param y Y value
 * @param z Z value
 * @param calibration The camera calibration
 * @return 2D pixel coordinate of the depth camera
 */
inline Point2d transform_3d_to_depth_2d(float x, float y, float z, Calibration &calibration) {
  Point2d new_point_2d(static_cast<tof_point_2d_t>(nullptr));
  auto ptr = reinterpret_cast<tof_point_2d_t*>(&new_point_2d);
  *ptr = tof_transform_3d_to_depth_2d(x, y, z, *reinterpret_cast<tof_calibration_t*>(&calibration), TOF_ERROR_HANDLER{});
  return new_point_2d;
}
 
/** Transform a 3D point into a 2D pixel coordinate of the color camera
 * @param x X value
 * @param y Y value
 * @param z Z value
 * @param calibration The camera calibration
 * @return 2D pixel coordinate of the color camera
 */
inline Point2d transform_3d_to_color_2d(float x, float y, float z, Calibration &calibration) {
  Point2d new_point_2d(static_cast<tof_point_2d_t>(nullptr));
  auto ptr = reinterpret_cast<tof_point_2d_t*>(&new_point_2d);
  *ptr = tof_transform_3d_to_color_2d(x, y, z, *reinterpret_cast<tof_calibration_t*>(&calibration), TOF_ERROR_HANDLER{});
  return new_point_2d;
}
 
/** Transform a 2D pixel coordinate of the color camera to the 2D pixel
 * coordinate of the depth camera.
 * @param row Row coordinate
 * @param col Column cooridnate
 * @param depth_data Depth data containing xyz information, XYZ, XYZAmp, XYZBGR
 * and XYZBGRI are all valid
 * @param calibration The camera calibration
 * @return 2D pixel coordinate of the depth camera
 */
inline Point2d transform_color_2d_to_depth_2d(int32_t row, int32_t col, Data &depth_data, Calibration &calibration) {
  Point2d new_point_2d(static_cast<tof_point_2d_t>(nullptr));
  auto ptr = reinterpret_cast<tof_point_2d_t*>(&new_point_2d);
  *ptr = tof_transform_color_2d_to_depth_2d(row, col, *reinterpret_cast<tof_data_t*>(&depth_data), *reinterpret_cast<tof_calibration_t*>(&calibration), TOF_ERROR_HANDLER{});
  return new_point_2d;
}
 
} // tof
} // chronoptics
 
#endif