Skip to content

Commit

Permalink
Added Inverse depth projection factor (nasa#416)
Browse files Browse the repository at this point in the history 
Added inverse depth projection factor and support functions to vision common.
  • Loading branch information
@rsoussan
rsoussan authored Feb 10, 2022
1 parent 9241e67 commit 0099694
Show file tree
Hide file tree
Showing 51 changed files with 1,532 additions and 436 deletions.
64 changes: 32 additions & 32 deletions localization/depth_odometry/test/test_depth_odometry_wrapper.cc
View file

Large diffs are not rendered by default.

55 changes: 25 additions & 30 deletions ...th_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,14 +44,13 @@ TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, RampedPoin
}
const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement);
ASSERT_TRUE(pose_with_covariance != boost::none);
EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(),
target_T_source.inverse().matrix());
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2);
const auto& correspondences = pose_with_covariance->depth_correspondences;
for (int i = 0; i < correspondences.source_image_points.size(); ++i) {
EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(),
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix());
EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(),
correspondences.target_3d_points[i].matrix());
EXPECT_MATRIX_NEAR(correspondences.source_image_points[i],
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2);
EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i],
1e-2);
}
}

Expand All @@ -76,14 +75,13 @@ TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, PointToPla
}
const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement);
ASSERT_TRUE(pose_with_covariance != boost::none);
EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(),
target_T_source.inverse().matrix());
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2);
const auto& correspondences = pose_with_covariance->depth_correspondences;
for (int i = 0; i < correspondences.source_image_points.size(); ++i) {
EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(),
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix());
EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(),
correspondences.target_3d_points[i].matrix());
EXPECT_MATRIX_NEAR(correspondences.source_image_points[i],
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2);
EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i],
1e-2);
}
}

Expand All @@ -108,14 +106,13 @@ TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, SymmetricP
}
const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement);
ASSERT_TRUE(pose_with_covariance != boost::none);
EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(),
target_T_source.inverse().matrix());
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2);
const auto& correspondences = pose_with_covariance->depth_correspondences;
for (int i = 0; i < correspondences.source_image_points.size(); ++i) {
EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(),
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix());
EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(),
correspondences.target_3d_points[i].matrix());
EXPECT_MATRIX_NEAR(correspondences.source_image_points[i],
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2);
EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i],
1e-2);
}
}

Expand All @@ -137,14 +134,13 @@ TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, UmeyamaRam
}
const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement);
ASSERT_TRUE(pose_with_covariance != boost::none);
EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(),
target_T_source.inverse().matrix());
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2);
const auto& correspondences = pose_with_covariance->depth_correspondences;
for (int i = 0; i < correspondences.source_image_points.size(); ++i) {
EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(),
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix());
EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(),
correspondences.target_3d_points[i].matrix());
EXPECT_MATRIX_NEAR(correspondences.source_image_points[i],
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2);
EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i],
1e-2);
}
}

Expand All @@ -166,14 +162,13 @@ TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, UmeyamaIni
}
const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement);
ASSERT_TRUE(pose_with_covariance != boost::none);
EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(),
target_T_source.inverse().matrix());
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-2);
const auto& correspondences = pose_with_covariance->depth_correspondences;
for (int i = 0; i < correspondences.source_image_points.size(); ++i) {
EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(),
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix());
EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(),
correspondences.target_3d_points[i].matrix());
EXPECT_MATRIX_NEAR(correspondences.source_image_points[i],
(correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)), 1e-2);
EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i],
1e-2);
}
}

Expand Down
22 changes: 8 additions & 14 deletions localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,12 +45,11 @@ TEST(PointToPlaneICPDepthOdometryTester, PointToPlaneCubicPoints) {
}
const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement);
ASSERT_TRUE(pose_with_covariance != boost::none);
EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance->pose_with_covariance.pose.matrix(),
target_T_source.inverse().matrix());
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-4);
const auto& correspondences = pose_with_covariance->depth_correspondences;
for (int i = 0; i < correspondences.source_image_points.size(); ++i) {
EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(),
correspondences.target_3d_points[i].matrix());
EXPECT_MATRIX_NEAR(target_T_source * correspondences.source_3d_points[i], correspondences.target_3d_points[i],
1e-2);
}
}

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

TEST(PointToPlaneICPDepthOdometryTester, SymmetricPointToPlaneCubicPoints) {
Expand All @@ -97,8 +95,7 @@ TEST(PointToPlaneICPDepthOdometryTester, SymmetricPointToPlaneCubicPoints) {
}
const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement);
ASSERT_TRUE(pose_with_covariance != boost::none);
EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance->pose_with_covariance.pose.matrix(),
target_T_source.inverse().matrix());
EXPECT_MATRIX_NEAR(pose_with_covariance->pose_with_covariance.pose, target_T_source.inverse(), 1e-4);
}

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

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

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

// Run all the tests that were declared with TEST()
Expand Down
58 changes: 33 additions & 25 deletions localization/graph_localizer/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@ find_package(catkin2 REQUIRED COMPONENTS
localization_common
localization_measurements
msg_conversions
vision_common
)

# Find OpenCV
Expand All @@ -59,6 +60,7 @@ catkin_package(
localization_common
localization_measurements
msg_conversions
vision_common
)

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

if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_rostest_gtest(test_rotation_factor_adder
test/test_rotation_factor_adder.test
test/test_rotation_factor_adder.cc
add_rostest_gtest(test_combined_nav_state_node_updater
test/test_combined_nav_state_node_updater.test
test/test_combined_nav_state_node_updater.cc
)
target_link_libraries(test_rotation_factor_adder
graph_localizer ${catkin_LIBRARIES}
target_link_libraries(test_combined_nav_state_node_updater
graph_localizer ${catkin_LIBRARIES}
)
add_rostest_gtest(test_rotation_factor
test/test_rotation_factor.test
test/test_rotation_factor.cc
add_rostest_gtest(test_depth_odometry_factor_adder
test/test_depth_odometry_factor_adder.test
test/test_depth_odometry_factor_adder.cc
)
target_link_libraries(test_rotation_factor
graph_localizer ${catkin_LIBRARIES}
target_link_libraries(test_depth_odometry_factor_adder
graph_localizer ${catkin_LIBRARIES}
)
add_rostest_gtest(test_inverse_depth_projection_factor
test/test_inverse_depth_projection_factor.test
test/test_inverse_depth_projection_factor.cc
)
target_link_libraries(test_inverse_depth_projection_factor
graph_localizer ${catkin_LIBRARIES}
)
add_rostest_gtest(test_point_to_line_factor
test/test_point_to_line_factor.test
Expand Down Expand Up @@ -134,28 +143,27 @@ if(CATKIN_ENABLE_TESTING)
target_link_libraries(test_point_to_handrail_endpoint_factor
graph_localizer ${catkin_LIBRARIES}
)
add_rostest_gtest(test_silu
test/test_silu.test
test/test_silu.cc
add_rostest_gtest(test_rotation_factor_adder
test/test_rotation_factor_adder.test
test/test_rotation_factor_adder.cc
)
target_link_libraries(test_silu
target_link_libraries(test_rotation_factor_adder
graph_localizer ${catkin_LIBRARIES}
)
add_rostest_gtest(test_depth_odometry_factor_adder
test/test_depth_odometry_factor_adder.test
test/test_depth_odometry_factor_adder.cc
add_rostest_gtest(test_rotation_factor
test/test_rotation_factor.test
test/test_rotation_factor.cc
)
target_link_libraries(test_depth_odometry_factor_adder
graph_localizer ${catkin_LIBRARIES}
target_link_libraries(test_rotation_factor
graph_localizer ${catkin_LIBRARIES}
)
add_rostest_gtest(test_combined_nav_state_node_updater
test/test_combined_nav_state_node_updater.test
test/test_combined_nav_state_node_updater.cc
add_rostest_gtest(test_silu
test/test_silu.test
test/test_silu.cc
)
target_link_libraries(test_combined_nav_state_node_updater
graph_localizer ${catkin_LIBRARIES}
target_link_libraries(test_silu
graph_localizer ${catkin_LIBRARIES}
)

endif()

#############
Expand Down
Loading

0 comments on commit 0099694

Please sign in to comment.