commit efc3bdea4857e4fb1316911517c623bf51491c3b
author Jim Ostrowski <yimmy13@gmail.com> Sat Mar 23 16:34:06 2024 -0700
committer Jim Ostrowski <yimmy13@gmail.com> Sat Mar 23 16:34:06 2024 -0700
tree 5b030f8446a36cf04e74a24a98a5d096ced8b908
parent 992e9dfe2a7235071b4f6260c8ca6be515061dd3

Adding visualization of the solved poses

Adding visualization of the solved poses before and after outlier rejection

Change-Id: I793d41772ba08388abcbc186af0ed2b92e032b21
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>

frc971/vision/target_mapper.cc

diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index de9e55a..7f40578 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -450,6 +450,21 @@
   cv::waitKey(0);
 }
 
+void TargetMapper::DisplaySolvedVsInitial() {
+  vis_robot_.ClearImage();
+  for (const auto &[id, solved_pose] : target_poses_) {
+    Eigen::Affine3d H_world_initial =
+        PoseUtils::Pose3dToAffine3d(ideal_target_poses_.at(id));
+    vis_robot_.DrawFrameAxes(H_world_initial, std::to_string(id),
+                             cv::Scalar(0, 0, 255));
+    Eigen::Affine3d H_world_solved = PoseUtils::Pose3dToAffine3d(solved_pose);
+    vis_robot_.DrawFrameAxes(H_world_solved, std::to_string(id) + "-est",
+                             cv::Scalar(255, 255, 255));
+  }
+  cv::imshow("Solved vs. Initial", vis_robot_.image_);
+  cv::waitKey(0);
+}
+
 // Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
 bool TargetMapper::SolveOptimizationProblem(ceres::Problem *problem) {
   CHECK_NOTNULL(problem);
@@ -474,20 +489,25 @@
                                      &target_pose_problem_1);
   CHECK(SolveOptimizationProblem(&target_pose_problem_1))
       << "The target pose solve 1 was not successful, exiting.";
+  if (FLAGS_visualize_solver) {
+    LOG(INFO) << "Displaying constraint graph before removing outliers";
+    DisplayConstraintGraph();
+    DisplaySolvedVsInitial();
+  }
 
   RemoveOutlierConstraints();
 
-  if (FLAGS_visualize_solver) {
-    LOG(INFO) << "Displaying constraint graph after removing outliers";
-    DisplayConstraintGraph();
-  }
-
   // Solve again once we've thrown out bad constraints
   ceres::Problem target_pose_problem_2;
   BuildTargetPoseOptimizationProblem(target_constraints_, &target_poses_,
                                      &target_pose_problem_2);
   CHECK(SolveOptimizationProblem(&target_pose_problem_2))
       << "The target pose solve 2 was not successful, exiting.";
+  if (FLAGS_visualize_solver) {
+    LOG(INFO) << "Displaying constraint graph before removing outliers";
+    DisplayConstraintGraph();
+    DisplaySolvedVsInitial();
+  }
 
   if (FLAGS_do_map_fitting) {
     LOG(INFO) << "Solving the overall map's best alignment to the previous map";