Skip to content

Commit

Permalink
Reverted formatting changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@svwilliams
svwilliams committed Mar 28, 2024
1 parent 4c9ba37 commit 2698686
Show file tree
Hide file tree
Showing 15 changed files with 701 additions and 422 deletions.
128 changes: 90 additions & 38 deletions fuse_constraints/test/test_absolute_constraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@
#include <utility>
#include <vector>


TEST(AbsoluteConstraint, Constructor)
{
// Construct a constraint for every type, just to make sure they compile.
Expand Down Expand Up @@ -79,23 +80,26 @@ TEST(AbsoluteConstraint, Constructor)
mean << 3.0;
fuse_core::Matrix1d cov;
cov << 1.0;
EXPECT_NO_THROW(fuse_constraints::AbsoluteOrientation2DStampedConstraint constraint("test", variable, mean, cov));
EXPECT_NO_THROW(
fuse_constraints::AbsoluteOrientation2DStampedConstraint constraint("test", variable, mean, cov));
}
{
fuse_variables::Position2DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("rosie"));
fuse_core::Vector2d mean;
mean << 1.0, 2.0;
fuse_core::Matrix2d cov;
cov << 1.0, 0.1, 0.1, 2.0;
EXPECT_NO_THROW(fuse_constraints::AbsolutePosition2DStampedConstraint constraint("test", variable, mean, cov));
EXPECT_NO_THROW(
fuse_constraints::AbsolutePosition2DStampedConstraint constraint("test", variable, mean, cov));
}
{
fuse_variables::Position3DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("clank"));
fuse_core::Vector3d mean;
mean << 1.0, 2.0, 3.0;
fuse_core::Matrix3d cov;
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("test", variable, mean, cov));
EXPECT_NO_THROW(
fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov));
}
{
fuse_variables::VelocityAngular2DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("gort"));
Expand Down Expand Up @@ -124,7 +128,7 @@ TEST(AbsoluteConstraint, PartialMeasurement)
mean << 3.0, 1.0;
fuse_core::Matrix2d cov;
cov << 3.0, 0.2, 0.2, 1.0;
auto indices = std::vector<size_t> { 2, 0 };
auto indices = std::vector<size_t>{2, 0};
EXPECT_NO_THROW(
fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov, indices));
}
Expand All @@ -142,7 +146,8 @@ TEST(AbsoluteConstraint, Covariance)
fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint("test", variable, mean, cov);
// Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))')
fuse_core::Matrix2d expected_sqrt_info;
expected_sqrt_info << 1.002509414234171, -0.050125470711709, 0.000000000000000, 0.707106781186547;
expected_sqrt_info << 1.002509414234171, -0.050125470711709,
0.000000000000000, 0.707106781186547;
fuse_core::Matrix2d expected_cov = cov;
// Compare
EXPECT_TRUE(expected_cov.isApprox(constraint.covariance(), 1.0e-9));
Expand All @@ -155,16 +160,16 @@ TEST(AbsoluteConstraint, Covariance)
mean << 3.0, 1.0;
fuse_core::Matrix2d cov;
cov << 3.0, 0.2, 0.2, 1.0;
auto indices = std::vector<size_t> { 2, 0 };
auto indices = std::vector<size_t>{2, 0};
fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov, indices);
// Define the expected matrices
fuse_core::Vector3d expected_mean;
expected_mean << 1.0, 0.0, 3.0;
fuse_core::Matrix3d expected_cov;
expected_cov << 1.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.2, 0.0, 3.0;
fuse_core::MatrixXd expected_sqrt_info(2, 3);
expected_sqrt_info << -0.116247638743819, 0.000000000000000, 0.581238193719096, 1.000000000000000,
0.000000000000000, 0.000000000000000;
expected_sqrt_info << -0.116247638743819, 0.000000000000000, 0.581238193719096,
1.000000000000000, 0.000000000000000, 0.000000000000000;
// Compare
EXPECT_TRUE(expected_mean.isApprox(constraint.mean(), 1.0e-9));
EXPECT_TRUE(expected_cov.isApprox(constraint.covariance(), 1.0e-9));
Expand All @@ -188,8 +193,11 @@ TEST(AbsoluteConstraint, Optimization)
mean << 1.0, 2.0;
fuse_core::Matrix2d cov;
cov << 1.0, 0.1, 0.1, 2.0;
auto constraint =
fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared("test", *variable, mean, cov);
auto constraint = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared(
"test",
*variable,
mean,
cov);
// Build the problem
ceres::Problem::Options problem_options;
problem_options.loss_function_ownership = fuse_core::Loss::Ownership;
Expand All @@ -202,10 +210,12 @@ TEST(AbsoluteConstraint, Optimization)
#else
variable->manifold());
#endif

std::vector<double*> parameter_blocks;
parameter_blocks.push_back(variable->data());
problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks);
problem.AddResidualBlock(
constraint->costFunction(),
constraint->lossFunction(),
parameter_blocks);
// Run the solver
ceres::Solver::Options options;
ceres::Solver::Summary summary;
Expand All @@ -214,7 +224,7 @@ TEST(AbsoluteConstraint, Optimization)
EXPECT_NEAR(1.0, variable->x(), 1.0e-5);
EXPECT_NEAR(2.0, variable->y(), 1.0e-5);
// Compute the covariance
std::vector<std::pair<const double*, const double*>> covariance_blocks;
std::vector<std::pair<const double*, const double*> > covariance_blocks;
covariance_blocks.emplace_back(variable->data(), variable->data());
ceres::Covariance::Options cov_options;
ceres::Covariance covariance(cov_options);
Expand All @@ -239,15 +249,26 @@ TEST(AbsoluteConstraint, Optimization)
fuse_core::Vector3d mean1;
mean1 << 1.0, 2.0, 3.0;
fuse_core::Matrix3d cov1;
cov1 << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
auto constraint1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *var, mean1, cov1);
cov1 << 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0;
auto constraint1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared(
"test",
*var,
mean1,
cov1);
fuse_core::Vector2d mean2;
mean2 << 4.0, 2.0;
fuse_core::Matrix2d cov2;
cov2 << 1.0, 0.0, 0.0, 1.0;
auto indices2 = std::vector<size_t> { 2, 0 };
auto constraint2 =
fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *var, mean2, cov2, indices2);
cov2 << 1.0, 0.0,
0.0, 1.0;
auto indices2 = std::vector<size_t>{2, 0};
auto constraint2 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared(
"test",
*var,
mean2,
cov2,
indices2);
// Build the problem
ceres::Problem::Options problem_options;
problem_options.loss_function_ownership = fuse_core::Loss::Ownership;
Expand All @@ -262,8 +283,14 @@ TEST(AbsoluteConstraint, Optimization)
#endif
std::vector<double*> parameter_blocks;
parameter_blocks.push_back(var->data());
problem.AddResidualBlock(constraint1->costFunction(), constraint1->lossFunction(), parameter_blocks);
problem.AddResidualBlock(constraint2->costFunction(), constraint2->lossFunction(), parameter_blocks);
problem.AddResidualBlock(
constraint1->costFunction(),
constraint1->lossFunction(),
parameter_blocks);
problem.AddResidualBlock(
constraint2->costFunction(),
constraint2->lossFunction(),
parameter_blocks);
// Run the solver
ceres::Solver::Options options;
ceres::Solver::Summary summary;
Expand All @@ -273,7 +300,7 @@ TEST(AbsoluteConstraint, Optimization)
EXPECT_NEAR(2.0, var->y(), 1.0e-5);
EXPECT_NEAR(3.5, var->z(), 1.0e-5);
// Compute the covariance
std::vector<std::pair<const double*, const double*>> covariance_blocks;
std::vector<std::pair<const double*, const double*> > covariance_blocks;
covariance_blocks.emplace_back(var->data(), var->data());
ceres::Covariance::Options cov_options;
ceres::Covariance covariance(cov_options);
Expand All @@ -282,7 +309,9 @@ TEST(AbsoluteConstraint, Optimization)
covariance.GetCovarianceBlock(var->data(), var->data(), covariance_vector.data());
fuse_core::Matrix3d actual_cov(covariance_vector.data());
fuse_core::Matrix3d expected_cov;
expected_cov << 0.5, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.5;
expected_cov << 0.5, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 0.5;
EXPECT_TRUE(expected_cov.isApprox(actual_cov, 1.0e-9));
}
}
Expand All @@ -299,19 +328,28 @@ TEST(AbsoluteConstraint, PartialOptimization)
fuse_core::Vector2d mean1;
mean1 << 1.0, 3.0;
fuse_core::Matrix2d cov1;
cov1 << 1.0, 0.0, 0.0, 1.0;
std::vector<size_t> indices1 = { fuse_variables::Position3DStamped::Z, fuse_variables::Position3DStamped::X };
auto constraint1 =
fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *var, mean1, cov1, indices1);
cov1 << 1.0, 0.0,
0.0, 1.0;
std::vector<size_t> indices1 = {fuse_variables::Position3DStamped::Z, fuse_variables::Position3DStamped::X};
auto constraint1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared(
"test",
*var,
mean1,
cov1,
indices1);

// Create another constraint for the second index
fuse_core::Vector1d mean2;
mean2 << 2.0;
fuse_core::Matrix1d cov2;
cov2 << 1.0;
std::vector<size_t> indices2 = { fuse_variables::Position3DStamped::Y };
auto constraint2 =
fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *var, mean2, cov2, indices2);
std::vector<size_t> indices2 = {fuse_variables::Position3DStamped::Y};
auto constraint2 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared(
"test",
*var,
mean2,
cov2,
indices2);

// Build the problem
ceres::Problem::Options problem_options;
Expand All @@ -327,8 +365,14 @@ TEST(AbsoluteConstraint, PartialOptimization)
#endif
std::vector<double*> parameter_blocks;
parameter_blocks.push_back(var->data());
problem.AddResidualBlock(constraint1->costFunction(), constraint1->lossFunction(), parameter_blocks);
problem.AddResidualBlock(constraint2->costFunction(), constraint2->lossFunction(), parameter_blocks);
problem.AddResidualBlock(
constraint1->costFunction(),
constraint1->lossFunction(),
parameter_blocks);
problem.AddResidualBlock(
constraint2->costFunction(),
constraint2->lossFunction(),
parameter_blocks);
// Run the solver
ceres::Solver::Options options;
ceres::Solver::Summary summary;
Expand All @@ -343,15 +387,20 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization)
{
// Optimize a single variable and single constraint, verify the expected value and covariance are generated.
// Create a variable
auto variable =
fuse_variables::Orientation2DStamped::make_shared(ros::Time(1234, 5678), fuse_core::uuid::generate("tiktok"));
auto variable = fuse_variables::Orientation2DStamped::make_shared(
ros::Time(1234, 5678),
fuse_core::uuid::generate("tiktok"));
variable->setYaw(0.7);
// Create an absolute constraint
fuse_core::Vector1d mean;
mean << 7.0;
fuse_core::Matrix1d cov;
cov << 0.10;
auto constraint = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared("test", *variable, mean, cov);
auto constraint = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared(
"test",
*variable,
mean,
cov);
// Build the problem
ceres::Problem::Options problem_options;
problem_options.loss_function_ownership = fuse_core::Loss::Ownership;
Expand All @@ -366,15 +415,18 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization)
#endif
std::vector<double*> parameter_blocks;
parameter_blocks.push_back(variable->data());
problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks);
problem.AddResidualBlock(
constraint->costFunction(),
constraint->lossFunction(),
parameter_blocks);
// Run the solver
ceres::Solver::Options options;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
// Check
EXPECT_NEAR(7.0 - 2 * M_PI, variable->getYaw(), 1.0e-5);
// Compute the covariance
std::vector<std::pair<const double*, const double*>> covariance_blocks;
std::vector<std::pair<const double*, const double*> > covariance_blocks;
covariance_blocks.emplace_back(variable->data(), variable->data());
ceres::Covariance::Options cov_options;
ceres::Covariance covariance(cov_options);
Expand Down Expand Up @@ -416,7 +468,7 @@ TEST(AbsoluteConstraint, Serialization)
EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation());
}

int main(int argc, char** argv)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
Expand Down
Loading

0 comments on commit 2698686

Please sign in to comment.