Program Listing for File distortion-equidistant.h

Return to documentation for file (aslam_cv2/aslam_cv_cameras/include/aslam/cameras/distortion-equidistant.h)

#ifndef ASLAM_EQUIDISTANT_DISTORTION_H_
#define ASLAM_EQUIDISTANT_DISTORTION_H_

#include <Eigen/Core>
#include <glog/logging.h>

#include <aslam/common/crtp-clone.h>
#include <aslam/cameras/distortion.h>
#include <aslam/common/macros.h>

namespace aslam {

class EquidistantDistortion : public aslam::Cloneable<Distortion, EquidistantDistortion> {
 public:
  enum { kNumOfParams = 4 };

  enum { CLASS_SERIALIZATION_VERSION = 1 };
  ASLAM_POINTER_TYPEDEFS(EquidistantDistortion);


  explicit EquidistantDistortion(const Eigen::VectorXd& distortionParams);

  friend std::ostream& operator<<(std::ostream& out, const EquidistantDistortion& distortion);

 public:
  EquidistantDistortion(const EquidistantDistortion&) = default;
  void operator=(const EquidistantDistortion&) = delete;


 public:
  virtual void distortUsingExternalCoefficients(const Eigen::VectorXd* dist_coeffs,
                                                Eigen::Vector2d* point,
                                                Eigen::Matrix2d* out_jacobian) const;

  template<typename ScalarType>
  void distortUsingExternalCoefficients(
      const Eigen::Map<Eigen::Matrix<ScalarType, Eigen::Dynamic, 1>>* dist_coeffs,
      const Eigen::Matrix<ScalarType, 2, 1>& point,
      Eigen::Matrix<ScalarType, 2, 1>* out_point) const;

  virtual void distortParameterJacobian(const Eigen::VectorXd* dist_coeffs,
                                        const Eigen::Vector2d& point,
                                        Eigen::Matrix<double, 2, Eigen::Dynamic>* out_jacobian) const;



  virtual void undistortUsingExternalCoefficients(const Eigen::VectorXd& dist_coeffs,
                                                  Eigen::Vector2d* point) const;



  static EquidistantDistortion::UniquePtr createTestDistortion() {
    Eigen::VectorXd params(4); params << 0.2, 0.01, 0.3, 0.05;
    return EquidistantDistortion::UniquePtr(new EquidistantDistortion(params));
  }

  static EquidistantDistortion::UniquePtr createZeroTestDistortion() {
    Eigen::VectorXd params(4); params << 0.0, 0.0, 0.0, 0.0;
    return EquidistantDistortion::UniquePtr(new EquidistantDistortion(params));
  }



  static bool areParametersValid(const Eigen::VectorXd& parameters);

  virtual bool distortionParametersValid(const Eigen::VectorXd& dist_coeffs) const;

  inline static constexpr size_t parameterCount() {
      return kNumOfParams;
  }

  virtual int getParameterSize() const {
    return kNumOfParams;
  }

  virtual void printParameters(std::ostream& out, const std::string& text) const;


};

} // namespace aslam

#endif /* ASLAM_EQUIDISTANT_DISTORTION_H_ */