diff --git a/aslam_cv_cameras/CMakeLists.txt b/aslam_cv_cameras/CMakeLists.txt index 0bec69c7..1da1b169 100644 --- a/aslam_cv_cameras/CMakeLists.txt +++ b/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 # ########## diff --git a/aslam_cv_cameras/include/aslam/cameras/camera-factory.h b/aslam_cv_cameras/include/aslam/cameras/camera-factory.h index 5bcab2f1..1f527388 100644 --- a/aslam_cv_cameras/include/aslam/cameras/camera-factory.h +++ b/aslam_cv_cameras/include/aslam/cameras/camera-factory.h @@ -26,7 +26,8 @@ template 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; diff --git a/aslam_cv_pipeline/include/aslam/pipeline/test/convert-maps-legacy.h b/aslam_cv_cameras/include/aslam/cameras/convert-maps-legacy.h similarity index 79% rename from aslam_cv_pipeline/include/aslam/pipeline/test/convert-maps-legacy.h rename to aslam_cv_cameras/include/aslam/cameras/convert-maps-legacy.h index 1e4faf15..f70871c8 100644 --- a/aslam_cv_pipeline/include/aslam/pipeline/test/convert-maps-legacy.h +++ b/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 @@ -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 diff --git a/aslam_cv_cameras/include/aslam/cameras/distortion.h b/aslam_cv_cameras/include/aslam/cameras/distortion.h index 68d6200c..31527fba 100644 --- a/aslam_cv_cameras/include/aslam/cameras/distortion.h +++ b/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; diff --git a/aslam_cv_cameras/include/aslam/cameras/ncamera.h b/aslam_cv_cameras/include/aslam/cameras/ncamera.h index 14aaf327..4dead01c 100644 --- a/aslam_cv_cameras/include/aslam/cameras/ncamera.h +++ b/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 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 getCameraShared(const CameraId& camera_id); + /// Get the geometry object for camera i. std::shared_ptr 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 getCameraShared(const CameraId& camera_id) const; + + /// Set the geometry object for camera i. void setCamera(size_t camera_index, std::shared_ptr camera); /// How many cameras does this system have? diff --git a/aslam_cv_pipeline/include/aslam/pipeline/undistorter-mapped-inl.h b/aslam_cv_cameras/include/aslam/cameras/undistorter-mapped-inl.h similarity index 79% rename from aslam_cv_pipeline/include/aslam/pipeline/undistorter-mapped-inl.h rename to aslam_cv_cameras/include/aslam/cameras/undistorter-mapped-inl.h index 8fb60fa3..87f7a096 100644 --- a/aslam_cv_pipeline/include/aslam/pipeline/undistorter-mapped-inl.h +++ b/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 #include +#include #include namespace aslam { template <> -inline std::unique_ptr createMappedUndistorter( +inline std::shared_ptr createMappedUndistorter( const aslam::Camera& camera, float alpha, float scale, aslam::InterpolationMethod interpolation_type) { switch (camera.getType()) { @@ -34,13 +35,12 @@ inline std::unique_ptr createMappedUndistorter( } template -inline std::unique_ptr createMappedUndistorter( +inline std::shared_ptr 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 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 createMappedUndistorter( common::buildUndistortMap( *input_camera, *output_camera, CV_16SC2, map_u, map_v); - return std::unique_ptr(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( + 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_ diff --git a/aslam_cv_pipeline/include/aslam/pipeline/undistorter-mapped.h b/aslam_cv_cameras/include/aslam/cameras/undistorter-mapped.h similarity index 81% rename from aslam_cv_pipeline/include/aslam/pipeline/undistorter-mapped.h rename to aslam_cv_cameras/include/aslam/cameras/undistorter-mapped.h index 384902d7..405250df 100644 --- a/aslam_cv_pipeline/include/aslam/pipeline/undistorter-mapped.h +++ b/aslam_cv_cameras/include/aslam/cameras/undistorter-mapped.h @@ -1,5 +1,5 @@ -#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 @@ -7,7 +7,7 @@ #include #include #include -#include +#include 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 -std::unique_ptr createMappedUndistorter( +std::shared_ptr createMappedUndistorter( const CameraType& camera, float alpha, float scale, aslam::InterpolationMethod interpolation_type); @@ -39,7 +39,7 @@ std::unique_ptr 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 createMappedUndistorterToPinhole( +std::shared_ptr 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_ diff --git a/aslam_cv_pipeline/include/aslam/pipeline/undistorter.h b/aslam_cv_cameras/include/aslam/cameras/undistorter.h similarity index 100% rename from aslam_cv_pipeline/include/aslam/pipeline/undistorter.h rename to aslam_cv_cameras/include/aslam/cameras/undistorter.h diff --git a/aslam_cv_cameras/src/camera-factory.cc b/aslam_cv_cameras/src/camera-factory.cc index 33e4df48..f8b1f1d7 100644 --- a/aslam_cv_cameras/src/camera-factory.cc +++ b/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::type>( - distortion_type); + << static_cast::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::type>(camera_type); + << static_cast::type>( + camera_type); } camera->setId(id); diff --git a/aslam_cv_pipeline/src/test/convert-maps-legacy.cc b/aslam_cv_cameras/src/convert-maps-legacy.cc similarity index 98% rename from aslam_cv_pipeline/src/test/convert-maps-legacy.cc rename to aslam_cv_cameras/src/convert-maps-legacy.cc index 60261443..dc45cca2 100644 --- a/aslam_cv_pipeline/src/test/convert-maps-legacy.cc +++ b/aslam_cv_cameras/src/convert-maps-legacy.cc @@ -1,4 +1,4 @@ -#include +#include void aslam::convertMapsLegacy(cv::InputArray _map1, cv::InputArray _map2, cv::OutputArray _dstmap1, cv::OutputArray _dstmap2, diff --git a/aslam_cv_cameras/src/distortion.cc b/aslam_cv_cameras/src/distortion.cc index a263414a..f5f88f17 100644 --- a/aslam_cv_cameras/src/distortion.cc +++ b/aslam_cv_cameras/src/distortion.cc @@ -5,8 +5,10 @@ #include #include -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) { @@ -14,13 +16,13 @@ std::ostream& operator<<(std::ostream& out, const Distortion& distortion) { 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; } diff --git a/aslam_cv_cameras/src/ncamera.cc b/aslam_cv_cameras/src/ncamera.cc index ac7d7e02..b8252989 100644 --- a/aslam_cv_cameras/src/ncamera.cc +++ b/aslam_cv_cameras/src/ncamera.cc @@ -133,22 +133,54 @@ const Camera& NCamera::getCamera(size_t camera_index) const { return *cameras_[camera_index]; } +const Camera& NCamera::getCamera(const CameraId& camera_id) const { + CHECK(camera_id.isValid()); + int camera_idx = getCameraIndex(camera_id); + CHECK_GE(camera_idx, 0) << "Camera with ID " << camera_id + << " not in NCamera! container"; + return getCamera(camera_idx); +} + Camera& NCamera::getCameraMutable(size_t camera_index) { CHECK_LT(camera_index, cameras_.size()); CHECK_NOTNULL(cameras_[camera_index].get()); return *cameras_[camera_index]; } +Camera& NCamera::getCameraMutable(const CameraId& camera_id) { + CHECK(camera_id.isValid()); + int camera_idx = getCameraIndex(camera_id); + CHECK_GE(camera_idx, 0) << "Camera with ID " << camera_id + << " not in NCamera! container"; + return getCameraMutable(camera_idx); +} + Camera::Ptr NCamera::getCameraShared(size_t camera_index) { CHECK_LT(camera_index, cameras_.size()); return cameras_[camera_index]; } +Camera::Ptr NCamera::getCameraShared(const CameraId& camera_id) { + CHECK(camera_id.isValid()); + int camera_idx = getCameraIndex(camera_id); + CHECK_GE(camera_idx, 0) << "Camera with ID " << camera_id + << " not in NCamera! container"; + return getCameraShared(camera_idx); +} + Camera::ConstPtr NCamera::getCameraShared(size_t camera_index) const { CHECK_LT(camera_index, cameras_.size()); return cameras_[camera_index]; } +Camera::ConstPtr NCamera::getCameraShared(const CameraId& camera_id) const { + CHECK(camera_id.isValid()); + int camera_idx = getCameraIndex(camera_id); + CHECK_GE(camera_idx, 0) << "Camera with ID " << camera_id + << " not in NCamera! container"; + return getCameraShared(camera_idx); +} + void NCamera::setCamera(size_t camera_index, Camera::Ptr camera) { CHECK(camera); CHECK_LT(camera_index, cameras_.size()); diff --git a/aslam_cv_cameras/src/undistorter-mapped.cc b/aslam_cv_cameras/src/undistorter-mapped.cc new file mode 100644 index 00000000..0afe4b8d --- /dev/null +++ b/aslam_cv_cameras/src/undistorter-mapped.cc @@ -0,0 +1,108 @@ +#include "aslam/cameras/undistorter-mapped.h" + +#include +#include +#include +#include +#include // cv::remap + +namespace aslam { + +std::shared_ptr createMappedUndistorterToPinhole( + const aslam::UnifiedProjectionCamera& unified_proj_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. + UnifiedProjectionCamera::Ptr input_camera( + dynamic_cast(unified_proj_camera.clone())); + CHECK(input_camera); + + // Create the scaled output camera with removed distortion. + const bool kUndistortToPinhole = true; + Eigen::Matrix3d output_camera_matrix = common::getOptimalNewCameraMatrix( + *input_camera, alpha, scale, kUndistortToPinhole); + + Eigen::Matrix intrinsics; + intrinsics << output_camera_matrix(0, 0), output_camera_matrix(1, 1), + output_camera_matrix(0, 2), output_camera_matrix(1, 2); + + const int output_width = static_cast(scale * input_camera->imageWidth()); + const int output_height = + static_cast(scale * input_camera->imageHeight()); + PinholeCamera::Ptr output_camera = aslam::createCamera( + intrinsics, output_width, output_height); + CHECK(output_camera); + + cv::Mat map_u, map_v; + VLOG(1) << "Building undistorter map"; + common::buildUndistortMap( + *input_camera, *output_camera, CV_16SC2, map_u, map_v); + + // 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( + new MappedUndistorter( + input_camera, output_camera, map_u, map_v, map_u_float, map_v_float, + interpolation_type)); +} + +MappedUndistorter::MappedUndistorter() + : interpolation_method_(aslam::InterpolationMethod::Linear) {} + +MappedUndistorter::MappedUndistorter( + Camera::Ptr input_camera, Camera::Ptr output_camera, const cv::Mat& map_u, + const cv::Mat& map_v, const cv::Mat& map_u_float, + const cv::Mat& map_v_float, aslam::InterpolationMethod interpolation) + : Undistorter(input_camera, output_camera), + map_u_(map_u), + map_v_(map_v), + map_u_float_(map_u_float), + map_v_float_(map_v_float), + interpolation_method_(interpolation) { + CHECK_EQ(static_cast(map_u_.rows), output_camera->imageHeight()); + CHECK_EQ(static_cast(map_u_.cols), output_camera->imageWidth()); + CHECK_EQ(static_cast(map_v_.rows), output_camera->imageHeight()); + CHECK_EQ(static_cast(map_v_.cols), output_camera->imageWidth()); + CHECK_EQ( + static_cast(map_u_float_.rows), output_camera->imageHeight()); + CHECK_EQ(static_cast(map_u_float_.cols), output_camera->imageWidth()); + CHECK_EQ( + static_cast(map_v_float_.rows), output_camera->imageHeight()); + CHECK_EQ(static_cast(map_v_float_.cols), output_camera->imageWidth()); +} + +void MappedUndistorter::processImage( + const cv::Mat& input_image, cv::Mat* output_image) const { + CHECK_EQ(input_camera_->imageWidth(), static_cast(input_image.cols)); + CHECK_EQ(input_camera_->imageHeight(), static_cast(input_image.rows)); + CHECK_NOTNULL(output_image); + cv::remap( + input_image, *output_image, map_u_, map_v_, + static_cast(interpolation_method_)); +} + +void MappedUndistorter::processPoint( + const Eigen::Vector2d& input_point, Eigen::Vector2d* output_point) const { + CHECK_NOTNULL(output_point); + *output_point = input_point; + processPoint(output_point); +} + +void MappedUndistorter::processPoint(Eigen::Vector2d* point) const { + VLOG(250) << "Distorted point: " << (*point)[0] << "/" << (*point)[1]; + CHECK_LE((*point)[0], map_u_.cols); + CHECK_LE((*point)[1], map_v_.rows); + *point = Eigen::Vector2d( + map_u_float_.at((*point)[1], (*point)[0]), + map_v_float_.at((*point)[1], (*point)[0])); + + VLOG(250) << "Undistorted point: " << (*point)[0] << "/" << (*point)[1]; +} + +} // namespace aslam diff --git a/aslam_cv_pipeline/src/undistorter.cc b/aslam_cv_cameras/src/undistorter.cc similarity index 83% rename from aslam_cv_pipeline/src/undistorter.cc rename to aslam_cv_cameras/src/undistorter.cc index 11f82b7a..d4ec3e65 100644 --- a/aslam_cv_pipeline/src/undistorter.cc +++ b/aslam_cv_cameras/src/undistorter.cc @@ -1,4 +1,4 @@ -#include +#include namespace aslam { diff --git a/aslam_cv_pipeline/CMakeLists.txt b/aslam_cv_pipeline/CMakeLists.txt index 0e20df31..65c2d0e5 100644 --- a/aslam_cv_pipeline/CMakeLists.txt +++ b/aslam_cv_pipeline/CMakeLists.txt @@ -8,10 +8,6 @@ catkin_simple(ALL_DEPS_REQUIRED) # LIBRARIES # ############# set(HEADERS - include/aslam/pipeline/test/convert-maps-legacy.h - include/aslam/pipeline/undistorter.h - include/aslam/pipeline/undistorter-mapped.h - include/aslam/pipeline/undistorter-mapped-inl.h include/aslam/pipeline/visual-npipeline.h include/aslam/pipeline/visual-pipeline.h include/aslam/pipeline/visual-pipeline-brisk.h @@ -19,10 +15,7 @@ set(HEADERS include/aslam/pipeline/visual-pipeline-null.h ) -set(SOURCES - src/test/convert-maps-legacy.cc - src/undistorter.cc - src/undistorter-mapped.cc +set(SOURCES src/visual-npipeline.cc src/visual-pipeline-brisk.cc src/visual-pipeline-freak.cc @@ -40,9 +33,6 @@ SET(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS} ########## # GTESTS # ########## -catkin_add_gtest(test_undistorters test/test-undistorters.cc) -target_link_libraries(test_undistorters ${PROJECT_NAME}) - catkin_add_gtest(test_visual-npipeline test/test-visual-npipeline.cc) target_link_libraries(test_visual-npipeline ${PROJECT_NAME}) diff --git a/aslam_cv_pipeline/include/aslam/pipeline/visual-pipeline.h b/aslam_cv_pipeline/include/aslam/pipeline/visual-pipeline.h index 1d3f5698..cbc9b530 100644 --- a/aslam_cv_pipeline/include/aslam/pipeline/visual-pipeline.h +++ b/aslam_cv_pipeline/include/aslam/pipeline/visual-pipeline.h @@ -6,8 +6,8 @@ #include #include +#include #include -#include #include namespace aslam { diff --git a/aslam_cv_pipeline/src/undistorter-mapped.cc b/aslam_cv_pipeline/src/undistorter-mapped.cc deleted file mode 100644 index a572b733..00000000 --- a/aslam_cv_pipeline/src/undistorter-mapped.cc +++ /dev/null @@ -1,66 +0,0 @@ -#include "aslam/pipeline/undistorter-mapped.h" - -#include -#include -#include -#include -#include // cv::remap - -namespace aslam { - -std::unique_ptr createMappedUndistorterToPinhole( - const aslam::UnifiedProjectionCamera& unified_proj_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. - UnifiedProjectionCamera::Ptr input_camera( - dynamic_cast(unified_proj_camera.clone())); - CHECK(input_camera); - - // Create the scaled output camera with removed distortion. - const bool kUndistortToPinhole = true; - Eigen::Matrix3d output_camera_matrix = common::getOptimalNewCameraMatrix( - *input_camera, alpha, scale, kUndistortToPinhole); - - Eigen::Matrix intrinsics; - intrinsics << output_camera_matrix(0, 0), output_camera_matrix(1, 1), - output_camera_matrix(0, 2), output_camera_matrix(1, 2); - - const int output_width = static_cast(scale * input_camera->imageWidth()); - const int output_height = static_cast(scale * input_camera->imageHeight()); - PinholeCamera::Ptr output_camera = aslam::createCamera( - intrinsics, output_width, output_height); - CHECK(output_camera); - - cv::Mat map_u, map_v; - common::buildUndistortMap(*input_camera, *output_camera, CV_16SC2, map_u, map_v); - - return std::unique_ptr( - new MappedUndistorter(input_camera, output_camera, map_u, map_v, interpolation_type)); -} - -MappedUndistorter::MappedUndistorter() - : interpolation_method_(aslam::InterpolationMethod::Linear) {} - -MappedUndistorter::MappedUndistorter(Camera::Ptr input_camera, Camera::Ptr output_camera, - const cv::Mat& map_u, const cv::Mat& map_v, - aslam::InterpolationMethod interpolation) -: Undistorter(input_camera, output_camera), map_u_(map_u), map_v_(map_v), - interpolation_method_(interpolation) { - CHECK_EQ(static_cast(map_u_.rows), output_camera->imageHeight()); - CHECK_EQ(static_cast(map_u_.cols), output_camera->imageWidth()); - CHECK_EQ(static_cast(map_v_.rows), output_camera->imageHeight()); - CHECK_EQ(static_cast(map_v_.cols), output_camera->imageWidth()); -} - -void MappedUndistorter::processImage(const cv::Mat& input_image, cv::Mat* output_image) const { - CHECK_EQ(input_camera_->imageWidth(), static_cast(input_image.cols)); - CHECK_EQ(input_camera_->imageHeight(), static_cast(input_image.rows)); - CHECK_NOTNULL(output_image); - cv::remap(input_image, *output_image, map_u_, map_v_, static_cast(interpolation_method_)); -} - -} // namespace aslam diff --git a/aslam_cv_pipeline/src/visual-pipeline-brisk.cc b/aslam_cv_pipeline/src/visual-pipeline-brisk.cc index f6e6c66a..87a51329 100644 --- a/aslam_cv_pipeline/src/visual-pipeline-brisk.cc +++ b/aslam_cv_pipeline/src/visual-pipeline-brisk.cc @@ -1,7 +1,7 @@ #include +#include #include -#include #include #include diff --git a/aslam_cv_pipeline/src/visual-pipeline-freak.cc b/aslam_cv_pipeline/src/visual-pipeline-freak.cc index a1e372eb..cfae184e 100644 --- a/aslam_cv_pipeline/src/visual-pipeline-freak.cc +++ b/aslam_cv_pipeline/src/visual-pipeline-freak.cc @@ -1,6 +1,7 @@ #include + +#include #include -#include #include #include #include diff --git a/aslam_cv_pipeline/src/visual-pipeline.cc b/aslam_cv_pipeline/src/visual-pipeline.cc index 13e013d6..9605c6eb 100644 --- a/aslam_cv_pipeline/src/visual-pipeline.cc +++ b/aslam_cv_pipeline/src/visual-pipeline.cc @@ -1,8 +1,8 @@ #include #include +#include #include -#include #include diff --git a/aslam_cv_pipeline/test/test-undistorters.cc b/aslam_cv_pipeline/test/test-undistorters.cc deleted file mode 100644 index d0a54631..00000000 --- a/aslam_cv_pipeline/test/test-undistorters.cc +++ /dev/null @@ -1,168 +0,0 @@ -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/////////////////////////////////////////////// -// Types to test -/////////////////////////////////////////////// -template -struct CameraDistortion { - typedef Camera CameraType; - typedef Distortion DistortionType; -}; - -using testing::Types; -typedef Types, - CameraDistortion, - CameraDistortion, - CameraDistortion, - CameraDistortion, - CameraDistortion, - CameraDistortion, - CameraDistortion> Implementations; - -typedef Types, - CameraDistortion, - CameraDistortion, - CameraDistortion> - ImplementationsNoPinhole; - -/////////////////////////////////////////////// -// Test fixture -/////////////////////////////////////////////// -template -class TestUndistorters : public testing::Test { - public: - typedef typename CameraDistortion::CameraType CameraType; - typedef typename CameraDistortion::DistortionType DistortionType; - protected: - TestUndistorters() : camera_(CameraType::template createTestCamera() ) {}; - virtual ~TestUndistorters() {}; - typename CameraType::Ptr camera_; -}; - -template -class TestUndistortersNoPinhole : public TestUndistorters { }; - -TYPED_TEST_CASE(TestUndistorters, Implementations); -TYPED_TEST_CASE(TestUndistortersNoPinhole, ImplementationsNoPinhole); - -/////////////////////////////////////////////// -// Generic test cases (run for all models) -/////////////////////////////////////////////// -TYPED_TEST(TestUndistorters, TestMappedUndistorter) { - std::unique_ptr undistorter = - aslam::createMappedUndistorter(*(this->camera_), 1.0, 1.0, - aslam::InterpolationMethod::Linear); - ASSERT_EQ(undistorter->getOutputCamera().getType(), undistorter->getInputCamera().getType()); - ASSERT_EQ(undistorter->getOutputCamera().getDistortion().getType(), - aslam::Distortion::Type::kNoDistortion); - - // Convert map to non-fixed point representation for easy lookup of values. - const cv::Mat& map_u = undistorter->getUndistortMapU(); - const cv::Mat& map_v = undistorter->getUndistortMapV(); - 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); - - // Distort using the maps. - auto query_map = [&map_u_float, &map_v_float](double u, float v) { - const double u_map = map_u_float.at(v, u); - const double v_map = map_v_float.at(v, u); - return Eigen::Vector2d(u_map, v_map); - }; - - // Test the undistortion on some points. - const double ru = undistorter->getOutputCamera().imageWidth(); - const double rv = undistorter->getOutputCamera().imageHeight(); - - for (double u = 0; u < ru; ++u) { - for (double v = 0; v < rv; ++v) { - // Create random keypoint (round the coordinates to avoid interpolation on the maps) - Eigen::Vector2d keypoint_undistorted(u, v); - - // Distort using internal camera functions. - Eigen::Vector2d keypoint_distorted; - Eigen::Vector3d point_3d; - - CHECK(undistorter->getOutputCamera().backProject3(keypoint_undistorted, &point_3d)); - point_3d /= point_3d[2]; - this->camera_->project3(point_3d, &keypoint_distorted); - - Eigen::Vector2d keypoint_distorted_maps = query_map(keypoint_undistorted[0], - keypoint_undistorted[1]); - EXPECT_TRUE(EIGEN_MATRIX_NEAR(keypoint_distorted, keypoint_distorted_maps, 5e-2)); - } - } -} - -//////////////////////////////////// -// Camera model specific test cases -//////////////////////////////////// -TEST(TestUndistortersNoPinhole, TestMappedUndistorterUpcToPinhole) { - aslam::UnifiedProjectionCamera::Ptr camera = aslam::UnifiedProjectionCamera::createTestCamera< - aslam::RadTanDistortion>(); - - std::unique_ptr undistorter = - aslam::createMappedUndistorterToPinhole(*camera, 1.0, 1.0, - aslam::InterpolationMethod::Linear); - ASSERT_EQ(undistorter->getOutputCamera().getType(), aslam::Camera::Type::kPinhole); - ASSERT_EQ(undistorter->getOutputCamera().getDistortion().getType(), - aslam::Distortion::Type::kNoDistortion); - - // Convert map to non-fixed point representation for easy lookup of values. - const cv::Mat& map_u = undistorter->getUndistortMapU(); - const cv::Mat& map_v = undistorter->getUndistortMapV(); - cv::Mat map_u_copy = map_u.clone(); - cv::Mat map_v_copy = map_v.clone(); - aslam::convertMapsLegacy(map_u, map_v, map_u_copy, map_v_copy, CV_32FC1); - - // Distort using the maps. - auto query_map = [&map_u_copy, &map_v_copy](double u, float v) { - const double u_map = map_u_copy.at(v, u); - const double v_map = map_v_copy.at(v, u); - return Eigen::Vector2d(u_map, v_map); - }; - - // Test the undistortion on some random points. - const double ru = undistorter->getOutputCamera().imageWidth(); - const double rv = undistorter->getOutputCamera().imageHeight(); - - for (double u = 0; u < ru; ++u) { - for (double v = 0; v < rv; ++v) { - // Create random keypoint (round the coordinates to avoid interpolation on the maps) - Eigen::Vector2d keypoint_undistorted(u, v); - keypoint_undistorted[0] = std::floor(keypoint_undistorted[0]); - keypoint_undistorted[1] = std::floor(keypoint_undistorted[1]); - - // Distort using internal camera functions. - Eigen::Vector2d keypoint_distorted; - Eigen::Vector3d point_3d; - undistorter->getOutputCamera().backProject3(keypoint_undistorted, &point_3d); - point_3d /= point_3d[2]; - camera->project3(point_3d, &keypoint_distorted); - - Eigen::Vector2d keypoint_distorted_maps = query_map(keypoint_undistorted[0], - keypoint_undistorted[1]); - EXPECT_TRUE(EIGEN_MATRIX_NEAR(keypoint_distorted, keypoint_distorted_maps, 5e-2)); - } - } -} - -ASLAM_UNITTEST_ENTRYPOINT