From c6a574191de9f90d919ee0016904a1ea0f11e98b Mon Sep 17 00:00:00 2001 From: Ryan Pavlik Date: Mon, 13 Jan 2020 17:48:32 -0600 Subject: [PATCH] aux/tracking: Add image undistort/normalize cache mechanism --- doc/changes/aux/mr.255.md | 1 + src/xrt/auxiliary/tracking/t_calibration.cpp | 132 ++++++++++++++++++ .../tracking/t_calibration_opencv.hpp | 102 ++++++++++++++ 3 files changed, 235 insertions(+) create mode 100644 doc/changes/aux/mr.255.md diff --git a/doc/changes/aux/mr.255.md b/doc/changes/aux/mr.255.md new file mode 100644 index 000000000..033aafdec --- /dev/null +++ b/doc/changes/aux/mr.255.md @@ -0,0 +1 @@ +tracking: Add image undistort/normalize cache mechanism, to avoid needing to remap every frame. diff --git a/src/xrt/auxiliary/tracking/t_calibration.cpp b/src/xrt/auxiliary/tracking/t_calibration.cpp index 31ce1e960..8404a02ec 100644 --- a/src/xrt/auxiliary/tracking/t_calibration.cpp +++ b/src/xrt/auxiliary/tracking/t_calibration.cpp @@ -1318,3 +1318,135 @@ t_calibration_stereo_create(struct xrt_frame_context *xfctx, #endif return ret; } + +//! Helper for NormalizedCoordsCache constructors +static inline std::vector +generateInputCoordsAndReserveOutputCoords(cv::Size size, + std::vector &outputCoords) +{ + std::vector inputCoords; + + const auto n = size.width * size.height; + assert(n != 0); + inputCoords.reserve(n); + for (int row = 0; row < size.height; ++row) { + for (int col = 0; col < size.width; ++col) { + inputCoords.emplace_back(col, row); + } + } + outputCoords.reserve(inputCoords.size()); + return inputCoords; +} + +//! Helper for NormalizedCoordsCache constructors +static inline void +populateCacheMats(cv::Size size, + const std::vector &inputCoords, + const std::vector &outputCoords, + cv::Mat_ &cacheX, + cv::Mat_ &cacheY) +{ + assert(size.height != 0); + assert(size.width != 0); + cacheX.create(size); + cacheY.create(size); + const auto n = size.width * size.height; + // Populate the cache matrices + for (int i = 0; i < n; ++i) { + auto input = + cv::Point{int(inputCoords[i][0]), int(inputCoords[i][1])}; + cacheX(input) = outputCoords[i][0]; + cacheY(input) = outputCoords[i][1]; + } +} + +NormalizedCoordsCache::NormalizedCoordsCache( + cv::Size size, + const cv::Matx33d &intrinsics, + const cv::Matx &distortion) +{ + std::vector outputCoords; + std::vector inputCoords = + generateInputCoordsAndReserveOutputCoords(size, outputCoords); + // Undistort/reproject those coordinates in one call, to make use of + // cached internal/intermediate computations. + cv::undistortPoints(inputCoords, outputCoords, intrinsics, distortion); + + populateCacheMats(size, inputCoords, outputCoords, cacheX_, cacheY_); +} +NormalizedCoordsCache::NormalizedCoordsCache( + cv::Size size, + const cv::Matx33d &intrinsics, + const cv::Matx &distortion, + const cv::Matx33d &rectification, + const cv::Matx33d &new_camera_matrix) +{ + std::vector outputCoords; + std::vector inputCoords = + generateInputCoordsAndReserveOutputCoords(size, outputCoords); + // Undistort/reproject those coordinates in one call, to make use of + // cached internal/intermediate computations. + cv::undistortPoints(inputCoords, outputCoords, intrinsics, distortion, + rectification, new_camera_matrix); + + populateCacheMats(size, inputCoords, outputCoords, cacheX_, cacheY_); +} + +NormalizedCoordsCache::NormalizedCoordsCache( + cv::Size size, + const cv::Matx33d &intrinsics, + const cv::Matx &distortion, + const cv::Matx33d &rectification, + const cv::Matx &new_projection_matrix) +{ + std::vector outputCoords; + std::vector inputCoords = + generateInputCoordsAndReserveOutputCoords(size, outputCoords); + // Undistort/reproject those coordinates in one call, to make use of + // cached internal/intermediate computations. + cv::undistortPoints(inputCoords, outputCoords, intrinsics, distortion, + rectification, new_projection_matrix); + + populateCacheMats(size, inputCoords, outputCoords, cacheX_, cacheY_); +} +NormalizedCoordsCache::NormalizedCoordsCache(cv::Size size, + const cv::Mat &intrinsics, + const cv::Mat &distortion) +{ + std::vector outputCoords; + std::vector inputCoords = + generateInputCoordsAndReserveOutputCoords(size, outputCoords); + // Undistort/reproject those coordinates in one call, to make use of + // cached internal/intermediate computations. + cv::undistortPoints(inputCoords, outputCoords, intrinsics, distortion); + + populateCacheMats(size, inputCoords, outputCoords, cacheX_, cacheY_); +} + +cv::Vec2f +NormalizedCoordsCache::getNormalizedImageCoords(cv::Point2f origCoords) const +{ + /* + * getRectSubPix is more strict than the docs would imply: + * + * - Source must be 1 or 3 channels + * - Can sample from u8 into u8, u8 into f32, or f32 into f32 - that's + * it (though the latter is provided by a template function internally + * so could be extended...) + */ + cv::Mat patch; + cv::getRectSubPix(cacheX_, cv::Size(1, 1), origCoords, patch); + auto x = patch.at(0, 0); + cv::getRectSubPix(cacheY_, cv::Size(1, 1), origCoords, patch); + auto y = patch.at(0, 0); + return {x, y}; +} + +cv::Vec3f +NormalizedCoordsCache::getNormalizedVector(cv::Point2f origCoords) const +{ + // cameras traditionally look along -z, so we want negative sqrt + auto pt = getNormalizedImageCoords(origCoords); + auto z = -std::sqrt(1.f - pt.dot(pt)); + return {pt[0], pt[1], z}; +} diff --git a/src/xrt/auxiliary/tracking/t_calibration_opencv.hpp b/src/xrt/auxiliary/tracking/t_calibration_opencv.hpp index 9dc881f7a..3731cd01e 100644 --- a/src/xrt/auxiliary/tracking/t_calibration_opencv.hpp +++ b/src/xrt/auxiliary/tracking/t_calibration_opencv.hpp @@ -203,3 +203,105 @@ struct StereoRectificationMaps */ StereoRectificationMaps(t_stereo_camera_calibration *data); }; + + +/*! + * @brief Provides cached, precomputed access to normalized image coordinates + * from original, distorted ones. + * + * Populates internal structures using cv::undistortPoints() and performs + * subpixel sampling to interpolate for each query. Essentially, this class lets + * you perform cv::undistortPoints() while caching the initial setup work + * required for that function. + */ +class NormalizedCoordsCache +{ +public: + /*! + * @brief Set up the precomputed cache for a given camera. + * + * @param size Size of the image in pixels + * @param intrinsics Camera intrinsics matrix + * @param distortion Distortion coefficients + * + * This overload applies no rectification (`R`) and uses a + * normalized/identity new camera matrix (`P`). + */ + NormalizedCoordsCache(cv::Size size, + const cv::Matx33d &intrinsics, + const cv::Matx &distortion); + /*! + * @brief Set up the precomputed cache for a given camera (overload for + * rectification and new camera matrix) + * + * @param size Size of the image in pixels + * @param intrinsics Camera intrinsics matrix + * @param distortion Distortion coefficients + * @param rectification Rectification matrix - corresponds to parameter + * `R` to cv::undistortPoints(). + * @param new_camera_matrix A 3x3 new camera matrix - corresponds to + * parameter `P` to cv::undistortPoints(). + */ + NormalizedCoordsCache(cv::Size size, + const cv::Matx33d &intrinsics, + const cv::Matx &distortion, + const cv::Matx33d &rectification, + const cv::Matx33d &new_camera_matrix); + + /*! + * @brief Set up the precomputed cache for a given camera. (overload for + * rectification and new projection matrix) + * + * @param size Size of the image in pixels + * @param intrinsics Camera intrinsics matrix + * @param distortion Distortion coefficients + * @param rectification Rectification matrix - corresponds to parameter + * `R` to cv::undistortPoints(). + * @param new_projection_matrix A 3x4 new projection matrix - + * corresponds to parameter `P` to cv::undistortPoints(). + */ + NormalizedCoordsCache( + cv::Size size, + const cv::Matx33d &intrinsics, + const cv::Matx &distortion, + const cv::Matx33d &rectification, + const cv::Matx &new_projection_matrix); + + /*! + * @brief Set up the precomputed cache for a given camera. + * + * Less-strongly-typed overload. + * + * @overload + * + * This overload applies no rectification (`R`) and uses a + * normalized/identity new camera matrix (`P`). + */ + NormalizedCoordsCache(cv::Size size, + const cv::Mat &intrinsics, + const cv::Mat &distortion); + + /*! + * @brief Get normalized, undistorted coordinates from a point in the + * original (distorted, etc.) image. + * + * @param origCoords Image coordinates in original image + * + * @return Corresponding undistorted coordinates in a "normalized" image + */ + cv::Vec2f + getNormalizedImageCoords(cv::Point2f origCoords) const; + + /*! + * @brief Get normalized vector in the camera-space direction + * corresponding to the original (distorted, etc.) image coordinates. + * + * Note that the Z component will be negative by convention. + */ + cv::Vec3f + getNormalizedVector(cv::Point2f origCoords) const; + +private: + cv::Mat_ cacheX_; + cv::Mat_ cacheY_; +};