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/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_;