Add FOV distortion model.

.gitignore

@@ -0,0 +1,46 @@
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+# temporary editor files
+*~
+*.orig
+
+# Compiled python.
+tools/lint/autolintc
+*.pyc
+
+# Eclipse.
+.project
+.cproject
+.settings
+CATKIN_IGNORE
+
+# QtCreator
+CMakeLists.txt.user
+
+*.mat
+*.csv
+*.asv
+*.jpg
+
+# MacOS Desktop Services Store
+.DS_Store

aslam_cv/aslam_cameras/CMakeLists.txt

@@ -18,6 +18,7 @@ cs_add_library(${PROJECT_NAME}
   src/NoDistortion.cpp
   src/RadialTangentialDistortion.cpp
   src/EquidistantDistortion.cpp
+  src/FovDistortion.cpp
   src/ImageMask.cpp
   src/GridCalibrationTargetObservation.cpp
   src/GridCalibrationTargetBase.cpp
@@ -45,6 +46,7 @@ catkin_add_gtest(${PROJECT_NAME}_tests
   test/OmniCameraGeometry.cpp
   test/RadialTangentialDistortion.cpp
   test/EquidistantDistortion.cpp
+  test/FovDistortion.cpp
   test/Triangulation.cpp
   test/testFrame.cpp
   test/GridCalibration.cpp

aslam_cv/aslam_cameras/include/aslam/cameras.hpp

@@ -13,6 +13,7 @@
 #include <aslam/cameras/NoDistortion.hpp>
 #include <aslam/cameras/RadialTangentialDistortion.hpp>
 #include <aslam/cameras/EquidistantDistortion.hpp>
+#include <aslam/cameras/FovDistortion.hpp>
 
 // Masks
 #include <aslam/cameras/NoMask.hpp>
@@ -30,55 +31,73 @@ typedef CameraGeometry<PinholeProjection<RadialTangentialDistortion>,
     GlobalShutter, NoMask> DistortedPinholeCameraGeometry;
 typedef CameraGeometry<PinholeProjection<EquidistantDistortion>, GlobalShutter,
     NoMask> EquidistantDistortedPinholeCameraGeometry;
+typedef CameraGeometry<PinholeProjection<FovDistortion>, GlobalShutter,
+    NoMask> FovDistortedPinholeCameraGeometry;
 
 typedef CameraGeometry<OmniProjection<NoDistortion>, GlobalShutter, NoMask> OmniCameraGeometry;
 typedef CameraGeometry<OmniProjection<RadialTangentialDistortion>,
     GlobalShutter, NoMask> DistortedOmniCameraGeometry;
 typedef CameraGeometry<OmniProjection<EquidistantDistortion>, GlobalShutter,
     NoMask> EquidistantDistortedOmniCameraGeometry;
+typedef CameraGeometry<OmniProjection<FovDistortion>, GlobalShutter,
+    NoMask> FovDistortedOmniCameraGeometry;
 
 typedef CameraGeometry<PinholeProjection<NoDistortion>, RollingShutter, NoMask> PinholeRsCameraGeometry;
 typedef CameraGeometry<PinholeProjection<RadialTangentialDistortion>,
     RollingShutter, NoMask> DistortedPinholeRsCameraGeometry;
 typedef CameraGeometry<PinholeProjection<EquidistantDistortion>, RollingShutter,
     NoMask> EquidistantDistortedPinholeRsCameraGeometry;
+typedef CameraGeometry<PinholeProjection<FovDistortion>, RollingShutter,
+    NoMask> FovDistortedPinholeRsCameraGeometry;
 
 typedef CameraGeometry<OmniProjection<NoDistortion>, RollingShutter, NoMask> OmniRsCameraGeometry;
 typedef CameraGeometry<OmniProjection<RadialTangentialDistortion>,
     RollingShutter, NoMask> DistortedOmniRsCameraGeometry;
 typedef CameraGeometry<OmniProjection<EquidistantDistortion>, RollingShutter,
     NoMask> EquidistantDistortedOmniRsCameraGeometry;
+typedef CameraGeometry<OmniProjection<FovDistortion>, RollingShutter,
+    NoMask> FovDistortedOmniRsCameraGeometry;
 
 typedef CameraGeometry<PinholeProjection<NoDistortion>, GlobalShutter, ImageMask> MaskedPinholeCameraGeometry;
 typedef CameraGeometry<PinholeProjection<RadialTangentialDistortion>,
     GlobalShutter, ImageMask> MaskedDistortedPinholeCameraGeometry;
 typedef CameraGeometry<PinholeProjection<EquidistantDistortion>, GlobalShutter,
     ImageMask> MaskedEquidistantDistortedPinholeCameraGeometry;
+typedef CameraGeometry<PinholeProjection<FovDistortion>, GlobalShutter,
+    ImageMask> MaskedFovDistortedPinholeCameraGeometry;
 
 typedef CameraGeometry<OmniProjection<NoDistortion>, GlobalShutter, ImageMask> MaskedOmniCameraGeometry;
 typedef CameraGeometry<OmniProjection<RadialTangentialDistortion>,
     GlobalShutter, ImageMask> MaskedDistortedOmniCameraGeometry;
 typedef CameraGeometry<OmniProjection<EquidistantDistortion>, GlobalShutter,
     ImageMask> MaskedEquidistantDistortedOmniCameraGeometry;
+typedef CameraGeometry<OmniProjection<FovDistortion>, GlobalShutter,
+    ImageMask> MaskedFovDistortedOmniCameraGeometry;
 
 typedef CameraGeometry<PinholeProjection<NoDistortion>, RollingShutter,
     ImageMask> MaskedPinholeRsCameraGeometry;
 typedef CameraGeometry<PinholeProjection<RadialTangentialDistortion>,
     RollingShutter, ImageMask> MaskedDistortedPinholeRsCameraGeometry;
 typedef CameraGeometry<PinholeProjection<EquidistantDistortion>, RollingShutter,
     ImageMask> MaskedEquidistantDistortedPinholeRsCameraGeometry;
+typedef CameraGeometry<PinholeProjection<FovDistortion>, RollingShutter,
+    ImageMask> MaskedFovDistortedPinholeRsCameraGeometry;
 
 typedef CameraGeometry<OmniProjection<NoDistortion>, RollingShutter, ImageMask> MaskedOmniRsCameraGeometry;
 typedef CameraGeometry<OmniProjection<RadialTangentialDistortion>,
     RollingShutter, ImageMask> MaskedDistortedOmniRsCameraGeometry;
 typedef CameraGeometry<OmniProjection<EquidistantDistortion>, RollingShutter,
     ImageMask> MaskedEquidistantDistortedOmniRsCameraGeometry;
+typedef CameraGeometry<OmniProjection<FovDistortion>, RollingShutter,
+    ImageMask> MaskedFovDistortedOmniRsCameraGeometry;
 
 typedef CameraGeometry<DepthProjection<NoDistortion>, GlobalShutter, NoMask> DepthCameraGeometry;
 typedef CameraGeometry<DepthProjection<RadialTangentialDistortion>,
     GlobalShutter, NoMask> DistortedDepthCameraGeometry;
 typedef CameraGeometry<DepthProjection<EquidistantDistortion>, GlobalShutter,
     NoMask> EquidistantDistortedDepthCameraGeometry;
+typedef CameraGeometry<DepthProjection<FovDistortion>, GlobalShutter,
+    NoMask> FovDistortedDepthCameraGeometry;
 
 }  // namespace cameras
 }  // namespace aslam

aslam_cv/aslam_cameras/include/aslam/cameras/FovDistortion.hpp

@@ -0,0 +1,158 @@
+#ifndef ASLAM_CAMERAS_FOV_DISTORTION_HPP
+#define ASLAM_CAMERAS_FOV_DISTORTION_HPP
+
+#include <Eigen/Dense>
+#include <boost/serialization/nvp.hpp>
+#include "StaticAssert.hpp"
+#include <sm/PropertyTree.hpp>
+#include <boost/serialization/split_member.hpp>
+#include <boost/serialization/version.hpp>
+#include <sm/boost/serialization.hpp>
+
+namespace aslam {
+namespace cameras {
+
+/**
+ * \class FovDistortion
+ * \brief An implementation of the fov distortion model for pinhole cameras.
+ */
+class FovDistortion {
+ public:
+
+  enum {
+    IntrinsicsDimension = 1
+  };
+  enum {
+    DesignVariableDimension = IntrinsicsDimension
+  };
+
+  /// \brief The default constructor sets all values to zero. 
+  FovDistortion();
+
+  /// \brief A constructor that initializes all values.
+  FovDistortion(double w);
+
+  /// \brief initialize from a property tree
+  FovDistortion(const sm::PropertyTree & config);
+
+  virtual ~FovDistortion();
+
+  /** 
+   * \brief Apply distortion to a point in the normalized image plane
+   * 
+   * @param y The point in the normalized image plane. After the function, this point is distorted.
+   */
+  template<typename DERIVED_Y>
+  void distort(const Eigen::MatrixBase<DERIVED_Y> & y) const;
+
+  /** 
+   * 
+   * \brief Apply distortion to a point in the normalized image plane
+   * 
+   * @param y The point in the normalized image plane. After the function, this point is distorted.
+   * @param outJy The Jacobian of the distortion function with respect to small changes in the input point.
+   */
+  template<typename DERIVED_Y, typename DERIVED_JY>
+  void distort(const Eigen::MatrixBase<DERIVED_Y> & y,
+               const Eigen::MatrixBase<DERIVED_JY> & outJy) const;
+
+  /** 
+   * \brief Apply undistortion to recover a point in the normalized image plane.
+   * 
+   * @param y The distorted point. After the function, this point is in the normalized image plane.
+   */
+  template<typename DERIVED>
+  void undistort(const Eigen::MatrixBase<DERIVED> & y) const;
+
+  /** 
+   * \brief Apply undistortion to recover a point in the normalized image plane.
+   * 
+   * @param y The distorted point. After the function, this point is in the normalized image plane.
+   * @param outJy The Jacobian of the undistortion function with respect to small changes in the input point.
+   */
+  template<typename DERIVED, typename DERIVED_JY>
+  void undistort(const Eigen::MatrixBase<DERIVED> & y,
+                 const Eigen::MatrixBase<DERIVED_JY> & outJy) const;
+
+  /** 
+   * \brief Apply distortion to the point and provide the Jacobian of the distortion with respect to small changes in the distortion parameters
+   * 
+   * @param imageY the point in the normalized image plane.
+   * @param outJd  the Jacobian of the distortion with respect to small changes in the distortion parameters.
+   */
+  template<typename DERIVED_Y, typename DERIVED_JD>
+  void distortParameterJacobian(
+      const Eigen::MatrixBase<DERIVED_Y> & imageY,
+      const Eigen::MatrixBase<DERIVED_JD> & outJd) const;
+
+  /** 
+   * \brief A function for compatibility with the aslam backend. This implements an update of the distortion parameter.
+   * 
+   * @param v A double array representing the update vector.
+   */
+  void update(const double * v);
+
+  /** 
+   * \brief A function for compatibility with the aslam backend. 
+   * 
+   * @param v The number of parameters expected by the update equation. This should also define the number of columns in the matrix returned by distortParameterJacobian.
+   */
+  int minimalDimensions() const;
+
+  /** 
+   * \brief A function for compatibility with the aslam backend. 
+   * 
+   * @param P This matrix is resized and filled with parameters representing the full state of the distortion. 
+   */
+  void getParameters(Eigen::MatrixXd & P) const;
+
+  /** 
+   * \brief A function for compatibility with the aslam backend. 
+   * 
+   * @param P The full state of the distortion class is set from the matrix of parameters.
+   */
+  void setParameters(const Eigen::MatrixXd & P);
+
+  Eigen::Vector2i parameterSize() const;
+
+  double w() {
+    return _w;
+  }
+
+  /// Static function that checks whether the given intrinsic parameters are valid for this model.
+  bool areParametersValid(double w) {
+    return std::abs(w) < 1e-16 || (w >= kMinValidW && w <= kMaxValidW);
+  }
+
+  /// \brief Compatibility with boost::serialization.
+  enum {
+    CLASS_SERIALIZATION_VERSION = 0
+  };BOOST_SERIALIZATION_SPLIT_MEMBER();
+
+  template<class Archive>
+  void load(Archive & ar, const unsigned int version);
+
+  template<class Archive>
+  void save(Archive & ar, const unsigned int version) const;
+
+  bool isBinaryEqual(const FovDistortion & rhs) const;
+
+  static FovDistortion getTestDistortion();
+
+  void clear();
+
+  /// \brief the disortion parameter
+  double _w;
+  static constexpr double kMaxValidAngle = (89.0 * M_PI / 180.0);
+  static constexpr double kMinValidW = 0.5;
+  static constexpr double kMaxValidW = 1.5;
+};
+
+}  // namespace cameras
+}  // namespace aslam
+
+#include "implementation/FovDistortion.hpp"
+
+SM_BOOST_CLASS_VERSION (aslam::cameras::FovDistortion);
+
+#endif /* ASLAM_CAMERAS_FOV_DISTORTION_HPP */