Skip to content

Commit

Permalink
Update tests
Browse files Browse the repository at this point in the history 
Signed-off-by: methylDragon <methylDragon@gmail.com>
  • Loading branch information
@methylDragon
methylDragon committed Dec 9, 2022
1 parent 04e24c6 commit 2ae878a
Show file tree
Hide file tree
Showing 16 changed files with 46 additions and 140 deletions.
20 changes: 5 additions & 15 deletions fuse_constraints/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(ament_cmake_ros REQUIRED)
find_package(fuse_core REQUIRED)
find_package(fuse_graphs REQUIRED)
find_package(fuse_variables REQUIRED)
Expand All @@ -31,7 +31,7 @@ include(suitesparse-extras.cmake)
###########

# fuse_constraints library
add_library(${PROJECT_NAME} SHARED
add_library(${PROJECT_NAME}
src/absolute_constraint.cpp
src/absolute_orientation_3d_stamped_constraint.cpp
src/absolute_orientation_3d_stamped_euler_constraint.cpp
Expand Down Expand Up @@ -95,21 +95,11 @@ install(DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
)

install(
FILES fuse_plugins.xml
DESTINATION share/${PROJECT_NAME}
)

find_package(ament_cmake REQUIRED)
find_package(fuse_core REQUIRED)
find_package(fuse_graphs REQUIRED)
find_package(fuse_variables REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
pluginlib_export_plugin_description_file(fuse_core fuse_plugins.xml)

ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET)
ament_export_dependencies(
ament_cmake
ament_cmake_ros
fuse_core
fuse_graphs
fuse_variables
Expand Down
4 changes: 2 additions & 2 deletions fuse_constraints/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,10 @@
</description>

<maintainer email="swilliams@locusrobotics.com">Stephen Williams</maintainer>
<author email="swilliams@locusrobotics.com">Stephen Williams</author>
<license>BSD</license>
<author email="swilliams@locusrobotics.com">Stephen Williams</author>

<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>ament_cmake_ros</buildtool_depend>

<depend>eigen</depend>
<depend>libceres-dev</depend>
Expand Down
6 changes: 0 additions & 6 deletions fuse_constraints/test/test_absolute_constraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -451,9 +451,3 @@ TEST(AbsoluteConstraint, Serialization)
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();
}
6 changes: 0 additions & 6 deletions fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -194,9 +194,3 @@ TEST(AbsoluteOrientation3DStampedConstraint, Serialization)
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();
}
6 changes: 0 additions & 6 deletions fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -251,9 +251,3 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, Serialization)
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();
}
6 changes: 0 additions & 6 deletions fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -309,9 +309,3 @@ TEST(AbsolutePose2DStampedConstraint, Serialization)
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();
}
42 changes: 18 additions & 24 deletions fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,12 +63,12 @@ TEST(AbsolutePose3DStampedConstraint, Constructor)

// Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the required precision)
fuse_core::Matrix6d cov;
cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951,
1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894,
1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061,
1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035,
1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092,
1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198;
cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT
1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT
1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT
1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT
1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT
1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT

EXPECT_NO_THROW(
AbsolutePose3DStampedConstraint constraint("test", position_variable, orientation_variable, mean, cov));
Expand All @@ -85,12 +85,12 @@ TEST(AbsolutePose3DStampedConstraint, Covariance)

// Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the required precision)
fuse_core::Matrix6d cov;
cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951,
1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894,
1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061,
1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035,
1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092,
1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198;
cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT
1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT
1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT
1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT
1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT
1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT

AbsolutePose3DStampedConstraint constraint("test", position_variable, orientation_variable, mean, cov);

Expand Down Expand Up @@ -227,12 +227,12 @@ TEST(AbsolutePose3DStampedConstraint, Serialization)

// Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the required precision)
fuse_core::Matrix6d cov;
cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951,
1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894,
1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061,
1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035,
1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092,
1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198;
cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT
1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT
1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT
1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT
1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT
1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT

AbsolutePose3DStampedConstraint expected("test", position_variable, orientation_variable, mean, cov);

Expand All @@ -256,9 +256,3 @@ TEST(AbsolutePose3DStampedConstraint, Serialization)
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();
}
6 changes: 0 additions & 6 deletions fuse_constraints/test/test_marginal_constraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -275,9 +275,3 @@ TEST(MarginalConstraint, Serialization)
EXPECT_TRUE(static_cast<bool>(actual_derived));
}
}

int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
6 changes: 0 additions & 6 deletions fuse_constraints/test/test_marginalize_variables.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -725,9 +725,3 @@ TEST(MarginalizeVariables, MarginalizeFixedVariables)
EXPECT_NEAR(expected_x3_cov[i], actual_x3_cov[i], 1.0e-3);
}
}

int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
6 changes: 0 additions & 6 deletions fuse_constraints/test/test_normal_delta_pose_2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,9 +138,3 @@ TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor
ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function);
}
}

int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
6 changes: 0 additions & 6 deletions fuse_constraints/test/test_normal_prior_pose_2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,9 +138,3 @@ TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor
ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function);
}
}

int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
6 changes: 0 additions & 6 deletions fuse_constraints/test/test_relative_constraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -522,9 +522,3 @@ TEST(RelativeConstraint, Serialization)
EXPECT_MATRIX_EQ(expected.delta(), actual.delta());
EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation());
}

int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
6 changes: 0 additions & 6 deletions fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -467,9 +467,3 @@ TEST(RelativePose2DStampedConstraint, Serialization)
EXPECT_MATRIX_EQ(expected.delta(), actual.delta());
EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation());
}

int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
48 changes: 21 additions & 27 deletions fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,12 +67,12 @@ TEST(RelativePose3DStampedConstraint, Constructor)

// Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the required precision)
fuse_core::Matrix6d cov;
cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951,
1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894,
1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061,
1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035,
1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092,
1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198;
cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT
1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT
1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT
1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT
1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT
1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT

EXPECT_NO_THROW(
RelativePose3DStampedConstraint constraint("test", position1, orientation1, position2, orientation2, delta, cov));
Expand All @@ -90,12 +90,12 @@ TEST(RelativePose3DStampedConstraint, Covariance)

// Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the required precision)
fuse_core::Matrix6d cov;
cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951,
1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894,
1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061,
1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035,
1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092,
1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198;
cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT
1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT
1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT
1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT
1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT
1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT

RelativePose3DStampedConstraint constraint(
"test",
Expand All @@ -108,9 +108,9 @@ TEST(RelativePose3DStampedConstraint, Covariance)

// Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))')
fuse_core::Matrix6d expected_sqrt_info;
expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT
0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT
0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT
expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT
0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT
0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT
0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT
0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT
0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT
Expand Down Expand Up @@ -318,12 +318,12 @@ TEST(RelativePose3DStampedConstraint, Serialization)

// Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the required precision)
fuse_core::Matrix6d cov;
cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951,
1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894,
1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061,
1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035,
1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092,
1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198;
cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT
1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT
1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT
1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT
1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT
1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT

RelativePose3DStampedConstraint expected("test", position1, orientation1, position2, orientation2, delta, cov);

Expand All @@ -347,9 +347,3 @@ TEST(RelativePose3DStampedConstraint, Serialization)
EXPECT_MATRIX_EQ(expected.delta(), actual.delta());
EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation());
}

int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
6 changes: 0 additions & 6 deletions fuse_constraints/test/test_uuid_ordering.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,9 +131,3 @@ TEST(UuidOrdering, Exists)
EXPECT_TRUE(order.exists(uuid3));
EXPECT_FALSE(order.exists(uuid4));
}

int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
6 changes: 0 additions & 6 deletions fuse_constraints/test/test_variable_constraints.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,9 +131,3 @@ TEST(VariableConstraints, InsertOrphanVariable)

EXPECT_TRUE(actual.empty());
}

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

0 comments on commit 2ae878a

Please sign in to comment.