diff --git a/frc971/vision/ceres/pose_graph_3d_error_term.h b/frc971/vision/ceres/pose_graph_3d_error_term.h
index 1f3e8de..c2ebde7 100644
--- a/frc971/vision/ceres/pose_graph_3d_error_term.h
+++ b/frc971/vision/ceres/pose_graph_3d_error_term.h
@@ -69,16 +69,17 @@
 // where I is the information matrix which is the inverse of the covariance.
 class PoseGraph3dErrorTerm {
  public:
-  PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
-                       const Eigen::Matrix<double, 6, 6>& sqrt_information)
-      : t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}
+  PoseGraph3dErrorTerm(const Pose3d &t_ab_measured,
+                       const Eigen::Matrix<double, 6, 6> &sqrt_information,
+                       double weight)
+      : t_ab_measured_(t_ab_measured),
+        sqrt_information_(sqrt_information),
+        weight_(weight) {}
 
   template <typename T>
-  bool operator()(const T* const p_a_ptr,
-                  const T* const q_a_ptr,
-                  const T* const p_b_ptr,
-                  const T* const q_b_ptr,
-                  T* residuals_ptr) const {
+  bool operator()(const T *const p_a_ptr, const T *const q_a_ptr,
+                  const T *const p_b_ptr, const T *const q_b_ptr,
+                  T *residuals_ptr) const {
     Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_a(p_a_ptr);
     Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);
 
@@ -107,14 +108,17 @@
     // Scale the residuals by the measurement uncertainty.
     residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
 
+    // Scale residual by the weight.
+    residuals *= T(weight_);
+
     return true;
   }
 
-  static ceres::CostFunction* Create(
-      const Pose3d& t_ab_measured,
-      const Eigen::Matrix<double, 6, 6>& sqrt_information) {
+  static ceres::CostFunction *Create(
+      const Pose3d &t_ab_measured,
+      const Eigen::Matrix<double, 6, 6> &sqrt_information, double weight) {
     return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
-        new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
+        new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information, weight));
   }
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -124,6 +128,8 @@
   const Pose3d t_ab_measured_;
   // The square root of the measurement information matrix.
   const Eigen::Matrix<double, 6, 6> sqrt_information_;
+  // Scalar for the cost of the constraint
+  const double weight_;
 };
 
 }  // namespace examples
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 3ec0c55..21bd443 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -231,6 +231,7 @@
         PoseUtils::TargetPoseFromFbs(*target_pose_fbs).pose;
   }
   target_poses_ = ideal_target_poses_;
+  CountConstraints();
 }
 
 TargetMapper::TargetMapper(
@@ -241,7 +242,28 @@
       target_constraints_(target_constraints),
       T_frozen_actual_(Eigen::Vector3d::Zero()),
       R_frozen_actual_(Eigen::Quaterniond::Identity()),
-      vis_robot_(cv::Size(1280, 1000)) {}
+      vis_robot_(cv::Size(1280, 1000)) {
+  CountConstraints();
+}
+
+namespace {
+std::pair<TargetMapper::TargetId, TargetMapper::TargetId> MakeIdPair(
+    const ceres::examples::Constraint3d &constraint) {
+  auto min_id = std::min(constraint.id_begin, constraint.id_end);
+  auto max_id = std::max(constraint.id_begin, constraint.id_end);
+  return std::make_pair(min_id, max_id);
+}
+}  // namespace
+
+void TargetMapper::CountConstraints() {
+  for (const auto &constraint : target_constraints_) {
+    auto id_pair = MakeIdPair(constraint);
+    if (constraint_counts_.count(id_pair) == 0) {
+      constraint_counts_[id_pair] = 0;
+    }
+    constraint_counts_[id_pair]++;
+  }
+}
 
 std::optional<TargetMapper::TargetPose> TargetMapper::GetTargetPoseById(
     std::vector<TargetMapper::TargetPose> target_poses, TargetId target_id) {
@@ -296,10 +318,23 @@
 
     const Eigen::Matrix<double, 6, 6> sqrt_information =
         constraint.information.llt().matrixL();
+
+    auto id_pair = MakeIdPair(constraint);
+    CHECK_GT(constraint_counts_.count(id_pair), 0ul)
+        << "Should have counted constraints for " << id_pair.first << "->"
+        << id_pair.second;
+
+    // Normalize constraint cost by occurances
+    size_t constraint_count = constraint_counts_[id_pair];
+    // Scale all costs so the total cost comes out to more reasonable numbers
+    constexpr double kGlobalWeight = 1000.0;
+    double constraint_weight =
+        kGlobalWeight / static_cast<double>(constraint_count);
+
     // Ceres will take ownership of the pointer.
     ceres::CostFunction *cost_function =
-        ceres::examples::PoseGraph3dErrorTerm::Create(constraint.t_be,
-                                                      sqrt_information);
+        ceres::examples::PoseGraph3dErrorTerm::Create(
+            constraint.t_be, sqrt_information, constraint_weight);
 
     problem->AddResidualBlock(cost_function, loss_function,
                               pose_begin_iter->second.p.data(),
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_;