Changing fixed3d_landmark to use a variable number of residuals.

locusrobotics · Apr 8, 2024 · e17b717 · e17b717
1 parent 3188b48
commit e17b717
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 8 deletions.
diff --git a/fuse_constraints/src/fixed_3d_landmark_constraint.cpp b/fuse_constraints/src/fixed_3d_landmark_constraint.cpp
@@ -107,8 +107,9 @@ void Fixed3DLandmarkConstraint::print(std::ostream& stream) const
 
 ceres::CostFunction* Fixed3DLandmarkConstraint::costFunction() const
 {
-  return new ceres::AutoDiffCostFunction<Fixed3DLandmarkCostFunctor, 8, 3, 4, 4>(
-      new Fixed3DLandmarkCostFunctor(sqrt_information_, mean_, observations_, pts3d_));
+  // 2 Residuals Per 3D point
+  return new ceres::AutoDiffCostFunction<Fixed3DLandmarkCostFunctor, ceres::DYNAMIC, 3, 4, 4>(
+      new Fixed3DLandmarkCostFunctor(sqrt_information_, mean_, observations_, pts3d_), 2*pts3d_.rows());
 }
 
 }  // namespace fuse_constraints

diff --git a/fuse_constraints/src/fixed_3d_landmark_simple_covariance_constraint.cpp b/fuse_constraints/src/fixed_3d_landmark_simple_covariance_constraint.cpp
@@ -106,8 +106,8 @@ void Fixed3DLandmarkSimpleCovarianceConstraint::print(std::ostream& stream) cons
 
 ceres::CostFunction* Fixed3DLandmarkSimpleCovarianceConstraint::costFunction() const
 {
-  return new ceres::AutoDiffCostFunction<Fixed3DLandmarkSimpleCovarianceCostFunctor, 8, 3, 4, 4>(
-      new Fixed3DLandmarkSimpleCovarianceCostFunctor(sqrt_information_, mean_, observations_, pts3d_));
+  return new ceres::AutoDiffCostFunction<Fixed3DLandmarkSimpleCovarianceCostFunctor, ceres::DYNAMIC, 3, 4, 4>(
+      new Fixed3DLandmarkSimpleCovarianceCostFunctor(sqrt_information_, mean_, observations_, pts3d_), pts3d_.rows());
 }
 
 }  // namespace fuse_constraints

diff --git a/fuse_constraints/test/test_fixed_3d_landmark_constraint.cpp b/fuse_constraints/test/test_fixed_3d_landmark_constraint.cpp
@@ -460,6 +460,8 @@ TEST(Fixed3DLandmarkConstraint, OptimizationPoints)
   EXPECT_NEAR(643.10717773437500, calibration_variable->fy(), 1.0e-3);
   EXPECT_NEAR(310.29060457226840, calibration_variable->cx(), 1.0e-3);
   EXPECT_NEAR(237.80861559081677, calibration_variable->cy(), 1.0e-3);
+
+  EXPECT_EQ(16, constraint->costFunction()->num_residuals());  // Ensure we have the right number of residuals.
 }
 
 TEST(Fixed3DLandmarkConstraint, MultiViewOptimization)

diff --git a/fuse_constraints/test/test_fixed_3d_landmark_constraint.py b/fuse_constraints/test/test_fixed_3d_landmark_constraint.py
@@ -225,8 +225,8 @@ def MultiViewOptimization(self):
 if __name__ == '__main__':
     tests = PinholeCameraProjection()
 
-    # tests.Optimization()
+    tests.Optimization()
     tests.OptimizationSnavelly()
-    # tests.OptimizationScaledMarker()
-    # tests.OptimizationPoints()
-    # tests.MultiViewOptimization()
+    tests.OptimizationScaledMarker()
+    tests.OptimizationPoints()
+    tests.MultiViewOptimization()