Added homography check capability for 3D correspondences (#82)

Jmeyer1292 · Sep 25, 2020 · 8692a0b · 8692a0b
1 parent 6e7b0f7
commit 8692a0b
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 0 deletions.
diff --git a/rct_optimizations/include/rct_optimizations/validation/homography_validation.h b/rct_optimizations/include/rct_optimizations/validation/homography_validation.h
@@ -68,4 +68,14 @@ struct ModifiedCircleGridCorrespondenceSampler : CorrespondenceSampler
 Eigen::VectorXd calculateHomographyError(const Correspondence2D3D::Set &correspondences,
                                          const CorrespondenceSampler &correspondence_sampler);
 
+/**
+ * @brief Calculates the homography error for correspondences of 3D planar points using @ref calculateHomographyError
+ * @param correspondences - A set of corresponding points
+ * @param correspondence_sampler - a struct for choosing correspondence indices to generate the homography matrix
+ * @return A vector of homography errors for each correspondence
+ */
+Eigen::VectorXd calculateHomographyError(const Correspondence3D3D::Set &correspondences,
+                                         const CorrespondenceSampler &correspondence_sampler);
+
+
 } //rct_optimizations
diff --git a/rct_optimizations/src/rct_optimizations/validation/homography_validation.cpp b/rct_optimizations/src/rct_optimizations/validation/homography_validation.cpp
@@ -105,5 +105,36 @@ Eigen::VectorXd calculateHomographyError(const Correspondence2D3D::Set &correspo
   return error;
 }
 
+Eigen::VectorXd calculateHomographyError(const Correspondence3D3D::Set& correspondences,
+                                         const CorrespondenceSampler& correspondence_sampler)
+{
+  // Convert the 3D correspondence points into 2D points by scaling the x and y components by the z component
+  Correspondence2D3D::Set correspondences_2d;
+  correspondences_2d.reserve(correspondences.size());
+
+  // Store the z values for scaling later
+  Eigen::VectorXd z_values(correspondences.size());
+
+  for (std::size_t i = 0; i < correspondences.size(); ++i)
+  {
+    const auto& corr = correspondences[i];
+
+    // Store the z value of the correspondence
+    z_values[i] = corr.in_image.z();
+
+    // Generate a scaled 2D correspondence
+    Correspondence2D3D corr_2d;
+    corr_2d.in_target = corr.in_target;
+    corr_2d.in_image = (corr.in_image / corr.in_image.z()).head<2>();
+    correspondences_2d.push_back(corr_2d);
+  }
+
+  // Calculate the homography error
+  Eigen::VectorXd error = calculateHomographyError(correspondences_2d, correspondence_sampler);
+
+  // Scale the errors again by the z values of the original correspondences
+  return error.cwiseProduct(z_values);
+}
+
 } // namespace rct_optimizations