Normalize target constraint costs
Scale by the number of times we've seen each target constraint's pair of
targets. That way, we aren't biased towards solving for certain pairs of
targets.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Icf8723108f81a37b7fd8cc2666d3486670b480e3
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index 95e0519..d10d6d8 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -86,6 +86,8 @@
// Removes constraints with very large errors
void RemoveOutlierConstraints();
+ void CountConstraints();
+
// Constructs the nonlinear least squares optimization problem from the
// pose graph constraints.
void BuildTargetPoseOptimizationProblem(
@@ -104,6 +106,10 @@
ceres::examples::MapOfPoses target_poses_;
ceres::examples::VectorOfConstraints target_constraints_;
+ // Counts of each pair of target ids we observe, so we can scale cost based on
+ // the inverse of this and remove bias towards certain pairs
+ std::map<std::pair<TargetId, TargetId>, size_t> constraint_counts_;
+
// Transformation moving the target map we solved for to where it actually
// should be in the world
Eigen::Translation3d T_frozen_actual_;