moved undistorter to cameras and added point processing function

aslam_cv_cameras/CMakeLists.txt

@@ -13,12 +13,15 @@ set(SOURCES
   src/camera-pinhole.cc
   src/camera-unified-projection.cc
   src/camera-yaml-serialization.cc
+  src/convert-maps-legacy.cc
   src/distortion.cc
   src/distortion-equidistant.cc
   src/distortion-fisheye.cc
   src/distortion-radtan.cc
   src/ncamera.cc
-  src/ncamera-yaml-serialization.cc
+  src/ncamera-yaml-serialization.cc   
+  src/undistorter.cc
+  src/undistorter-mapped.cc 
 )
 
 cs_add_library(${PROJECT_NAME} ${SOURCES})
@@ -37,6 +40,8 @@ target_link_libraries(test_distortions ${PROJECT_NAME})
 catkin_add_gtest(test_ncamera test/test-ncamera.cc)
 target_link_libraries(test_ncamera ${PROJECT_NAME})
 
+catkin_add_gtest(test_undistorters test/test-undistorters.cc)
+target_link_libraries(test_undistorters ${PROJECT_NAME}) 
 ##########
 # EXPORT #
 ##########

aslam_cv_cameras/include/aslam/cameras/camera-factory.h

@@ -26,7 +26,8 @@ template <typename CameraType, typename DistortionType>
 typename CameraType::Ptr createCamera(const Eigen::VectorXd& intrinsics,
                                       uint32_t image_width, uint32_t image_height,
                                       const Eigen::VectorXd& distortion_parameters) {
-  typename aslam::Distortion::UniquePtr distortion(new DistortionType(distortion_parameters));
+  typename aslam::Distortion::UniquePtr distortion(
+      new DistortionType(distortion_parameters));
   typename CameraType::Ptr camera(
       new CameraType(intrinsics, image_width, image_height, distortion));
   aslam::CameraId id;

aslam_cv_pipeline/include/aslam/pipeline/test/convert-maps-legacy.h → aslam_cv_cameras/include/aslam/cameras/convert-maps-legacy.h

@@ -1,5 +1,5 @@
-#ifndef ASLAM_TEST_CONVERT_MAPS_LEGACY_H
-#define ASLAM_TEST_CONVERT_MAPS_LEGACY_H
+#ifndef ASLAM_CAMERAS_CONVERT_MAPS_LEGACY_H
+#define ASLAM_CAMERAS_CONVERT_MAPS_LEGACY_H
 
 #include <opencv2/imgproc/imgproc.hpp>
 
@@ -13,4 +13,4 @@ void convertMapsLegacy(cv::InputArray _map1, cv::InputArray _map2,
                        int dstm1type, bool nninterpolate = false);
 } // namespace aslam
 
-#endif // ASLAM_TEST_CONVERT_MAPS_LEGACY_H
+#endif // ASLAM_CAMERAS_CONVERT_MAPS_LEGACY_H

aslam_cv_cameras/include/aslam/cameras/distortion.h

@@ -38,8 +38,7 @@ class Distortion {
   /// @param[in] dist_coeffs     Vector containing the distortion parameters.
   /// @param[in] distortion_type DistortionType enum value with information which distortion
   ///                            model is used by the derived class.
-  Distortion(const Eigen::VectorXd& dist_coeffs,
-             Type distortion_type);
+  Distortion(const Eigen::VectorXd& dist_coeffs, Type distortion_type);
 
  public:
   virtual ~Distortion() { };
@@ -121,7 +120,7 @@ class Distortion {
   void undistort(Eigen::Vector2d* point) const;
 
   /// \brief Apply undistortion to recover a point in the normalized image plane.
-  /// @param[in]    point     External distortion coefficients to use.
+  /// @param[in]    point     The distorted point.
   /// @param[out]   out_point The undistorted point in normalized image plane.
   void undistort(const Eigen::Vector2d& point, Eigen::Vector2d* out_point) const;
 

aslam_cv_cameras/include/aslam/cameras/ncamera.h

@@ -107,16 +107,32 @@ class NCamera {
   /// Get the geometry object for camera i.
   const Camera& getCamera(size_t camera_index) const;
 
+  /// Get the geometry object for camera i.
+  /// The method will assert that the camera is not in the rig!
+  const Camera& getCamera(const CameraId& camera_id) const;
+
   /// Get the geometry object for camera i.
   Camera& getCameraMutable(size_t camera_index);
 
+  /// Get the geometry object for camera i.
+  /// The method will assert that the camera is not in the rig!
+  Camera& getCameraMutable(const CameraId& camera_id);
+
   /// Get the geometry object for camera i.
   std::shared_ptr<Camera> getCameraShared(size_t camera_index);
 
+  /// Get the geometry object for camera i.
+  /// The method will assert that the camera is not in the rig!
+  std::shared_ptr<Camera> getCameraShared(const CameraId& camera_id);
+
   /// Get the geometry object for camera i.
   std::shared_ptr<const Camera> getCameraShared(size_t camera_index) const;
 
   /// Get the geometry object for camera i.
+  /// The method will assert that the camera is not in the rig!
+  std::shared_ptr<const Camera> getCameraShared(const CameraId& camera_id) const;
+
+  /// Set the geometry object for camera i.
   void setCamera(size_t camera_index, std::shared_ptr<Camera> camera);
 
   /// How many cameras does this system have?

aslam_cv_pipeline/include/aslam/pipeline/undistorter-mapped-inl.h → aslam_cv_cameras/include/aslam/cameras/undistorter-mapped-inl.h

@@ -1,14 +1,15 @@
-#ifndef ASLAM_PIPELINE_MAPPED_UNDISTORTER_INL_H_
-#define ASLAM_PIPELINE_MAPPED_UNDISTORTER_INL_H_
+#ifndef ASLAM_CAMERAS_MAPPED_UNDISTORTER_INL_H_
+#define ASLAM_CAMERAS_MAPPED_UNDISTORTER_INL_H_
 
 #include <aslam/cameras/camera-factory.h>
 #include <aslam/cameras/camera.h>
+#include <aslam/cameras/convert-maps-legacy.h>
 #include <aslam/common/undistort-helpers.h>
 
 namespace aslam {
 
 template <>
-inline std::unique_ptr<MappedUndistorter> createMappedUndistorter(
+inline std::shared_ptr<MappedUndistorter> createMappedUndistorter(
     const aslam::Camera& camera, float alpha, float scale,
     aslam::InterpolationMethod interpolation_type) {
   switch (camera.getType()) {
@@ -34,13 +35,12 @@ inline std::unique_ptr<MappedUndistorter> createMappedUndistorter(
 }
 
 template <typename CameraType>
-inline std::unique_ptr<MappedUndistorter> createMappedUndistorter(
+inline std::shared_ptr<MappedUndistorter> createMappedUndistorter(
     const CameraType& camera, float alpha, float scale,
     aslam::InterpolationMethod interpolation_type) {
   CHECK_GE(alpha, 0.0);
   CHECK_LE(alpha, 1.0);
   CHECK_GT(scale, 0.0);
-
   // Create a copy of the input camera.
   aslam::Camera::Ptr input_camera(camera.clone());
   CHECK(input_camera);
@@ -70,8 +70,7 @@ inline std::unique_ptr<MappedUndistorter> createMappedUndistorter(
               input_camera);
       CHECK(unified_proj_cam_ptr != nullptr)
           << "Cast to unified projection camera failed.";
-      intrinsics << unified_proj_cam_ptr->xi(), output_camera_matrix(0, 0),
-          output_camera_matrix(1, 1), output_camera_matrix(0, 2),
+      output_camera_matrix(1, 1), output_camera_matrix(0, 2),
           output_camera_matrix(1, 2);
       output_camera.reset(
           new UnifiedProjectionCamera(intrinsics, output_width, output_height));
@@ -88,9 +87,16 @@ inline std::unique_ptr<MappedUndistorter> createMappedUndistorter(
   common::buildUndistortMap(
       *input_camera, *output_camera, CV_16SC2, map_u, map_v);
 
-  return std::unique_ptr<MappedUndistorter>(new MappedUndistorter(
-      input_camera, output_camera, map_u, map_v, interpolation_type));
+  // Convert map to non-fixed point representation for easy lookup of values.
+  cv::Mat map_u_float = map_u.clone();
+  cv::Mat map_v_float = map_v.clone();
+  aslam::convertMapsLegacy(map_u, map_v, map_u_float, map_v_float, CV_32FC1);
+
+  return std::shared_ptr<MappedUndistorter>(
+      new MappedUndistorter(
+          input_camera, output_camera, map_u, map_v, map_u_float, map_v_float,
+          interpolation_type));
 }
 
 }  // namespace aslam
-#endif  // ASLAM_PIPELINE_MAPPED_UNDISTORTER_INL_H_
+#endif  // ASLAM_CAMERAS_MAPPED_UNDISTORTER_INL_H_

aslam_cv_pipeline/include/aslam/pipeline/undistorter-mapped.h → aslam_cv_cameras/include/aslam/cameras/undistorter-mapped.h

@@ -1,13 +1,13 @@
-#ifndef ASLAM_PIPELINE_MAPPED_UNDISTORTER_H_
-#define ASLAM_PIPELINE_MAPPED_UNDISTORTER_H_
+#ifndef ASLAM_CAMERAS_MAPPED_UNDISTORTER_H_
+#define ASLAM_CAMERAS_MAPPED_UNDISTORTER_H_
 
 #include <opencv2/core/core.hpp>
 
 #include <aslam/common/types.h>
 #include <aslam/cameras/camera.h>
 #include <aslam/cameras/camera-pinhole.h>
 #include <aslam/cameras/camera-unified-projection.h>
-#include <aslam/pipeline/undistorter.h>
+#include <aslam/cameras/undistorter.h>
 
 namespace aslam {
 
@@ -24,7 +24,7 @@ namespace aslam {
 /// @param[in] interpolation_type Check \ref InterpolationMethod to see the available types.
 /// @return Pointer to the created mapped undistorter.
 template <typename CameraType>
-std::unique_ptr<MappedUndistorter> createMappedUndistorter(
+std::shared_ptr<MappedUndistorter> createMappedUndistorter(
     const CameraType& camera, float alpha, float scale,
     aslam::InterpolationMethod interpolation_type);
 
@@ -39,7 +39,7 @@ std::unique_ptr<MappedUndistorter> createMappedUndistorter(
 /// @param[in] scale Output image size scaling parameter wrt. to input image size.
 /// @param[in] interpolation_type Check \ref MappedUndistorter to see the available types.
 /// @return Pointer to the created mapped undistorter.
-std::unique_ptr<MappedUndistorter> createMappedUndistorterToPinhole(
+std::shared_ptr<MappedUndistorter> createMappedUndistorterToPinhole(
     const aslam::UnifiedProjectionCamera& unified_proj_camera,
     float alpha, float scale, aslam::InterpolationMethod interpolation_type);
 
@@ -69,13 +69,21 @@ class MappedUndistorter : public Undistorter {
   /// \param[in] interpolation Interpolation method used for undistortion.
   ///                          (\ref InterpolationMethod)
   MappedUndistorter(aslam::Camera::Ptr input_camera, aslam::Camera::Ptr output_camera,
-                    const cv::Mat& map_u, const cv::Mat& map_v, InterpolationMethod interpolation);
+                   const cv::Mat& map_u, const cv::Mat& map_v, 
+                   const cv::Mat& map_u_float, const cv::Mat& map_v_float, 
+                   InterpolationMethod interpolation);
 
   virtual ~MappedUndistorter() = default;
 
   /// \brief Produce an undistorted image from an input image.
   virtual void processImage(const cv::Mat& input_image, cv::Mat* output_image) const;
 
+  /// \brief Undisort a point using the map.
+  void processPoint(const Eigen::Vector2d& input_point, Eigen::Vector2d* output_point) const;
+
+  /// \brief Undistort a point using the map.
+  void processPoint(Eigen::Vector2d* point) const;
+
   /// Get the undistorter map for the u-coordinate.
   const cv::Mat& getUndistortMapU() const { return map_u_; };
 
@@ -87,12 +95,16 @@ class MappedUndistorter : public Undistorter {
   const cv::Mat map_u_;
   /// \brief LUT for v coordinates.
   const cv::Mat map_v_;
+  /// \brief Non-fixed point LUT for u coordinates.
+  const cv::Mat map_u_float_;
+  /// \brief Non-fixed point LUT for v coordinates.
+  const cv::Mat map_v_float_;
   /// \brief Interpolation strategy
   InterpolationMethod interpolation_method_;
 };
 
 }  // namespace aslam
 
-#include "aslam/pipeline/undistorter-mapped-inl.h"
+#include "aslam/cameras/undistorter-mapped-inl.h"
 
-#endif // ASLAM_PIPELINE_MAPPED_UNDISTORTER_H_
+#endif // ASLAM_CAMERAS_MAPPED_UNDISTORTER_H_

aslam_cv_cameras/src/camera-factory.cc

@@ -13,15 +13,15 @@
 
 namespace aslam {
 
-Camera::Ptr createCamera(aslam::CameraId id, const Eigen::VectorXd& intrinsics,
-                         uint32_t image_width, uint32_t image_height,
-                         const Eigen::VectorXd& distortion_parameters,
-                         Camera::Type camera_type,
-                         Distortion::Type distortion_type) {
+Camera::Ptr createCamera(
+    aslam::CameraId id, const Eigen::VectorXd& intrinsics,
+    uint32_t image_width, uint32_t image_height,
+    const Eigen::VectorXd& distortion_parameters, Camera::Type camera_type,
+    Distortion::Type distortion_type) {
   CHECK(id.isValid()) << "Invalid camera id: " << id.hexString();
 
   Distortion::UniquePtr distortion;
-  switch(distortion_type) {
+  switch (distortion_type) {
     case Distortion::Type::kNoDistortion:
       distortion.reset(new NullDistortion());
       break;
@@ -36,26 +36,27 @@ Camera::Ptr createCamera(aslam::CameraId id, const Eigen::VectorXd& intrinsics,
       break;
     default:
       LOG(FATAL) << "Unknown distortion model: "
-          << static_cast<std::underlying_type<Distortion::Type>::type>(
-              distortion_type);
+                 << static_cast<std::underlying_type<Distortion::Type>::type>(
+                        distortion_type);
   }
   CHECK(distortion->distortionParametersValid(distortion_parameters))
-      << "Invalid distortion parameters: "
-      << distortion_parameters.transpose();
+      << "Invalid distortion parameters: " << distortion_parameters.transpose();
 
   Camera::Ptr camera;
-  switch(camera_type) {
+  switch (camera_type) {
     case Camera::Type::kPinhole:
-      camera.reset(new PinholeCamera(intrinsics, image_width, image_height,
-                                     distortion));
+      camera.reset(
+          new PinholeCamera(intrinsics, image_width, image_height, distortion));
       break;
     case Camera::Type::kUnifiedProjection:
-      camera.reset(new UnifiedProjectionCamera(intrinsics, image_width,
-                                               image_height, distortion));
+      camera.reset(
+          new UnifiedProjectionCamera(
+              intrinsics, image_width, image_height, distortion));
       break;
     default:
       LOG(FATAL) << "Unknown camera model: "
-        << static_cast<std::underlying_type<Camera::Type>::type>(camera_type);
+                 << static_cast<std::underlying_type<Camera::Type>::type>(
+                        camera_type);
   }
 
   camera->setId(id);

aslam_cv_pipeline/src/test/convert-maps-legacy.cc → aslam_cv_cameras/src/convert-maps-legacy.cc

@@ -1,4 +1,4 @@
-#include <aslam/pipeline/test/convert-maps-legacy.h>
+#include <aslam/cameras/convert-maps-legacy.h>
 
 void aslam::convertMapsLegacy(cv::InputArray _map1, cv::InputArray _map2,
                        cv::OutputArray _dstmap1, cv::OutputArray _dstmap2,

aslam_cv_cameras/src/distortion.cc

@@ -5,22 +5,24 @@
 #include <gflags/gflags.h>
 #include <glog/logging.h>
 
-DEFINE_double(acv_inv_distortion_tolerance, 1e-8, "Convergence tolerance for iterated"
-              "inverse distortion functions.");
+DEFINE_double(
+    acv_inv_distortion_tolerance, 1e-8,
+    "Convergence tolerance for iterated"
+    "inverse distortion functions.");
 
 namespace aslam {
 std::ostream& operator<<(std::ostream& out, const Distortion& distortion) {
   distortion.printParameters(out, std::string(""));
   return out;
 }
 
-Distortion::Distortion(const Eigen::VectorXd& dist_coeffs,
-                       Type distortion_type)
+Distortion::Distortion(
+    const Eigen::VectorXd& dist_coeffs, Type distortion_type)
     : distortion_coefficients_(dist_coeffs),
       distortion_type_(distortion_type) {}
 
 bool Distortion::operator==(const Distortion& rhs) const {
-  //check for same distortion type
+  // check for same distortion type
   if (typeid(*this) != typeid(rhs))
     return false;
 
@@ -35,16 +37,15 @@ void Distortion::distort(Eigen::Vector2d* point) const {
   distortUsingExternalCoefficients(nullptr, point, nullptr);
 }
 
-void Distortion::distort(const Eigen::Vector2d& point,
-                         Eigen::Vector2d* out_point) const {
+void Distortion::distort(
+    const Eigen::Vector2d& point, Eigen::Vector2d* out_point) const {
   CHECK_NOTNULL(out_point);
   *out_point = point;
   distortUsingExternalCoefficients(nullptr, out_point, nullptr);
 }
 
 void Distortion::distort(
-    Eigen::Vector2d* point,
-    Eigen::Matrix2d* out_jacobian) const {
+    Eigen::Vector2d* point, Eigen::Matrix2d* out_jacobian) const {
   CHECK_NOTNULL(point);
   CHECK_NOTNULL(out_jacobian);
   distortUsingExternalCoefficients(nullptr, point, out_jacobian);
@@ -55,15 +56,16 @@ void Distortion::undistort(Eigen::Vector2d* point) const {
   undistortUsingExternalCoefficients(distortion_coefficients_, point);
 }
 
-void Distortion::undistort(const Eigen::Vector2d& point,
-                           Eigen::Vector2d* out_point) const {
+void Distortion::undistort(
+    const Eigen::Vector2d& point, Eigen::Vector2d* out_point) const {
   CHECK_NOTNULL(out_point);
   *out_point = point;
   undistortUsingExternalCoefficients(distortion_coefficients_, out_point);
 }
 
 void Distortion::setParameters(const Eigen::VectorXd& dist_coeffs) {
-  CHECK(distortionParametersValid(dist_coeffs)) << "Distortion parameters invalid!";
+  CHECK(distortionParametersValid(dist_coeffs))
+      << "Distortion parameters invalid!";
   distortion_coefficients_ = dist_coeffs;
 }