Adding option to solve pose (solvePnP) without RANSAC by default
Change-Id: Ibeedb1ea32fe64949f52a9daf8c269b7d128d30c
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index ed2b4b9..d6f3716 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -19,6 +19,8 @@
DEFINE_string(config, "config.json", "Path to the config file to use.");
DEFINE_bool(skip_sift, false,
"If true don't run any feature extraction. Just forward images.");
+DEFINE_bool(ransac_pose, false,
+ "If true, do pose estimate with RANSAC; else, use ITERATIVE mode.");
namespace frc971 {
namespace vision {
@@ -436,9 +438,27 @@
cv::Mat R_field_camera_vec, R_field_camera, T_field_camera;
// Compute the pose of the camera (global origin relative to camera)
- cv::solvePnPRansac(per_image_good_match.training_points_3d,
- per_image_good_match.query_points, CameraIntrinsics(),
- CameraDistCoeffs(), R_camera_field_vec, T_camera_field);
+ if (FLAGS_ransac_pose) {
+ // RANSAC computation is designed to be more robust to outliers.
+ // But, we found it bounces around a lot, even with identical points
+ cv::solvePnPRansac(per_image_good_match.training_points_3d,
+ per_image_good_match.query_points, CameraIntrinsics(),
+ CameraDistCoeffs(), R_camera_field_vec,
+ T_camera_field);
+ } else {
+ // ITERATIVE mode is potentially less robust to outliers, but we
+ // found it to be more stable
+ //
+ // TODO<Jim>: We should explore feeding an initial estimate into
+ // the pose calculations. I found this to help guide the solution
+ // to something hearby, but it also can lock it into an incorrect
+ // solution
+ cv::solvePnP(per_image_good_match.training_points_3d,
+ per_image_good_match.query_points, CameraIntrinsics(),
+ CameraDistCoeffs(), R_camera_field_vec, T_camera_field,
+ false, CV_ITERATIVE);
+ }
+
CHECK_EQ(cv::Size(1, 3), T_camera_field.size());
// Convert to float32's (from float64) to be compatible with the rest