aux/tracking: Add image undistort/normalize cache mechanism

doc/changes/aux/mr.255.md

@@ -0,0 +1 @@
+tracking: Add image undistort/normalize cache mechanism, to avoid needing to remap every frame.

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<cv::Vec2f>
+generateInputCoordsAndReserveOutputCoords(cv::Size size,
+                                          std::vector<cv::Vec2f> &outputCoords)
+{
+	std::vector<cv::Vec2f> 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<cv::Vec2f> &inputCoords,
+                  const std::vector<cv::Vec2f> &outputCoords,
+                  cv::Mat_<float> &cacheX,
+                  cv::Mat_<float> &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<double, 5, 1> &distortion)
+{
+	std::vector<cv::Vec2f> outputCoords;
+	std::vector<cv::Vec2f> 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<double, 5, 1> &distortion,
+    const cv::Matx33d &rectification,
+    const cv::Matx33d &new_camera_matrix)
+{
+	std::vector<cv::Vec2f> outputCoords;
+	std::vector<cv::Vec2f> 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<double, 5, 1> &distortion,
+    const cv::Matx33d &rectification,
+    const cv::Matx<double, 3, 4> &new_projection_matrix)
+{
+	std::vector<cv::Vec2f> outputCoords;
+	std::vector<cv::Vec2f> 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<cv::Vec2f> outputCoords;
+	std::vector<cv::Vec2f> 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<float>(0, 0);
+	cv::getRectSubPix(cacheY_, cv::Size(1, 1), origCoords, patch);
+	auto y = patch.at<float>(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};
+}

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<double, 5, 1> &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<double, 5, 1> &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<double, 5, 1> &distortion,
+	    const cv::Matx33d &rectification,
+	    const cv::Matx<double, 3, 4> &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_<float> cacheX_;
+	cv::Mat_<float> cacheY_;
+};