Skip to content

Commit 0099694

Browse files
rsoussanrsoussan
authored
Added Inverse depth projection factor (#416)
Added inverse depth projection factor and support functions to vision common.
1 parent 9241e67 commit 0099694

File tree

51 files changed

+1532
-436
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

51 files changed

+1532
-436
lines changed

‎localization/depth_odometry/test/test_depth_odometry_wrapper.cc

Lines changed: 32 additions & 32 deletions
Large diffs are not rendered by default.

‎localization/depth_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc

Lines changed: 25 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -44,14 +44,13 @@ TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, RampedPoin
4444
}
4545
const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement);
4646
ASSERT_TRUE(pose_with_covariance != boost::none);
47-
EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(),
48-
target_T_source.inverse().matrix());
47+
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2);
4948
const auto& correspondences = pose_with_covariance->depth_correspondences;
5049
for (int i = 0; i < correspondences.source_image_points.size(); ++i) {
51-
EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(),
52-
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix());
53-
EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(),
54-
correspondences.target_3d_points[i].matrix());
50+
EXPECT_MATRIX_NEAR(correspondences.source_image_points[i],
51+
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2);
52+
EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i],
53+
1e-2);
5554
}
5655
}
5756

@@ -76,14 +75,13 @@ TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, PointToPla
7675
}
7776
const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement);
7877
ASSERT_TRUE(pose_with_covariance != boost::none);
79-
EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(),
80-
target_T_source.inverse().matrix());
78+
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2);
8179
const auto& correspondences = pose_with_covariance->depth_correspondences;
8280
for (int i = 0; i < correspondences.source_image_points.size(); ++i) {
83-
EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(),
84-
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix());
85-
EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(),
86-
correspondences.target_3d_points[i].matrix());
81+
EXPECT_MATRIX_NEAR(correspondences.source_image_points[i],
82+
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2);
83+
EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i],
84+
1e-2);
8785
}
8886
}
8987

@@ -108,14 +106,13 @@ TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, SymmetricP
108106
}
109107
const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement);
110108
ASSERT_TRUE(pose_with_covariance != boost::none);
111-
EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(),
112-
target_T_source.inverse().matrix());
109+
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2);
113110
const auto& correspondences = pose_with_covariance->depth_correspondences;
114111
for (int i = 0; i < correspondences.source_image_points.size(); ++i) {
115-
EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(),
116-
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix());
117-
EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(),
118-
correspondences.target_3d_points[i].matrix());
112+
EXPECT_MATRIX_NEAR(correspondences.source_image_points[i],
113+
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2);
114+
EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i],
115+
1e-2);
119116
}
120117
}
121118

@@ -137,14 +134,13 @@ TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, UmeyamaRam
137134
}
138135
const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement);
139136
ASSERT_TRUE(pose_with_covariance != boost::none);
140-
EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(),
141-
target_T_source.inverse().matrix());
137+
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2);
142138
const auto& correspondences = pose_with_covariance->depth_correspondences;
143139
for (int i = 0; i < correspondences.source_image_points.size(); ++i) {
144-
EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(),
145-
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix());
146-
EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(),
147-
correspondences.target_3d_points[i].matrix());
140+
EXPECT_MATRIX_NEAR(correspondences.source_image_points[i],
141+
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2);
142+
EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i],
143+
1e-2);
148144
}
149145
}
150146

@@ -166,14 +162,13 @@ TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, UmeyamaIni
166162
}
167163
const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement);
168164
ASSERT_TRUE(pose_with_covariance != boost::none);
169-
EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(),
170-
target_T_source.inverse().matrix());
165+
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2);
171166
const auto& correspondences = pose_with_covariance->depth_correspondences;
172167
for (int i = 0; i < correspondences.source_image_points.size(); ++i) {
173-
EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(),
174-
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix());
175-
EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(),
176-
correspondences.target_3d_points[i].matrix());
168+
EXPECT_MATRIX_NEAR(correspondences.source_image_points[i],
169+
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2);
170+
EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i],
171+
1e-2);
177172
}
178173
}
179174

‎localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.cc

Lines changed: 8 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -45,12 +45,11 @@ TEST(PointToPlaneICPDepthOdometryTester, PointToPlaneCubicPoints) {
4545
}
4646
const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement);
4747
ASSERT_TRUE(pose_with_covariance != boost::none);
48-
EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance->pose_with_covariance.pose.matrix(),
49-
target_T_source.inverse().matrix());
48+
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-4);
5049
const auto& correspondences = pose_with_covariance->depth_correspondences;
5150
for (int i = 0; i < correspondences.source_image_points.size(); ++i) {
52-
EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(),
53-
correspondences.target_3d_points[i].matrix());
51+
EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i],
52+
1e-2);
5453
}
5554
}
5655

@@ -74,8 +73,7 @@ TEST(PointToPlaneICPDepthOdometryTester, PointToPlaneDownsampledCubicPoints) {
7473
}
7574
const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement);
7675
ASSERT_TRUE(pose_with_covariance != boost::none);
77-
EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance->pose_with_covariance.pose.matrix(),
78-
target_T_source.inverse().matrix());
76+
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-4);
7977
}
8078

8179
TEST(PointToPlaneICPDepthOdometryTester, SymmetricPointToPlaneCubicPoints) {
@@ -97,8 +95,7 @@ TEST(PointToPlaneICPDepthOdometryTester, SymmetricPointToPlaneCubicPoints) {
9795
}
9896
const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement);
9997
ASSERT_TRUE(pose_with_covariance != boost::none);
100-
EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance->pose_with_covariance.pose.matrix(),
101-
target_T_source.inverse().matrix());
98+
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-4);
10299
}
103100

104101
TEST(PointToPlaneICPDepthOdometryTester, CorrespondenceRejectorPointToPlaneCubicPoints) {
@@ -120,8 +117,7 @@ TEST(PointToPlaneICPDepthOdometryTester, CorrespondenceRejectorPointToPlaneCubic
120117
}
121118
const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement);
122119
ASSERT_TRUE(pose_with_covariance != boost::none);
123-
EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance->pose_with_covariance.pose.matrix(),
124-
target_T_source.inverse().matrix());
120+
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-4);
125121
}
126122

127123
TEST(PointToPlaneICPDepthOdometryTester, PointToPlane3MeasurementsCubicPoints) {
@@ -142,8 +138,7 @@ TEST(PointToPlaneICPDepthOdometryTester, PointToPlane3MeasurementsCubicPoints) {
142138
}
143139
const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement);
144140
ASSERT_TRUE(pose_with_covariance != boost::none);
145-
EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance->pose_with_covariance.pose.matrix(),
146-
target_T_source.inverse().matrix());
141+
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-4);
147142

148143
// Add third measurement
149144
const auto target2_T_target =
@@ -152,8 +147,7 @@ TEST(PointToPlaneICPDepthOdometryTester, PointToPlane3MeasurementsCubicPoints) {
152147
dd::TransformDepthImageMeasurement(target_depth_image_measurement, 0.2, target2_T_target);
153148
const auto pose_with_covariance2 = icp_depth_odometry.DepthImageCallback(target2_depth_image_measurement);
154149
ASSERT_TRUE(pose_with_covariance2 != boost::none);
155-
EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance2->pose_with_covariance.pose.matrix(),
156-
target2_T_target.inverse().matrix());
150+
EXPECT_MATRIX_NEAR(pose_with_covariance2->pose_with_covariance.pose, target2_T_target.inverse(), 1e-4);
157151
}
158152

159153
// Run all the tests that were declared with TEST()

‎localization/graph_localizer/CMakeLists.txt

Lines changed: 33 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@ find_package(catkin2 REQUIRED COMPONENTS
3939
localization_common
4040
localization_measurements
4141
msg_conversions
42+
vision_common
4243
)
4344

4445
# Find OpenCV
@@ -59,6 +60,7 @@ catkin_package(
5960
localization_common
6061
localization_measurements
6162
msg_conversions
63+
vision_common
6264
)
6365

6466
###########
@@ -85,19 +87,26 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
8587

8688
if(CATKIN_ENABLE_TESTING)
8789
find_package(rostest REQUIRED)
88-
add_rostest_gtest(test_rotation_factor_adder
89-
test/test_rotation_factor_adder.test
90-
test/test_rotation_factor_adder.cc
90+
add_rostest_gtest(test_combined_nav_state_node_updater
91+
test/test_combined_nav_state_node_updater.test
92+
test/test_combined_nav_state_node_updater.cc
9193
)
92-
target_link_libraries(test_rotation_factor_adder
93-
graph_localizer ${catkin_LIBRARIES}
94+
target_link_libraries(test_combined_nav_state_node_updater
95+
graph_localizer ${catkin_LIBRARIES}
9496
)
95-
add_rostest_gtest(test_rotation_factor
96-
test/test_rotation_factor.test
97-
test/test_rotation_factor.cc
97+
add_rostest_gtest(test_depth_odometry_factor_adder
98+
test/test_depth_odometry_factor_adder.test
99+
test/test_depth_odometry_factor_adder.cc
98100
)
99-
target_link_libraries(test_rotation_factor
100-
graph_localizer ${catkin_LIBRARIES}
101+
target_link_libraries(test_depth_odometry_factor_adder
102+
graph_localizer ${catkin_LIBRARIES}
103+
)
104+
add_rostest_gtest(test_inverse_depth_projection_factor
105+
test/test_inverse_depth_projection_factor.test
106+
test/test_inverse_depth_projection_factor.cc
107+
)
108+
target_link_libraries(test_inverse_depth_projection_factor
109+
graph_localizer ${catkin_LIBRARIES}
101110
)
102111
add_rostest_gtest(test_point_to_line_factor
103112
test/test_point_to_line_factor.test
@@ -134,28 +143,27 @@ if(CATKIN_ENABLE_TESTING)
134143
target_link_libraries(test_point_to_handrail_endpoint_factor
135144
graph_localizer ${catkin_LIBRARIES}
136145
)
137-
add_rostest_gtest(test_silu
138-
test/test_silu.test
139-
test/test_silu.cc
146+
add_rostest_gtest(test_rotation_factor_adder
147+
test/test_rotation_factor_adder.test
148+
test/test_rotation_factor_adder.cc
140149
)
141-
target_link_libraries(test_silu
150+
target_link_libraries(test_rotation_factor_adder
142151
graph_localizer ${catkin_LIBRARIES}
143152
)
144-
add_rostest_gtest(test_depth_odometry_factor_adder
145-
test/test_depth_odometry_factor_adder.test
146-
test/test_depth_odometry_factor_adder.cc
153+
add_rostest_gtest(test_rotation_factor
154+
test/test_rotation_factor.test
155+
test/test_rotation_factor.cc
147156
)
148-
target_link_libraries(test_depth_odometry_factor_adder
149-
graph_localizer ${catkin_LIBRARIES}
157+
target_link_libraries(test_rotation_factor
158+
graph_localizer ${catkin_LIBRARIES}
150159
)
151-
add_rostest_gtest(test_combined_nav_state_node_updater
152-
test/test_combined_nav_state_node_updater.test
153-
test/test_combined_nav_state_node_updater.cc
160+
add_rostest_gtest(test_silu
161+
test/test_silu.test
162+
test/test_silu.cc
154163
)
155-
target_link_libraries(test_combined_nav_state_node_updater
156-
graph_localizer ${catkin_LIBRARIES}
164+
target_link_libraries(test_silu
165+
graph_localizer ${catkin_LIBRARIES}
157166
)
158-
159167
endif()
160168

161169
#############

0 commit comments

Comments
 (0)