bug/rectification

components/rgbd-sources/src/sources/stereovideo/rectification.cpp

@@ -140,25 +140,29 @@ void StereoRectification::rectify(cv::InputOutputArray im, Channel c) {
 }
 
 cv::Mat StereoRectification::getPose(Channel c) {
+	// NOTE: FTL poses are camera-to-world transformations while the parameters
+	//		 in calibration are world-to-camera. cv::stereoRectify() rotation
+	//		 is unrectified-to-rectified.
+
 	using ftl::calibration::transform::inverse;
 
 	if (enabled_ && valid_) {
-		cv::Mat T = cv::Mat::eye(4, 4, CV_64FC1);
+		cv::Mat R = cv::Mat::eye(4, 4, CV_64FC1);
 		if (c == Channel::Left) {
-			R_l_.copyTo(T(cv::Rect(0, 0, 3, 3)));
-			return calib_left_.extrinsic.matrix() * inverse(T);
+			R_l_.copyTo(R(cv::Rect(0, 0, 3, 3)));
+			return inverse(R * calib_left_.extrinsic.matrix());
 		}
 		else if (c == Channel::Right) {
-			R_r_.copyTo(T(cv::Rect(0, 0, 3, 3)));
-			return calib_right_.extrinsic.matrix() * inverse(T);
+			R_r_.copyTo(R(cv::Rect(0, 0, 3, 3)));
+			return inverse(R * calib_right_.extrinsic.matrix());
 		}
 	}
 	else {
 		if (c == Channel::Left) {
-			return calib_left_.extrinsic.matrix();
+			return inverse(calib_left_.extrinsic.matrix());
 		}
 		else if (c == Channel::Right) {
-			return calib_right_.extrinsic.matrix();
+			return inverse(calib_right_.extrinsic.matrix());
 		}
 	}
 	throw ftl::exception("Invalid channel, expected Left or Right");

components/rgbd-sources/src/sources/stereovideo/rectification.hpp

@@ -21,12 +21,15 @@ namespace detail {
 
 /**
  * Stereo rectification. Performs rectification for left and right channels.
- * Rectified image is same size as input image.
+ * Rectified image is same size as input image. Camera parameters reported by
+ * getPose() and cameraMatrix() are for rectified camera (if enabled and valid
+ * calibration set).
  */
 class StereoRectification : public ftl::Configurable {
 public:
 	StereoRectification(nlohmann::json &config, cv::Size image_size);
 
+	/** Set OpenCV interpolation mode, see cv::InterpolationFlags */
 	void setInterpolation(int interpolation);
 
 	void setSize(cv::Size);
@@ -38,26 +41,23 @@ public:
 
 	void rectify(cv::InputOutputArray im, ftl::codecs::Channel c);
 
-	/**
-	 * Enable/disable rectification.
+	/** Enable/disable rectification. TODO: move outside (to stereovideo)?
 	 */
 	void setEnabled(bool enabled);
 	bool enabled();
 
-	/**
-	 * Get camera pose (rectified if enabled and valid)
+	/** Get camera pose (camera to world). Returns rectified pose if
+	 * rectification is enabled (and valid calibration is set).
 	 */
 	cv::Mat getPose(ftl::codecs::Channel c = ftl::codecs::Channel::Left);
 
-	/**
-	 * Get intrinsic matrix.
-	 */
+	/** Get intrinsic matrix. */
 	cv::Mat cameraMatrix(ftl::codecs::Channel c = ftl::codecs::Channel::Left);
 	cv::Mat cameraMatrix(cv::Size size, ftl::codecs::Channel c = ftl::codecs::Channel::Left);
 
-	/** Stereo baseline */
+	/** Stereo baseline. In same unit as calibration (usually meters) */
 	double baseline();
-	/** Disparity offset */
+	/** Disparity offset (pixels) */
 	double doff();
 	double doff(cv::Size);
 
@@ -65,13 +65,12 @@ protected:
 	void calculateParameters();
 
 private:
-	cv::Size calib_resolution_;
 	ftl::calibration::CalibrationData::Calibration calib_left_;
 	ftl::calibration::CalibrationData::Calibration calib_right_;
 
 	cv::Size image_resolution_;
 
-	// rectification parameters and maps
+	// rectification parameters
 	bool enabled_;
 	bool valid_;
 	int interpolation_;
@@ -82,12 +81,13 @@ private:
 	cv::Mat P_l_;
 	cv::Mat P_r_;
 
+	// rectification maps for cv::remap(); should be CV_16SC2 if remap done on
+	// CPU and CV_32SC2 for GPU (generated by calculateParameters(), used by
+	// rectify())
 	std::pair<cv::Mat,cv::Mat> map_l_;
 	std::pair<cv::Mat,cv::Mat> map_r_;
 
-	// temporary buffers
-	// cv::cuda::HostMem tmp_l_;
-	// cv::cuda::HostMem tmp_r_;
+	// temporary buffers for left/right
 	cv::Mat tmp_l_;
 	cv::Mat tmp_r_;
 };