From 1cfe8536cc1e6d605bf36654bbc80d7c0fc4789c Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Tue, 1 Aug 2023 20:13:08 -0700 Subject: [PATCH 1/2] Add some unit tests for the 2D orientation constraints; Create getters/setters for the 2D orientation variable is preparation for a fix. --- fuse_constraints/test/CMakeLists.txt | 1 + .../test/test_absolute_constraint.cpp | 4 +- ...lute_orientation_2d_stamped_constraint.cpp | 382 ++++++++++++++++++ ...st_absolute_pose_2d_stamped_constraint.cpp | 8 +- .../test/test_relative_constraint.cpp | 8 +- ...st_relative_pose_2d_stamped_constraint.cpp | 16 +- .../fuse_models/common/sensor_proc.hpp | 10 +- fuse_models/src/odometry_2d_publisher.cpp | 2 +- fuse_models/src/unicycle_2d_ignition.cpp | 15 +- fuse_models/test/test_unicycle_2d.cpp | 10 +- fuse_publishers/src/path_2d_publisher.cpp | 2 +- fuse_publishers/src/pose_2d_publisher.cpp | 2 +- .../test/test_path_2d_publisher.cpp | 8 +- .../test/test_pose_2d_publisher.cpp | 8 +- .../fuse_variables/orientation_2d_stamped.hpp | 16 + fuse_variables/src/orientation_2d_stamped.cpp | 2 +- .../test/test_orientation_2d_stamped.cpp | 8 +- fuse_viz/include/fuse_viz/conversions.hpp | 2 +- 18 files changed, 452 insertions(+), 52 deletions(-) create mode 100644 fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp diff --git a/fuse_constraints/test/CMakeLists.txt b/fuse_constraints/test/CMakeLists.txt index 09f42f98f..d1797385f 100644 --- a/fuse_constraints/test/CMakeLists.txt +++ b/fuse_constraints/test/CMakeLists.txt @@ -1,6 +1,7 @@ # CORE GTESTS ====================================================================================== set(TEST_TARGETS test_absolute_constraint + test_absolute_orientation_2d_stamped_constraint test_absolute_orientation_3d_stamped_constraint test_absolute_orientation_3d_stamped_euler_constraint test_absolute_pose_2d_stamped_constraint diff --git a/fuse_constraints/test/test_absolute_constraint.cpp b/fuse_constraints/test/test_absolute_constraint.cpp index 7b113552a..9a7e91ee9 100644 --- a/fuse_constraints/test/test_absolute_constraint.cpp +++ b/fuse_constraints/test/test_absolute_constraint.cpp @@ -401,7 +401,7 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) auto variable = fuse_variables::Orientation2DStamped::make_shared( rclcpp::Time(1234, 5678), fuse_core::uuid::generate("tiktok")); - variable->yaw() = 0.7; + variable->setYaw(0.7); // Create an absolute constraint fuse_core::Vector1d mean; mean << 7.0; @@ -428,7 +428,7 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); // Check - EXPECT_NEAR(7.0 - 2 * M_PI, variable->yaw(), 1.0e-5); + EXPECT_NEAR(7.0 - 2 * M_PI, variable->getYaw(), 1.0e-5); // Compute the covariance std::vector> covariance_blocks; covariance_blocks.emplace_back(variable->data(), variable->data()); diff --git a/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp new file mode 100644 index 000000000..80b98d5ec --- /dev/null +++ b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp @@ -0,0 +1,382 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using fuse_constraints::AbsoluteOrientation2DStampedConstraint; +using fuse_variables::Orientation2DStamped; + + +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; + mean << 1.0; + fuse_core::Matrix1d cov; + cov << 1.0; + EXPECT_NO_THROW( + AbsoluteOrientation2DStampedConstraint constraint( + "test", orientation_variable, + mean, cov)); +} + +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; + mean << 1.0; + fuse_core::Matrix1d cov; + cov << 1.0; + AbsoluteOrientation2DStampedConstraint constraint("test", orientation_variable, mean, cov); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix1d expected_sqrt_info; + expected_sqrt_info << 1.0; + fuse_core::Matrix1d expected_cov = cov; + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-9); +} + +TEST(AbsoluteOrientation2DStampedConstraint, Optimization) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are + // generated. Create the variables + auto orientation_variable = Orientation2DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->setYaw(1.0); + + // Create an absolute orientation constraint + fuse_core::Vector1d mean; + mean << 1.0; + + fuse_core::Matrix1d cov; + cov << 1.0; + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *orientation_variable, + mean, + cov); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), + orientation_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(orientation_variable->data()); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + EXPECT_TRUE(summary.IsSolutionUsable()) << summary.FullReport(); + + // Check + EXPECT_NEAR(1.0, orientation_variable->getYaw(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + + // Define the expected covariance + fuse_core::Matrix1d expected_covariance; + expected_covariance << 1.0; + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); +} + +TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero) +{ + // Optimize a single orientation at zero and single constraint, verify the expected value and + // covariance are generated. + + // Create the variables + auto orientation_variable = Orientation2DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->setYaw(0.0); + + // Create an absolute orientation constraint + fuse_core::Vector1d mean; + mean << 0.0; + + fuse_core::Matrix1d cov; + cov << 1.0; + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *orientation_variable, + mean, + cov); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), + orientation_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(orientation_variable->data()); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + EXPECT_TRUE(summary.IsSolutionUsable()) << summary.FullReport(); + + // Check + EXPECT_NEAR(0.0, orientation_variable->getYaw(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + + // Define the expected covariance + fuse_core::Matrix1d expected_covariance; + expected_covariance << 1.0; + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); +} + +TEST(AbsoluteOrientation2DStampedConstraint, OptimizationPositivePi) +{ + // Optimize a single orientation at +PI and single constraint, verify the expected value and + // covariance are generated. + + // Create the variables + auto orientation_variable = Orientation2DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->setYaw(M_PI); + + // Create an absolute orientation constraint + fuse_core::Vector1d mean; + mean << M_PI; + + fuse_core::Matrix1d cov; + cov << 1.0; + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *orientation_variable, + mean, + cov); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), + orientation_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(orientation_variable->data()); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + EXPECT_TRUE(summary.IsSolutionUsable()) << summary.FullReport(); + + // Check + // We expect +PI to roll over to -PI because our range is [-PI, PI) + EXPECT_NEAR(-M_PI, orientation_variable->getYaw(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + + // Define the expected covariance + fuse_core::Matrix1d expected_covariance; + expected_covariance << 1.0; + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); +} + +TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) +{ + // Optimize a single orientation at -PI and single constraint, verify the expected value and + // covariance are generated. + + // Create the variables + auto orientation_variable = Orientation2DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->setYaw(-M_PI); + + // Create an absolute orientation constraint + fuse_core::Vector1d mean; + mean << -M_PI; + + fuse_core::Matrix1d cov; + cov << 1.0; + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *orientation_variable, + mean, + cov); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), + orientation_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(orientation_variable->data()); + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + EXPECT_TRUE(summary.IsSolutionUsable()) << summary.FullReport(); + + // Check + EXPECT_NEAR(-M_PI, orientation_variable->getYaw(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + + // Define the expected covariance + fuse_core::Matrix1d expected_covariance; + expected_covariance << 1.0; + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); +} + +TEST(AbsoluteOrientation2DStampedConstraint, Serialization) +{ + // Construct a constraint + Orientation2DStamped orientation_variable( + rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); + fuse_core::Vector1d mean; + mean << 1.0; + fuse_core::Matrix1d cov; + cov << 1.0; + AbsoluteOrientation2DStampedConstraint expected("test", orientation_variable, mean, cov); + + // Serialize the constraint into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new constraint from that same stream + AbsoluteOrientation2DStampedConstraint actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.uuid(), actual.uuid()); + EXPECT_EQ(expected.variables(), actual.variables()); + EXPECT_MATRIX_EQ(expected.mean(), actual.mean()); + EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp index 927c24517..f8ae2047a 100644 --- a/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp @@ -99,7 +99,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) // generated. Create the variables auto orientation_variable = Orientation2DStamped::make_shared( rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); - orientation_variable->yaw() = 0.8; + orientation_variable->setYaw(0.8); auto position_variable = Position2DStamped::make_shared( rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; @@ -141,7 +141,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) // Check EXPECT_NEAR(1.0, position_variable->x(), 1.0e-5); EXPECT_NEAR(2.0, position_variable->y(), 1.0e-5); - EXPECT_NEAR(3.0, orientation_variable->yaw(), 1.0e-5); + EXPECT_NEAR(3.0, orientation_variable->getYaw(), 1.0e-5); // Compute the covariance std::vector> covariance_blocks; covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); @@ -184,7 +184,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) // generated. Create the variables auto orientation_variable = Orientation2DStamped::make_shared( rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); - orientation_variable->yaw() = 0.8; + orientation_variable->setYaw(0.8); auto position_variable = Position2DStamped::make_shared( rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; @@ -255,7 +255,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) // Check EXPECT_NEAR(1.0, position_variable->x(), 1.0e-5); EXPECT_NEAR(2.0, position_variable->y(), 1.0e-5); - EXPECT_NEAR(3.0, orientation_variable->yaw(), 1.0e-5); + EXPECT_NEAR(3.0, orientation_variable->getYaw(), 1.0e-5); // Compute the covariance std::vector> covariance_blocks; diff --git a/fuse_constraints/test/test_relative_constraint.cpp b/fuse_constraints/test/test_relative_constraint.cpp index 821f2dfe5..f6e0398cc 100644 --- a/fuse_constraints/test/test_relative_constraint.cpp +++ b/fuse_constraints/test/test_relative_constraint.cpp @@ -443,11 +443,11 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) auto x1 = fuse_variables::Orientation2DStamped::make_shared( rclcpp::Time(1234, 5678), fuse_core::uuid::generate("t800")); - x1->yaw() = 0.7; + x1->setYaw(0.7); auto x2 = fuse_variables::Orientation2DStamped::make_shared( rclcpp::Time(1235, 5678), fuse_core::uuid::generate("t800")); - x2->yaw() = -2.2; + x2->setYaw(-2.2); // Create an absolute constraint fuse_core::Vector1d mean; mean << 1.0; @@ -499,8 +499,8 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); // Check - EXPECT_NEAR(1.0, x1->yaw(), 1.0e-5); - EXPECT_NEAR(1.1, x2->yaw(), 1.0e-5); + EXPECT_NEAR(1.0, x1->getYaw(), 1.0e-5); + EXPECT_NEAR(1.1, x2->getYaw(), 1.0e-5); // Compute the covariance std::vector> covariance_blocks; covariance_blocks.emplace_back(x1->data(), x1->data()); diff --git a/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp b/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp index 4850a0367..6ab649a03 100644 --- a/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp @@ -110,14 +110,14 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) // Create two poses auto orientation1 = Orientation2DStamped::make_shared( rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); - orientation1->yaw() = 0.8; + orientation1->setYaw(0.8); auto position1 = Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); position1->x() = 1.5; position1->y() = -3.0; auto orientation2 = Orientation2DStamped::make_shared( rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); - orientation2->yaw() = -2.7; + orientation2->setYaw(-2.7); auto position2 = Position2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); position2->x() = 3.7; @@ -189,10 +189,10 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) // Check EXPECT_NEAR(0.0, position1->x(), 1.0e-5); EXPECT_NEAR(0.0, position1->y(), 1.0e-5); - EXPECT_NEAR(0.0, orientation1->yaw(), 1.0e-5); + EXPECT_NEAR(0.0, orientation1->getYaw(), 1.0e-5); EXPECT_NEAR(1.0, position2->x(), 1.0e-5); EXPECT_NEAR(0.0, position2->y(), 1.0e-5); - EXPECT_NEAR(0.0, orientation2->yaw(), 1.0e-5); + EXPECT_NEAR(0.0, orientation2->getYaw(), 1.0e-5); // Compute the marginal covariance for pose1 { std::vector> covariance_blocks; @@ -269,7 +269,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) // Create two poses auto orientation1 = Orientation2DStamped::make_shared( rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); - orientation1->yaw() = 0.8; + orientation1->setYaw(0.8); auto position1 = Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); position1->x() = 1.5; @@ -277,7 +277,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) auto orientation2 = Orientation2DStamped::make_shared( rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); - orientation2->yaw() = -2.7; + orientation2->setYaw(-2.7); auto position2 = Position2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); position2->x() = 3.7; @@ -382,10 +382,10 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) // Check EXPECT_NEAR(0.0, position1->x(), 1.0e-5); EXPECT_NEAR(0.0, position1->y(), 1.0e-5); - EXPECT_NEAR(0.0, orientation1->yaw(), 1.0e-5); + EXPECT_NEAR(0.0, orientation1->getYaw(), 1.0e-5); EXPECT_NEAR(1.0, position2->x(), 1.0e-5); EXPECT_NEAR(0.0, position2->y(), 1.0e-5); - EXPECT_NEAR(0.0, orientation2->yaw(), 1.0e-5); + EXPECT_NEAR(0.0, orientation2->getYaw(), 1.0e-5); // Compute the marginal covariance for pose1 { diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 40bb4bc14..12297654a 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -326,7 +326,7 @@ inline bool processAbsolutePoseWithCovariance( fuse_variables::Orientation2DStamped::make_shared(pose.header.stamp, device_id); position->x() = absolute_pose_2d.x(); position->y() = absolute_pose_2d.y(); - orientation->yaw() = absolute_pose_2d.yaw(); + orientation->setYaw(absolute_pose_2d.yaw()); // Create the pose for the constraint fuse_core::Vector3d pose_mean; @@ -448,7 +448,7 @@ inline bool processDifferentialPoseWithCovariance( fuse_variables::Orientation2DStamped::make_shared(pose1.header.stamp, device_id); position1->x() = pose1_2d.x(); position1->y() = pose1_2d.y(); - orientation1->yaw() = pose1_2d.yaw(); + orientation1->setYaw(pose1_2d.yaw()); auto position2 = fuse_variables::Position2DStamped::make_shared(pose2.header.stamp, device_id); auto orientation2 = fuse_variables::Orientation2DStamped::make_shared( @@ -456,7 +456,7 @@ inline bool processDifferentialPoseWithCovariance( device_id); position2->x() = pose2_2d.x(); position2->y() = pose2_2d.y(); - orientation2->yaw() = pose2_2d.yaw(); + orientation2->setYaw(pose2_2d.yaw()); // Create the delta for the constraint const double sy = ::sin(-pose1_2d.yaw()); @@ -813,7 +813,7 @@ inline bool processDifferentialPoseWithTwistCovariance( fuse_variables::Orientation2DStamped::make_shared(pose1.header.stamp, device_id); position1->x() = pose1_2d.x(); position1->y() = pose1_2d.y(); - orientation1->yaw() = pose1_2d.yaw(); + orientation1->setYaw(pose1_2d.yaw()); auto position2 = fuse_variables::Position2DStamped::make_shared(pose2.header.stamp, device_id); auto orientation2 = fuse_variables::Orientation2DStamped::make_shared( @@ -821,7 +821,7 @@ inline bool processDifferentialPoseWithTwistCovariance( device_id); position2->x() = pose2_2d.x(); position2->y() = pose2_2d.y(); - orientation2->yaw() = pose2_2d.yaw(); + orientation2->setYaw(pose2_2d.yaw()); // Create the delta for the constraint const auto delta = pose1_2d.inverseTimes(pose2_2d); diff --git a/fuse_models/src/odometry_2d_publisher.cpp b/fuse_models/src/odometry_2d_publisher.cpp index a4f108e4a..612af6723 100644 --- a/fuse_models/src/odometry_2d_publisher.cpp +++ b/fuse_models/src/odometry_2d_publisher.cpp @@ -322,7 +322,7 @@ bool Odometry2DPublisher::getState( odometry.pose.pose.position.x = position_variable.x(); odometry.pose.pose.position.y = position_variable.y(); odometry.pose.pose.position.z = 0.0; - odometry.pose.pose.orientation = tf2::toMsg(tf2_2d::Rotation(orientation_variable.yaw())); + odometry.pose.pose.orientation = tf2::toMsg(tf2_2d::Rotation(orientation_variable.getYaw())); odometry.twist.twist.linear.x = velocity_linear_variable.x(); odometry.twist.twist.linear.y = velocity_linear_variable.y(); odometry.twist.twist.linear.z = 0.0; diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index 44e439c57..5aad14cd4 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -315,11 +315,12 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS position->x() = pose.pose.pose.position.x; position->y() = pose.pose.pose.position.y; auto orientation = fuse_variables::Orientation2DStamped::make_shared(stamp, device_id_); - orientation->yaw() = fuse_core::getYaw( - pose.pose.pose.orientation.w, - pose.pose.pose.orientation.x, - pose.pose.pose.orientation.y, - pose.pose.pose.orientation.z); + orientation->setYaw( + fuse_core::getYaw( + pose.pose.pose.orientation.w, + pose.pose.pose.orientation.x, + pose.pose.pose.orientation.y, + pose.pose.pose.orientation.z)); auto linear_velocity = fuse_variables::VelocityLinear2DStamped::make_shared(stamp, device_id_); linear_velocity->x() = params_.initial_state[3]; linear_velocity->y() = params_.initial_state[4]; @@ -358,7 +359,7 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared( name(), *orientation, - fuse_core::Vector1d(orientation->yaw()), + fuse_core::Vector1d(orientation->getYaw()), orientation_cov); auto linear_velocity_constraint = fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint::make_shared( @@ -401,7 +402,7 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS logger_, "Received a set_pose request (stamp: " << rclcpp::Time(stamp).nanoseconds() << ", x: " << position->x() << ", y: " - << position->y() << ", yaw: " << orientation->yaw() << + << position->y() << ", yaw: " << orientation->getYaw() << ")"); } diff --git a/fuse_models/test/test_unicycle_2d.cpp b/fuse_models/test/test_unicycle_2d.cpp index a132a82f3..ac01bc37f 100644 --- a/fuse_models/test/test_unicycle_2d.cpp +++ b/fuse_models/test/test_unicycle_2d.cpp @@ -37,7 +37,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(1, 0)); position1->x() = 1.1; position1->y() = 2.1; - yaw1->yaw() = 3.1; + yaw1->setYaw(3.1); linear_velocity1->x() = 1.0; linear_velocity1->y() = 0.0; yaw_velocity1->yaw() = 0.0; @@ -51,7 +51,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(2, 0)); position2->x() = 1.2; position2->y() = 2.2; - yaw2->yaw() = M_PI / 2.0; + yaw2->setYaw(M_PI / 2.0); linear_velocity2->x() = 0.0; linear_velocity2->y() = 1.0; yaw_velocity2->yaw() = 0.0; @@ -65,7 +65,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(3, 0)); position3->x() = 1.3; position3->y() = 2.3; - yaw3->yaw() = 3.3; + yaw3->setYaw(3.3); linear_velocity3->x() = 4.3; linear_velocity3->y() = 5.3; yaw_velocity3->yaw() = 6.3; @@ -79,7 +79,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(4, 0)); position4->x() = 1.4; position4->y() = 2.4; - yaw4->yaw() = 3.4; + yaw4->setYaw(3.4); linear_velocity4->x() = 4.4; linear_velocity4->y() = 5.4; yaw_velocity4->yaw() = 6.4; @@ -93,7 +93,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(5, 0)); position5->x() = 1.5; position5->y() = 2.5; - yaw5->yaw() = 3.5; + yaw5->setYaw(3.5); linear_velocity5->x() = 4.5; linear_velocity5->y() = 5.5; yaw_velocity5->yaw() = 6.5; diff --git a/fuse_publishers/src/path_2d_publisher.cpp b/fuse_publishers/src/path_2d_publisher.cpp index 72b0ce389..9db3c5ea3 100644 --- a/fuse_publishers/src/path_2d_publisher.cpp +++ b/fuse_publishers/src/path_2d_publisher.cpp @@ -132,7 +132,7 @@ void Path2DPublisher::notifyCallback( pose.pose.position.y = position->y(); pose.pose.position.z = 0.0; pose.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation->yaw())); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation->getYaw())); poses.push_back(std::move(pose)); } } diff --git a/fuse_publishers/src/pose_2d_publisher.cpp b/fuse_publishers/src/pose_2d_publisher.cpp index fe5c88dcc..5c4c10a91 100644 --- a/fuse_publishers/src/pose_2d_publisher.cpp +++ b/fuse_publishers/src/pose_2d_publisher.cpp @@ -83,7 +83,7 @@ bool findPose( pose.position.y = position_variable.y(); pose.position.z = 0.0; pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation_variable.yaw())); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation_variable.getYaw())); } catch (const std::exception & e) { RCLCPP_WARN_STREAM_THROTTLE( logger, clock, 10.0 * 1000, diff --git a/fuse_publishers/test/test_path_2d_publisher.cpp b/fuse_publishers/test/test_path_2d_publisher.cpp index 1c2026d3c..619e14f2b 100644 --- a/fuse_publishers/test/test_path_2d_publisher.cpp +++ b/fuse_publishers/test/test_path_2d_publisher.cpp @@ -76,28 +76,28 @@ class Path2DPublisherTestFixture : public ::testing::Test position1->y() = 2.01; auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); - orientation1->yaw() = 3.01; + orientation1->setYaw(3.01); auto position2 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); position2->x() = 1.02; position2->y() = 2.02; auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); - orientation2->yaw() = 3.02; + orientation2->setYaw(3.02); auto position3 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); position3->x() = 1.03; position3->y() = 2.03; auto orientation3 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); - orientation3->yaw() = 3.03; + orientation3->setYaw(3.03); auto position4 = fuse_variables::Position2DStamped::make_shared( rclcpp::Time(1235, 11, RCL_ROS_TIME), fuse_core::uuid::generate("kitt")); position4->x() = 1.04; position4->y() = 2.04; auto orientation4 = fuse_variables::Orientation2DStamped::make_shared( rclcpp::Time(1235, 11, RCL_ROS_TIME), fuse_core::uuid::generate("kitt")); - orientation4->yaw() = 3.04; + orientation4->setYaw(3.04); transaction_->addInvolvedStamp(position1->stamp()); transaction_->addInvolvedStamp(orientation1->stamp()); diff --git a/fuse_publishers/test/test_pose_2d_publisher.cpp b/fuse_publishers/test/test_pose_2d_publisher.cpp index 947255cff..b6d89ea25 100644 --- a/fuse_publishers/test/test_pose_2d_publisher.cpp +++ b/fuse_publishers/test/test_pose_2d_publisher.cpp @@ -79,21 +79,21 @@ class Pose2DPublisherTestFixture : public ::testing::Test position1->y() = 2.01; auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); - orientation1->yaw() = 3.01; + orientation1->setYaw(3.01); auto position2 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); position2->x() = 1.02; position2->y() = 2.02; auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); - orientation2->yaw() = 3.02; + orientation2->setYaw(3.02); auto position3 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); position3->x() = 1.03; position3->y() = 2.03; auto orientation3 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); - orientation3->yaw() = 3.03; + orientation3->setYaw(3.03); auto position4 = fuse_variables::Position2DStamped::make_shared( rclcpp::Time(1235, 11, RCL_ROS_TIME), fuse_core::uuid::generate("kitt")); @@ -102,7 +102,7 @@ class Pose2DPublisherTestFixture : public ::testing::Test auto orientation4 = fuse_variables::Orientation2DStamped::make_shared( rclcpp::Time(1235, 11, RCL_ROS_TIME), fuse_core::uuid::generate("kitt")); - orientation4->yaw() = 3.04; + orientation4->setYaw(3.04); transaction_->addInvolvedStamp(position1->stamp()); transaction_->addInvolvedStamp(orientation1->stamp()); diff --git a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp index f7bcb74f6..7d0658c33 100644 --- a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp @@ -168,13 +168,29 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped /** * @brief Read-write access to the heading angle. */ + [[deprecated( + "Yaw must be in the range [-pi, pi). Use the setYaw(value) method to ensure minimum phase.") + ]] double & yaw() {return data_[YAW];} /** * @brief Read-only access to the heading angle. */ + [[deprecated( + "Use the getYaw()/setYaw(value) methods to ensure const-correctness.") + ]] const double & yaw() const {return data_[YAW];} + /** + * @brief Read-only access to the heading angle. + */ + const double & getYaw() const {return data_[YAW];} + + /** + * @brief Write access to the heading angle. + */ + void setYaw(const double yaw) {data_[YAW] = yaw;} + /** * @brief Print a human-readable description of the variable to the provided stream. * diff --git a/fuse_variables/src/orientation_2d_stamped.cpp b/fuse_variables/src/orientation_2d_stamped.cpp index 2de7f2d8e..5e7b88799 100644 --- a/fuse_variables/src/orientation_2d_stamped.cpp +++ b/fuse_variables/src/orientation_2d_stamped.cpp @@ -61,7 +61,7 @@ void Orientation2DStamped::print(std::ostream & stream) const << " device_id: " << deviceId() << "\n" << " size: " << size() << "\n" << " data:\n" - << " - yaw: " << yaw() << "\n"; + << " - yaw: " << getYaw() << "\n"; } fuse_core::LocalParameterization * Orientation2DStamped::localParameterization() const diff --git a/fuse_variables/test/test_orientation_2d_stamped.cpp b/fuse_variables/test/test_orientation_2d_stamped.cpp index dabdc7fc1..45e8bb8f4 100644 --- a/fuse_variables/test/test_orientation_2d_stamped.cpp +++ b/fuse_variables/test/test_orientation_2d_stamped.cpp @@ -242,7 +242,7 @@ TEST(Orientation2DStamped, Optimization) // Create a Orientation2DStamped Orientation2DStamped orientation( rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); - orientation.yaw() = 1.5; + orientation.setYaw(1.5); // Create a simple a constraint ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( @@ -262,7 +262,7 @@ TEST(Orientation2DStamped, Optimization) ceres::Solve(options, &problem, &summary); // Check - EXPECT_NEAR(3.0, orientation.yaw(), 1.0e-5); + EXPECT_NEAR(3.0, orientation.getYaw(), 1.0e-5); } TEST(Orientation2DStamped, Serialization) @@ -270,7 +270,7 @@ TEST(Orientation2DStamped, Serialization) // Create a Orientation2DStamped Orientation2DStamped expected( rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); - expected.yaw() = 1.5; + expected.setYaw(1.5); // Serialize the variable into an archive std::stringstream stream; @@ -289,5 +289,5 @@ TEST(Orientation2DStamped, Serialization) // Compare EXPECT_EQ(expected.deviceId(), actual.deviceId()); EXPECT_EQ(expected.stamp(), actual.stamp()); - EXPECT_EQ(expected.yaw(), actual.yaw()); + EXPECT_EQ(expected.getYaw(), actual.getYaw()); } diff --git a/fuse_viz/include/fuse_viz/conversions.hpp b/fuse_viz/include/fuse_viz/conversions.hpp index 18428a628..4304ceb1b 100644 --- a/fuse_viz/include/fuse_viz/conversions.hpp +++ b/fuse_viz/include/fuse_viz/conversions.hpp @@ -90,7 +90,7 @@ inline tf2::Vector3 toTF(const fuse_variables::Position2DStamped & position) inline tf2::Quaternion toTF(const fuse_variables::Orientation2DStamped & orientation) { - return {tf2::Vector3{0, 0, 1}, orientation.yaw()}; + return {tf2::Vector3{0, 0, 1}, orientation.getYaw()}; } inline tf2::Transform toTF( From 4e4651a185db11f9355ee67a7a36e2e24bd77cb3 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Tue, 1 Aug 2023 20:14:39 -0700 Subject: [PATCH 2/2] Force the 2D orientation value to be is minimum phase --- .../include/fuse_variables/orientation_2d_stamped.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp index 7d0658c33..cc1695a9e 100644 --- a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp @@ -189,7 +189,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped /** * @brief Write access to the heading angle. */ - void setYaw(const double yaw) {data_[YAW] = yaw;} + void setYaw(const double yaw) {data_[YAW] = fuse_core::wrapAngle2D(yaw);} /** * @brief Print a human-readable description of the variable to the provided stream.