Changes to camera extrinsic calibration to improve accuracy
Adds Outlier Rejection
Do proper rotation (Quaternion) averaging
When camera sees two targets at once, store that, and store
a single observation to match with other cameras. This leads
to a lot more good connections
Removed dead code, and refactored a couple pieces, e.g., writing extrinsic file
Also refactored some of the utilities to use quaternion averaging from utils,
and move other utility functions to vision_utils_lib
Change-Id: I918ce9c937d80717daa6659abbb139006506d4cc
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index cae1c69..aa34006 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -9,8 +9,11 @@
#include "aos/events/simulated_event_loop.h"
#include "frc971/vision/ceres/types.h"
#include "frc971/vision/target_map_generated.h"
+#include "frc971/vision/vision_util_lib.h"
#include "frc971/vision/visualize_robot.h"
+ABSL_DECLARE_FLAG(double, outlier_std_devs);
+
namespace frc971::vision {
// Estimates positions of vision targets (ex. April Tags) using
@@ -46,6 +49,15 @@
// Prints target poses into a TargetMap flatbuffer json
std::string MapToJson(std::string_view field_name) const;
+ // Builds a TargetPoseFbs from a TargetPose
+ static flatbuffers::Offset<TargetPoseFbs> TargetPoseToFbs(
+ const TargetMapper::TargetPose &target_pose,
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ // Converts a TargetPoseFbs to a TargetPose
+ static TargetMapper::TargetPose TargetPoseFromFbs(
+ const TargetPoseFbs &target_pose_fbs);
+
static std::optional<TargetPose> GetTargetPoseById(
std::vector<TargetPose> target_poses, TargetId target_id);
@@ -134,43 +146,6 @@
Stats stats_with_outliers_;
};
-// Utility functions for dealing with ceres::examples::Pose3d structs
-class PoseUtils {
- public:
- // Embeds a 3d pose into an affine transformation
- static Eigen::Affine3d Pose3dToAffine3d(
- const ceres::examples::Pose3d &pose3d);
- // Inverse of above function
- static ceres::examples::Pose3d Affine3dToPose3d(const Eigen::Affine3d &H);
-
- // Computes pose_2 relative to pose_1. This is equivalent to (pose_1^-1 *
- // pose_2)
- static ceres::examples::Pose3d ComputeRelativePose(
- const ceres::examples::Pose3d &pose_1,
- const ceres::examples::Pose3d &pose_2);
-
- // Computes pose_2 given a pose_1 and pose_2 relative to pose_1. This is
- // equivalent to (pose_1 * pose_2_relative)
- static ceres::examples::Pose3d ComputeOffsetPose(
- const ceres::examples::Pose3d &pose_1,
- const ceres::examples::Pose3d &pose_2_relative);
-
- // Converts a rotation with roll, pitch, and yaw into a quaternion
- static Eigen::Quaterniond EulerAnglesToQuaternion(const Eigen::Vector3d &rpy);
- // Inverse of above function
- static Eigen::Vector3d QuaternionToEulerAngles(const Eigen::Quaterniond &q);
- // Converts a 3d rotation matrix into a rotation with roll, pitch, and yaw
- static Eigen::Vector3d RotationMatrixToEulerAngles(const Eigen::Matrix3d &R);
-
- // Builds a TargetPoseFbs from a TargetPose
- static flatbuffers::Offset<TargetPoseFbs> TargetPoseToFbs(
- const TargetMapper::TargetPose &target_pose,
- flatbuffers::FlatBufferBuilder *fbb);
- // Converts a TargetPoseFbs to a TargetPose
- static TargetMapper::TargetPose TargetPoseFromFbs(
- const TargetPoseFbs &target_pose_fbs);
-};
-
// Transforms robot position and target detection data into target constraints
// to be used for mapping.
class DataAdapter {