Add statistics and filter outliers in mapping
Compute the standard deviation and error for each position and rotation
element in our constraints, so we can understand how good the resulting
solved target map will be. Then, filter based on these stats to get rid
of constraints with large errors, and resolve for a better target map.
Also, delete the constraint seeding because these get flagged as
outliers.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I364311f8a4558eaf259882488b8a7ff6d5079b09
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 45c00ce..804583c 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -46,7 +46,9 @@
DEFINE_uint64(skip_to, 1,
"Start at combined image of this number (1 is the first image)");
DEFINE_bool(solve, true, "Whether to solve for the field's target map.");
-DEFINE_string(dump_constraints_to, "/tmp/constraints.txt",
+DEFINE_string(dump_constraints_to, "/tmp/mapping_constraints.txt",
+ "Write the target constraints to this path");
+DEFINE_string(dump_stats_to, "/tmp/mapping_stats.txt",
"Write the target constraints to this path");
DECLARE_int32(frozen_target_id);
DECLARE_int32(min_target_id);
@@ -381,53 +383,15 @@
}),
target_constraints.end());
- if (!FLAGS_dump_constraints_to.empty()) {
- std::ofstream fout(FLAGS_dump_constraints_to);
- for (const auto &constraint : target_constraints) {
- fout << constraint << std::endl;
- }
- fout.flush();
- fout.close();
- }
-
- // Give seed constraints with a higher confidence to ground the solver.
- // This "distance from camera" controls the noise of the seed measurement
- constexpr double kSeedDistanceFromCamera = 1.0;
-
- constexpr double kSeedDistortionFactor = 0.0;
- const DataAdapter::TimestampedDetection frozen_detection_seed = {
- .time = aos::distributed_clock::min_time,
- .H_robot_target = PoseUtils::Pose3dToAffine3d(
- kFixedTargetMapper.GetTargetPoseById(FLAGS_frozen_target_id)
- .value()
- .pose),
- .distance_from_camera = kSeedDistanceFromCamera,
- .distortion_factor = kSeedDistortionFactor,
- .id = FLAGS_frozen_target_id};
-
- constexpr TargetMapper::TargetId kAbsMinTargetId = 1;
- constexpr TargetMapper::TargetId kAbsMaxTargetId = 8;
- for (TargetMapper::TargetId id = kAbsMinTargetId; id <= kAbsMaxTargetId;
- id++) {
- if (id == FLAGS_frozen_target_id) {
- continue;
- }
-
- const DataAdapter::TimestampedDetection detection_seed = {
- .time = aos::distributed_clock::min_time,
- .H_robot_target = PoseUtils::Pose3dToAffine3d(
- kFixedTargetMapper.GetTargetPoseById(id).value().pose),
- .distance_from_camera = kSeedDistanceFromCamera,
- .distortion_factor = kSeedDistortionFactor,
- .id = id};
- target_constraints.emplace_back(DataAdapter::ComputeTargetConstraint(
- frozen_detection_seed, detection_seed,
- DataAdapter::ComputeConfidence(frozen_detection_seed,
- detection_seed)));
- }
-
TargetMapper mapper(FLAGS_json_path, target_constraints);
mapper.Solve(FLAGS_field_name, FLAGS_output_dir);
+
+ if (!FLAGS_dump_constraints_to.empty()) {
+ mapper.DumpConstraints(FLAGS_dump_constraints_to);
+ }
+ if (!FLAGS_dump_stats_to.empty()) {
+ mapper.DumpStats(FLAGS_dump_stats_to);
+ }
}
}