|
2 | 2 |
|
3 | 3 | #include "colmap/geometry/pose_prior.h" |
4 | 4 |
|
| 5 | +#include "colmap/geometry/gps.h" |
| 6 | +#include "colmap/util/eigen_matchers.h" |
| 7 | + |
| 8 | +#include <gmock/gmock.h> |
5 | 9 | #include <gtest/gtest.h> |
6 | 10 |
|
7 | 11 | namespace colmap { |
@@ -95,5 +99,60 @@ TEST(PosePrior, ComputeRot90FromGravity) { |
95 | 99 | EXPECT_EQ(ComputeRot90FromGravity(gravity), 3); |
96 | 100 | } |
97 | 101 |
|
| 102 | +TEST(PosePrior, ConvertPosePriorsToENU) { |
| 103 | + std::vector<PosePrior> empty_priors; |
| 104 | + ConvertPosePriorsToENU(empty_priors); |
| 105 | + EXPECT_TRUE(empty_priors.empty()); |
| 106 | + |
| 107 | + // Gravity-only and already-Cartesian priors are left unchanged. |
| 108 | + PosePrior gravity_only_prior; |
| 109 | + gravity_only_prior.gravity = Eigen::Vector3d(0, 1, 0); |
| 110 | + PosePrior cartesian_prior; |
| 111 | + cartesian_prior.position = Eigen::Vector3d(1, 2, 3); |
| 112 | + cartesian_prior.coordinate_system = PosePrior::CoordinateSystem::CARTESIAN; |
| 113 | + std::vector<PosePrior> non_gps_priors = {gravity_only_prior, cartesian_prior}; |
| 114 | + ConvertPosePriorsToENU(non_gps_priors); |
| 115 | + EXPECT_EQ(non_gps_priors[0], gravity_only_prior); |
| 116 | + EXPECT_EQ(non_gps_priors[1], cartesian_prior); |
| 117 | + |
| 118 | + // Mixed WGS84 + gravity-only (UNDEFINED, NaN position) priors convert only |
| 119 | + // the WGS84 entries and preserve gravity-only entries intact. |
| 120 | + PosePrior gps_prior1; |
| 121 | + gps_prior1.position = Eigen::Vector3d(47.3769, 8.5417, 408.0); |
| 122 | + gps_prior1.coordinate_system = PosePrior::CoordinateSystem::WGS84; |
| 123 | + gps_prior1.gravity = Eigen::Vector3d(0, 1, 0); |
| 124 | + PosePrior gps_prior2; |
| 125 | + gps_prior2.position = Eigen::Vector3d(47.3779, 8.5427, 418.0); |
| 126 | + gps_prior2.coordinate_system = PosePrior::CoordinateSystem::WGS84; |
| 127 | + |
| 128 | + std::vector<PosePrior> mixed_priors = { |
| 129 | + gravity_only_prior, gps_prior1, gravity_only_prior, gps_prior2}; |
| 130 | + ConvertPosePriorsToENU(mixed_priors); |
| 131 | + EXPECT_EQ(mixed_priors[0], gravity_only_prior); |
| 132 | + EXPECT_EQ(mixed_priors[2], gravity_only_prior); |
| 133 | + EXPECT_EQ(mixed_priors[1].coordinate_system, |
| 134 | + PosePrior::CoordinateSystem::CARTESIAN); |
| 135 | + EXPECT_EQ(mixed_priors[1].gravity, gps_prior1.gravity); |
| 136 | + EXPECT_THAT(mixed_priors[1].position, |
| 137 | + EigenMatrixNear(Eigen::Vector3d(0, 0, 0), 1e-6)); |
| 138 | + EXPECT_EQ(mixed_priors[3].coordinate_system, |
| 139 | + PosePrior::CoordinateSystem::CARTESIAN); |
| 140 | + const GPSTransform gps_transform(GPSTransform::Ellipsoid::WGS84); |
| 141 | + const Eigen::Vector3d expected_enu2 = |
| 142 | + gps_transform |
| 143 | + .EllipsoidToENU({gps_prior2.position}, |
| 144 | + gps_prior1.position.x(), |
| 145 | + gps_prior1.position.y(), |
| 146 | + gps_prior1.position.z()) |
| 147 | + .front(); |
| 148 | + EXPECT_THAT(mixed_priors[3].position, EigenMatrixNear(expected_enu2, 1e-6)); |
| 149 | + |
| 150 | + // Mixing WGS84 and CARTESIAN finite positions throws. |
| 151 | + std::vector<PosePrior> inconsistent_priors = {gps_prior1, cartesian_prior}; |
| 152 | + EXPECT_THAT([&]() { ConvertPosePriorsToENU(inconsistent_priors); }, |
| 153 | + testing::ThrowsMessage<std::invalid_argument>( |
| 154 | + testing::HasSubstr("Inconsistent coordinate systems"))); |
| 155 | +} |
| 156 | + |
98 | 157 | } // namespace |
99 | 158 | } // namespace colmap |
0 commit comments