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/vision_util_lib.h b/frc971/vision/vision_util_lib.h
index a26af3b..ee621dd 100644
--- a/frc971/vision/vision_util_lib.h
+++ b/frc971/vision/vision_util_lib.h
@@ -3,9 +3,13 @@
#include <optional>
#include <string_view>
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+#include "Eigen/SVD"
#include "opencv2/imgproc.hpp"
#include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/ceres/types.h"
// Extract the CameraExtrinsics from a CameraCalibration struct
namespace frc971::vision {
@@ -31,6 +35,55 @@
int camera_number, std::string camera_id,
std::string timestamp);
+// 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);
+};
+
+// Compute the average of a set of quaternions
+Eigen::Vector4d QuaternionAverage(std::vector<Eigen::Vector4d> quaternions);
+
+// Compute the average pose given a list of translations (as
+// Eigen::Vector3d's) and rotations (as Eigen::Vector4d quaternions)
+// Returned as an Eigen::Affine3d pose
+// Also, compute the variance of each of these list of vectors
+Eigen::Affine3d ComputeAveragePose(
+ std::vector<Eigen::Vector3d> &translation_list,
+ std::vector<Eigen::Vector4d> &rotation_list,
+ Eigen::Vector3d *translation_variance = nullptr,
+ Eigen::Vector3d *rotation_variance = nullptr);
+
+// Helper function to compute average pose when supplied with list
+// of Eigen::Affine3d's
+Eigen::Affine3d ComputeAveragePose(
+ std::vector<Eigen::Affine3d> &pose_list,
+ Eigen::Vector3d *translation_variance = nullptr,
+ Eigen::Vector3d *rotation_variance = nullptr);
+
} // namespace frc971::vision
#endif // FRC971_VISION_VISION_UTIL_LIB_H_