Add secondary solver to target mapper
We first already solved for the relative constraints between targets.
Now fit this whole map onto where it should be in the world, by solving
for the transformation between the fixed target pose and where it
actually should be on the field.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ib93bc2d5f38a1e282826461a7cd33389f8566021
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index c782992..ca36866 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -7,6 +7,7 @@
#include "ceres/ceres.h"
#include "frc971/vision/ceres/types.h"
#include "frc971/vision/target_map_generated.h"
+#include "frc971/vision/visualize_robot.h"
namespace frc971::vision {
@@ -51,18 +52,36 @@
ceres::examples::MapOfPoses target_poses() { return target_poses_; }
+ // Cost function for the secondary solver finding out where the whole map fits
+ // in the world
+ template <typename S>
+ bool operator()(const S *const translation, const S *const rotation,
+ S *residual) const;
+
private:
// Constructs the nonlinear least squares optimization problem from the
// pose graph constraints.
- void BuildOptimizationProblem(
+ void BuildTargetPoseOptimizationProblem(
const ceres::examples::VectorOfConstraints &constraints,
ceres::examples::MapOfPoses *poses, ceres::Problem *problem);
+ // Constructs the nonlinear least squares optimization problem for the solved
+ // -> actual pose solver.
+ void BuildMapFittingOptimizationProblem(ceres::Problem *problem);
+
// Returns true if the solve was successful.
bool SolveOptimizationProblem(ceres::Problem *problem);
+ ceres::examples::MapOfPoses ideal_target_poses_;
ceres::examples::MapOfPoses target_poses_;
ceres::examples::VectorOfConstraints target_constraints_;
+
+ // Transformation moving the target map we solved for to where it actually
+ // should be in the world
+ Eigen::Translation3d T_frozen_actual_;
+ Eigen::Quaterniond R_frozen_actual_;
+
+ mutable VisualizeRobot vis_robot_;
};
// Utility functions for dealing with ceres::examples::Pose3d structs