Fix ros2 build (#1)

* changes to get building with latest rclcpp * Fix stringop-overread compile issues --------- Co-authored-by: Stephen Williams <[email protected]>
PickNikRobotics · Sep 17, 2024 · d0a6a0b · d0a6a0b
1 parent ff41ec0
commit d0a6a0b
Show file tree

Hide file tree

Showing 8 changed files with 115 additions and 108 deletions.
diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp
@@ -120,7 +120,7 @@ class NormalDeltaOrientation3DCostFunctor
 
     // Scale the residuals by the square root information matrix to account for the measurement
     // uncertainty.
-    Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>> residuals_map(residuals, A_.rows());
+    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals);
     residuals_map.applyOnTheLeft(A_.template cast<T>());
 
     return true;

diff --git a/fuse_constraints/test/test_absolute_constraint.cpp b/fuse_constraints/test/test_absolute_constraint.cpp
@@ -59,9 +59,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::AccelerationAngular2DStamped variable(
       rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby"));
-    fuse_core::Vector1d mean;
+    fuse_core::VectorXd mean(1);
     mean << 3.0;
-    fuse_core::Matrix1d cov;
+    fuse_core::MatrixXd cov(1, 1);
     cov << 1.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint constraint(
@@ -70,9 +70,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::AccelerationLinear2DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("bender"));
-    fuse_core::Vector2d mean;
+    fuse_core::VectorXd mean(2);
     mean << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint(
@@ -81,9 +81,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::Orientation2DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("johnny5"));
-    fuse_core::Vector1d mean;
+    fuse_core::VectorXd mean(1);
     mean << 3.0;
-    fuse_core::Matrix1d cov;
+    fuse_core::MatrixXd cov(1, 1);
     cov << 1.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsoluteOrientation2DStampedConstraint constraint(
@@ -92,9 +92,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::Position2DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("rosie"));
-    fuse_core::Vector2d mean;
+    fuse_core::VectorXd mean(2);
     mean << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsolutePosition2DStampedConstraint constraint(
@@ -103,9 +103,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("clank"));
-    fuse_core::Vector3d mean;
+    fuse_core::VectorXd mean(3);
     mean << 1.0, 2.0, 3.0;
-    fuse_core::Matrix3d cov;
+    fuse_core::MatrixXd cov(3, 3);
     cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsolutePosition3DStampedConstraint constraint(
@@ -114,9 +114,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::VelocityAngular2DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("gort"));
-    fuse_core::Vector1d mean;
+    fuse_core::VectorXd mean(1);
     mean << 3.0;
-    fuse_core::Matrix1d cov;
+    fuse_core::MatrixXd cov(1, 1);
     cov << 1.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint constraint(
@@ -125,9 +125,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::VelocityLinear2DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("bishop"));
-    fuse_core::Vector2d mean;
+    fuse_core::VectorXd mean(2);
     mean << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint constraint(
@@ -140,9 +140,9 @@ TEST(AbsoluteConstraint, PartialMeasurement)
 {
   fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678),
     fuse_core::uuid::generate("vici"));
-  fuse_core::Vector2d mean;
+  fuse_core::VectorXd mean(2);
   mean << 3.0, 1.0;
-  fuse_core::Matrix2d cov;
+  fuse_core::MatrixXd cov(2, 2);
   cov << 3.0, 0.2, 0.2, 1.0;
   auto indices = std::vector<size_t>{2, 0};
   EXPECT_NO_THROW(
@@ -157,9 +157,9 @@ TEST(AbsoluteConstraint, Covariance)
     // Verify the covariance <--> sqrt information conversions are correct
     fuse_variables::AccelerationLinear2DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("chappie"));
-    fuse_core::Vector2d mean;
+    fuse_core::VectorXd mean(2);
     mean << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint("test", variable,
       mean, cov);
@@ -178,9 +178,9 @@ TEST(AbsoluteConstraint, Covariance)
   {
     fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("astroboy"));
-    fuse_core::Vector2d mean;
+    fuse_core::VectorXd mean(2);
     mean << 3.0, 1.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 3.0, 0.2, 0.2, 1.0;
     auto indices = std::vector<size_t>{2, 0};
     fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov,
@@ -214,9 +214,9 @@ TEST(AbsoluteConstraint, Optimization)
     variable->x() = 10.7;
     variable->y() = -3.2;
     // Create an absolute constraint
-    fuse_core::Vector2d mean;
+    fuse_core::VectorXd mean(2);
     mean << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     auto constraint = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared(
       "test",
@@ -354,9 +354,9 @@ TEST(AbsoluteConstraint, PartialOptimization)
   var->z() = 0.9;
 
   // Create a partial constraint for the first and third indices
-  fuse_core::Vector2d mean1;
+  fuse_core::VectorXd mean1(2);
   mean1 << 1.0, 3.0;
-  fuse_core::Matrix2d cov1;
+  fuse_core::MatrixXd cov1(2, 2);
   /* *INDENT-OFF* */
   cov1 << 1.0, 0.0,
           0.0, 1.0;
@@ -367,9 +367,9 @@ TEST(AbsoluteConstraint, PartialOptimization)
     "test", *var, mean1, cov1, indices1);
 
   // Create another constraint for the second index
-  fuse_core::Vector1d mean2;
+  fuse_core::VectorXd mean2(1);
   mean2 << 2.0;
-  fuse_core::Matrix1d cov2;
+  fuse_core::MatrixXd cov2(1, 1);
   cov2 << 1.0;
   std::vector<size_t> indices2 = {fuse_variables::Position3DStamped::Y};
   auto constraint2 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared(
@@ -416,9 +416,9 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization)
     fuse_core::uuid::generate("tiktok"));
   variable->yaw() = 0.7;
   // Create an absolute constraint
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 7.0;
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 0.10;
   auto constraint = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared(
     "test", *variable, mean, cov);
@@ -463,9 +463,9 @@ TEST(AbsoluteConstraint, Serialization)
   // Construct a constraint
   fuse_variables::AccelerationAngular2DStamped variable(rclcpp::Time(1234, 5678),
     fuse_core::uuid::generate("robby"));
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 3.0;
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint expected("test", variable, mean,
     cov);

diff --git a/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp
@@ -58,9 +58,9 @@ TEST(AbsoluteOrientation2DStampedConstraint, Constructor)
   // Construct a constraint just to make sure it compiles.
   Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678),
     fuse_core::uuid::generate("walle"));
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 1.0;
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   EXPECT_NO_THROW(
     AbsoluteOrientation2DStampedConstraint constraint(
@@ -73,9 +73,9 @@ TEST(AbsoluteOrientation2DStampedConstraint, Covariance)
   // Verify the covariance <--> sqrt information conversions are correct
   Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate(
       "mo"));
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 1.0;
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   AbsoluteOrientation2DStampedConstraint constraint("test", orientation_variable, mean, cov);
 
@@ -99,10 +99,9 @@ TEST(AbsoluteOrientation2DStampedConstraint, Optimization)
   orientation_variable->yaw() = 1.0;
 
   // Create an absolute orientation constraint
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 1.0;
-
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared(
     "test",
@@ -168,10 +167,9 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero)
   orientation_variable->yaw() = 0.0;
 
   // Create an absolute orientation constraint
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 0.0;
-
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared(
     "test",
@@ -308,10 +306,9 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi)
   orientation_variable->yaw() = -M_PI;
 
   // Create an absolute orientation constraint
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << -M_PI;
-
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared(
     "test",
@@ -370,9 +367,9 @@ TEST(AbsoluteOrientation2DStampedConstraint, Serialization)
   // Construct a constraint
   Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678),
     fuse_core::uuid::generate("walle"));
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 1.0;
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   AbsoluteOrientation2DStampedConstraint expected("test", orientation_variable, mean, cov);