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/BUILD b/frc971/vision/BUILD
index 48a8fb1..dc40bab 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -238,6 +238,7 @@
":target_map_fbs",
"//aos/events:simulated_event_loop",
"//frc971/control_loops:control_loop",
+ "//frc971/vision:vision_util_lib",
"//frc971/vision:visualize_robot",
"//frc971/vision/ceres:pose_graph_3d_lib",
"//third_party:opencv",
@@ -437,11 +438,16 @@
hdrs = ["vision_util_lib.h"],
visibility = ["//visibility:public"],
deps = [
+ "//aos/util:math",
+ "//frc971/control_loops:quaternion_utils",
"//frc971/vision:calibration_fbs",
+ "//frc971/vision/ceres:pose_graph_3d_lib",
"//third_party:opencv",
"@com_google_absl//absl/log",
"@com_google_absl//absl/log:check",
"@com_google_absl//absl/strings:str_format",
+ "@com_google_ceres_solver//:ceres",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 36f0a29..99b67d2 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -5,6 +5,7 @@
#include "frc971/control_loops/control_loop.h"
#include "frc971/vision/ceres/pose_graph_3d_error_term.h"
#include "frc971/vision/geometry.h"
+#include "frc971/vision/vision_util_lib.h"
ABSL_FLAG(uint64_t, max_num_iterations, 100,
"Maximum number of iterations for the ceres solver");
@@ -28,89 +29,6 @@
"Whether to do a final fit of the solved map to the original map");
namespace frc971::vision {
-Eigen::Affine3d PoseUtils::Pose3dToAffine3d(
- const ceres::examples::Pose3d &pose3d) {
- Eigen::Affine3d H_world_pose =
- Eigen::Translation3d(pose3d.p(0), pose3d.p(1), pose3d.p(2)) * pose3d.q;
- return H_world_pose;
-}
-
-ceres::examples::Pose3d PoseUtils::Affine3dToPose3d(const Eigen::Affine3d &H) {
- return ceres::examples::Pose3d{.p = H.translation(),
- .q = Eigen::Quaterniond(H.rotation())};
-}
-
-ceres::examples::Pose3d PoseUtils::ComputeRelativePose(
- const ceres::examples::Pose3d &pose_1,
- const ceres::examples::Pose3d &pose_2) {
- Eigen::Affine3d H_world_1 = Pose3dToAffine3d(pose_1);
- Eigen::Affine3d H_world_2 = Pose3dToAffine3d(pose_2);
-
- // Get the location of 2 in the 1 frame
- Eigen::Affine3d H_1_2 = H_world_1.inverse() * H_world_2;
- return Affine3dToPose3d(H_1_2);
-}
-
-ceres::examples::Pose3d PoseUtils::ComputeOffsetPose(
- const ceres::examples::Pose3d &pose_1,
- const ceres::examples::Pose3d &pose_2_relative) {
- auto H_world_1 = Pose3dToAffine3d(pose_1);
- auto H_1_2 = Pose3dToAffine3d(pose_2_relative);
- auto H_world_2 = H_world_1 * H_1_2;
-
- return Affine3dToPose3d(H_world_2);
-}
-
-Eigen::Quaterniond PoseUtils::EulerAnglesToQuaternion(
- const Eigen::Vector3d &rpy) {
- Eigen::AngleAxisd roll_angle(rpy.x(), Eigen::Vector3d::UnitX());
- Eigen::AngleAxisd pitch_angle(rpy.y(), Eigen::Vector3d::UnitY());
- Eigen::AngleAxisd yaw_angle(rpy.z(), Eigen::Vector3d::UnitZ());
-
- return yaw_angle * pitch_angle * roll_angle;
-}
-
-Eigen::Vector3d PoseUtils::QuaternionToEulerAngles(
- const Eigen::Quaterniond &q) {
- return RotationMatrixToEulerAngles(q.toRotationMatrix());
-}
-
-Eigen::Vector3d PoseUtils::RotationMatrixToEulerAngles(
- const Eigen::Matrix3d &R) {
- double roll = aos::math::NormalizeAngle(std::atan2(R(2, 1), R(2, 2)));
- double pitch = aos::math::NormalizeAngle(-std::asin(R(2, 0)));
- double yaw = aos::math::NormalizeAngle(std::atan2(R(1, 0), R(0, 0)));
-
- return Eigen::Vector3d(roll, pitch, yaw);
-}
-
-flatbuffers::Offset<TargetPoseFbs> PoseUtils::TargetPoseToFbs(
- const TargetMapper::TargetPose &target_pose,
- flatbuffers::FlatBufferBuilder *fbb) {
- const auto position_offset =
- CreatePosition(*fbb, target_pose.pose.p(0), target_pose.pose.p(1),
- target_pose.pose.p(2));
- const auto orientation_offset =
- CreateQuaternion(*fbb, target_pose.pose.q.w(), target_pose.pose.q.x(),
- target_pose.pose.q.y(), target_pose.pose.q.z());
- return CreateTargetPoseFbs(*fbb, target_pose.id, position_offset,
- orientation_offset);
-}
-
-TargetMapper::TargetPose PoseUtils::TargetPoseFromFbs(
- const TargetPoseFbs &target_pose_fbs) {
- return {.id = static_cast<TargetMapper::TargetId>(target_pose_fbs.id()),
- .pose = ceres::examples::Pose3d{
- Eigen::Vector3d(target_pose_fbs.position()->x(),
- target_pose_fbs.position()->y(),
- target_pose_fbs.position()->z()),
- Eigen::Quaterniond(target_pose_fbs.orientation()->w(),
- target_pose_fbs.orientation()->x(),
- target_pose_fbs.orientation()->y(),
- target_pose_fbs.orientation()->z())
- .normalized()}};
-}
-
ceres::examples::VectorOfConstraints DataAdapter::MatchTargetDetections(
const std::vector<DataAdapter::TimestampedDetection>
×tamped_target_detections,
@@ -245,7 +163,7 @@
aos::JsonFileToFlatbuffer<TargetMap>(target_poses_path);
for (const auto *target_pose_fbs : *target_map.message().target_poses()) {
ideal_target_poses_[target_pose_fbs->id()] =
- PoseUtils::TargetPoseFromFbs(*target_pose_fbs).pose;
+ TargetPoseFromFbs(*target_pose_fbs).pose;
}
target_poses_ = ideal_target_poses_;
CountConstraints();
@@ -263,6 +181,33 @@
CountConstraints();
}
+flatbuffers::Offset<TargetPoseFbs> TargetMapper::TargetPoseToFbs(
+ const TargetMapper::TargetPose &target_pose,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const auto position_offset =
+ CreatePosition(*fbb, target_pose.pose.p(0), target_pose.pose.p(1),
+ target_pose.pose.p(2));
+ const auto orientation_offset =
+ CreateQuaternion(*fbb, target_pose.pose.q.w(), target_pose.pose.q.x(),
+ target_pose.pose.q.y(), target_pose.pose.q.z());
+ return CreateTargetPoseFbs(*fbb, target_pose.id, position_offset,
+ orientation_offset);
+}
+
+TargetMapper::TargetPose TargetMapper::TargetPoseFromFbs(
+ const TargetPoseFbs &target_pose_fbs) {
+ return {.id = static_cast<TargetMapper::TargetId>(target_pose_fbs.id()),
+ .pose = ceres::examples::Pose3d{
+ Eigen::Vector3d(target_pose_fbs.position()->x(),
+ target_pose_fbs.position()->y(),
+ target_pose_fbs.position()->z()),
+ Eigen::Quaterniond(target_pose_fbs.orientation()->w(),
+ target_pose_fbs.orientation()->x(),
+ target_pose_fbs.orientation()->y(),
+ target_pose_fbs.orientation()->z())
+ .normalized()}};
+}
+
namespace {
std::pair<TargetMapper::TargetId, TargetMapper::TargetId> MakeIdPair(
const ceres::examples::Constraint3d &constraint) {
@@ -580,7 +525,7 @@
std::vector<flatbuffers::Offset<TargetPoseFbs>> target_poses_fbs;
for (const auto &[id, pose] : target_poses_) {
target_poses_fbs.emplace_back(
- PoseUtils::TargetPoseToFbs(TargetPose{.id = id, .pose = pose}, &fbb));
+ TargetPoseToFbs(TargetPose{.id = id, .pose = pose}, &fbb));
}
const auto field_name_offset = fbb.CreateString(field_name);
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 {
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index 4e9db6f..62f969c 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -12,11 +12,13 @@
#include "aos/testing/path.h"
#include "aos/testing/random_seed.h"
#include "aos/util/math.h"
+#include "vision_util_lib.h"
ABSL_DECLARE_FLAG(int32_t, min_target_id);
ABSL_DECLARE_FLAG(int32_t, max_target_id);
namespace frc971::vision {
+using frc971::vision::PoseUtils;
namespace {
constexpr double kToleranceMeters = 0.05;
@@ -151,44 +153,6 @@
} // namespace
-TEST(PoseUtilsTest, EulerAnglesAndQuaternionConversions) {
- // Make sure that the conversions are consistent back and forth.
- // These angles shouldn't get changed to a different, equivalent roll pitch
- // yaw.
- EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, M_PI);
- EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, -M_PI);
- EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, M_PI_2);
- EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, -M_PI_2);
- EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, 0.0);
- EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, M_PI_4, 0.0);
- EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, -M_PI_4, 0.0);
- EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, -M_PI_4, M_PI_4);
- EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(M_PI_4, -M_PI_4, M_PI_4);
- EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(-M_PI_2, -M_PI_4, M_PI_4);
-
- // Now, do a sweep of roll, pitch, and yaws in the normalized
- // range.
- // - roll: (-pi/2, pi/2)
- // - pitch: (-pi/2, pi/2)
- // - yaw: [-pi, pi)
- constexpr double kThetaMaxRoll = M_PI_2 - kToleranceRadians;
- constexpr double kThetaMaxPitch = M_PI_2 - kToleranceRadians;
- constexpr double kThetaMaxYaw = M_PI;
- constexpr double kDeltaTheta = M_PI / 16;
-
- for (double roll = -kThetaMaxRoll; roll < kThetaMaxRoll;
- roll += kDeltaTheta) {
- for (double pitch = -kThetaMaxPitch; pitch < kThetaMaxPitch;
- pitch += kDeltaTheta) {
- for (double yaw = -kThetaMaxYaw; yaw < kThetaMaxYaw; yaw += kDeltaTheta) {
- SCOPED_TRACE(
- absl::StrFormat("roll: %f, pitch: %f, yaw: %f", roll, pitch, yaw));
- EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(roll, pitch, yaw);
- }
- }
- }
-}
-
TEST(DataAdapterTest, ComputeConfidence) {
// Check the confidence matrices. Don't check the actual values
// in case the constants change, just check that the confidence of contraints
@@ -500,7 +464,7 @@
ceres::examples::MapOfPoses target_poses;
for (const auto *target_pose_fbs : *target_map_fbs.message().target_poses()) {
const TargetMapper::TargetPose target_pose =
- PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+ TargetMapper::TargetPoseFromFbs(*target_pose_fbs);
actual_target_poses.emplace_back(target_pose);
target_poses[target_pose.id] = target_pose.pose;
}
diff --git a/frc971/vision/vision_util_lib.cc b/frc971/vision/vision_util_lib.cc
index 089f3ac..944fa76 100644
--- a/frc971/vision/vision_util_lib.cc
+++ b/frc971/vision/vision_util_lib.cc
@@ -1,9 +1,14 @@
#include "frc971/vision/vision_util_lib.h"
+#include <numeric>
+
#include "absl/log/check.h"
#include "absl/log/log.h"
#include "absl/strings/str_format.h"
+#include "aos/util/math.h"
+#include "frc971/control_loops/quaternion_utils.h"
+
namespace frc971::vision {
std::optional<cv::Mat> CameraExtrinsics(
@@ -75,4 +80,147 @@
return calibration_filename;
}
+Eigen::Affine3d PoseUtils::Pose3dToAffine3d(
+ const ceres::examples::Pose3d &pose3d) {
+ Eigen::Affine3d H_world_pose =
+ Eigen::Translation3d(pose3d.p(0), pose3d.p(1), pose3d.p(2)) * pose3d.q;
+ return H_world_pose;
+}
+
+ceres::examples::Pose3d PoseUtils::Affine3dToPose3d(const Eigen::Affine3d &H) {
+ return ceres::examples::Pose3d{.p = H.translation(),
+ .q = Eigen::Quaterniond(H.rotation())};
+}
+
+ceres::examples::Pose3d PoseUtils::ComputeRelativePose(
+ const ceres::examples::Pose3d &pose_1,
+ const ceres::examples::Pose3d &pose_2) {
+ Eigen::Affine3d H_world_1 = Pose3dToAffine3d(pose_1);
+ Eigen::Affine3d H_world_2 = Pose3dToAffine3d(pose_2);
+
+ // Get the location of 2 in the 1 frame
+ Eigen::Affine3d H_1_2 = H_world_1.inverse() * H_world_2;
+ return Affine3dToPose3d(H_1_2);
+}
+
+ceres::examples::Pose3d PoseUtils::ComputeOffsetPose(
+ const ceres::examples::Pose3d &pose_1,
+ const ceres::examples::Pose3d &pose_2_relative) {
+ auto H_world_1 = Pose3dToAffine3d(pose_1);
+ auto H_1_2 = Pose3dToAffine3d(pose_2_relative);
+ auto H_world_2 = H_world_1 * H_1_2;
+
+ return Affine3dToPose3d(H_world_2);
+}
+
+Eigen::Quaterniond PoseUtils::EulerAnglesToQuaternion(
+ const Eigen::Vector3d &rpy) {
+ Eigen::AngleAxisd roll_angle(rpy.x(), Eigen::Vector3d::UnitX());
+ Eigen::AngleAxisd pitch_angle(rpy.y(), Eigen::Vector3d::UnitY());
+ Eigen::AngleAxisd yaw_angle(rpy.z(), Eigen::Vector3d::UnitZ());
+
+ return yaw_angle * pitch_angle * roll_angle;
+}
+
+Eigen::Vector3d PoseUtils::QuaternionToEulerAngles(
+ const Eigen::Quaterniond &q) {
+ return RotationMatrixToEulerAngles(q.toRotationMatrix());
+}
+
+Eigen::Vector3d PoseUtils::RotationMatrixToEulerAngles(
+ const Eigen::Matrix3d &R) {
+ double roll = aos::math::NormalizeAngle(std::atan2(R(2, 1), R(2, 2)));
+ double pitch = aos::math::NormalizeAngle(-std::asin(R(2, 0)));
+ double yaw = aos::math::NormalizeAngle(std::atan2(R(1, 0), R(0, 0)));
+
+ return Eigen::Vector3d(roll, pitch, yaw);
+}
+
+// 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
+
+// NOTE: variance for rotations can get dicey, so we've cheated a
+// little by doing two things:
+// 1) Computing variance relative to the mean, so that we're doing
+// this on small angles and don't have to deal with wrapping at
+// 180/360 degrees
+// 2) Returning the variance in Euler angles, since I'm not sure of a
+// better way to represent variance for rotations. (Maybe log of
+// rotations, in the Lie algebra?)
+
+Eigen::Affine3d ComputeAveragePose(
+ std::vector<Eigen::Vector3d> &translation_list,
+ std::vector<Eigen::Vector4d> &rotation_list,
+ Eigen::Vector3d *translation_variance, Eigen::Vector3d *rotation_variance) {
+ Eigen::Vector3d avg_translation =
+ std::accumulate(translation_list.begin(), translation_list.end(),
+ Eigen::Vector3d{0, 0, 0}) /
+ translation_list.size();
+
+ Eigen::Quaterniond avg_rotation_q(
+ frc971::controls::QuaternionMean(rotation_list));
+ Eigen::Affine3d average_pose =
+ Eigen::Translation3d(avg_translation) * avg_rotation_q;
+
+ CHECK_EQ(translation_list.size(), rotation_list.size());
+ if (translation_variance != nullptr) {
+ CHECK(rotation_variance != nullptr);
+ Eigen::Vector3d translation_variance_sum(0.0, 0.0, 0.0);
+ Eigen::Vector3d rotation_variance_sum(0.0, 0.0, 0.0);
+ for (uint i = 0; i < translation_list.size(); i++) {
+ Eigen::Quaterniond rotation_q(rotation_list[i]);
+ Eigen::Affine3d pose =
+ Eigen::Translation3d(translation_list[i]) * rotation_q;
+ Eigen::Affine3d delta_pose = average_pose * pose.inverse();
+ translation_variance_sum =
+ translation_variance_sum +
+ Eigen::Vector3d(delta_pose.translation().array().square());
+ rotation_variance_sum =
+ rotation_variance_sum +
+ Eigen::Vector3d(PoseUtils::RotationMatrixToEulerAngles(
+ delta_pose.rotation().matrix())
+ .array()
+ .square());
+ }
+ // Compute the variance on the translations (in m)
+ if (translation_variance != nullptr) {
+ CHECK(translation_list.size() > 1)
+ << "Have to have at least two translations to compute variance";
+ *translation_variance =
+ translation_variance_sum / translation_list.size();
+ }
+
+ // Compute the variance on the rotations (in euler angles, radians),
+ // referenced to the mean, to remove issues with Euler angles by
+ // keeping them near zero
+ if (rotation_variance != nullptr) {
+ CHECK(rotation_list.size() > 1)
+ << "Have to have at least two rotations to compute variance";
+ *rotation_variance = rotation_variance_sum / rotation_list.size();
+ }
+ }
+ return average_pose;
+}
+
+// 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,
+ Eigen::Vector3d *rotation_variance) {
+ std::vector<Eigen::Vector3d> translation_list;
+ std::vector<Eigen::Vector4d> rotation_list;
+
+ for (Eigen::Affine3d pose : pose_list) {
+ translation_list.push_back(pose.translation());
+ Eigen::Quaterniond quat(pose.rotation().matrix());
+ rotation_list.push_back(
+ Eigen::Vector4d(quat.x(), quat.y(), quat.z(), quat.w()));
+ }
+
+ return ComputeAveragePose(translation_list, rotation_list,
+ translation_variance, rotation_variance);
+}
+
} // namespace frc971::vision
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_
diff --git a/frc971/vision/vision_util_lib_test.cc b/frc971/vision/vision_util_lib_test.cc
index ff9c0a3..bdf8258 100644
--- a/frc971/vision/vision_util_lib_test.cc
+++ b/frc971/vision/vision_util_lib_test.cc
@@ -1,7 +1,10 @@
#include "frc971/vision/vision_util_lib.h"
+#include "absl/strings/str_format.h"
#include "gtest/gtest.h"
+#include "aos/util/math.h"
+
namespace frc971::vision {
// For now, just testing extracting camera number from channel name
TEST(VisionUtilsTest, CameraNumberFromChannel) {
@@ -12,4 +15,114 @@
ASSERT_EQ(CameraNumberFromChannel("/orin1/camera1").value(), 1);
ASSERT_EQ(CameraNumberFromChannel("/orin1"), std::nullopt);
}
+
+namespace {
+constexpr double kToleranceRadians = 0.05;
+// Conversions between euler angles and quaternion result in slightly-off
+// doubles
+constexpr double kOrientationEqTolerance = 1e-10;
+} // namespace
+
+// Angles normalized by aos::math::NormalizeAngle()
+#define EXPECT_NORMALIZED_ANGLES_NEAR(theta1, theta2, tolerance) \
+ { \
+ double delta = std::abs(aos::math::DiffAngle(theta1, theta2)); \
+ /* Have to check delta - 2pi for the case that one angle is very */ \
+ /* close to -pi, and the other is very close to +pi */ \
+ EXPECT_TRUE(delta < tolerance || std::abs(aos::math::DiffAngle( \
+ delta, 2.0 * M_PI)) < tolerance); \
+ }
+
+#define EXPECT_POSE_NEAR(pose1, pose2) \
+ { \
+ for (size_t i = 0; i < 3; i++) { \
+ EXPECT_NEAR(pose1.p(i), pose2.p(i), kToleranceMeters); \
+ } \
+ auto rpy_1 = PoseUtils::QuaternionToEulerAngles(pose1.q); \
+ auto rpy_2 = PoseUtils::QuaternionToEulerAngles(pose2.q); \
+ for (size_t i = 0; i < 3; i++) { \
+ SCOPED_TRACE(absl::StrFormat("rpy_1(%d) = %f, rpy_2(%d) = %f", i, \
+ rpy_1(i), i, rpy_2(i))); \
+ EXPECT_NORMALIZED_ANGLES_NEAR(rpy_1(i), rpy_2(i), kToleranceRadians) \
+ } \
+ }
+
+#define EXPECT_POSE_EQ(pose1, pose2) \
+ EXPECT_EQ(pose1.p, pose2.p); \
+ EXPECT_EQ(pose1.q, pose2.q);
+
+#define EXPECT_QUATERNION_NEAR(q1, q2) \
+ EXPECT_NEAR(q1.x(), q2.x(), kOrientationEqTolerance) << q1 << " != " << q2; \
+ EXPECT_NEAR(q1.y(), q2.y(), kOrientationEqTolerance) << q1 << " != " << q2; \
+ EXPECT_NEAR(q1.z(), q2.z(), kOrientationEqTolerance) << q1 << " != " << q2; \
+ EXPECT_NEAR(q1.w(), q2.w(), kOrientationEqTolerance) << q1 << " != " << q2;
+
+// Expects same roll, pitch, and yaw values (not equivalent rotation)
+#define EXPECT_RPY_EQ(rpy_1, rpy_2) \
+ { \
+ for (size_t i = 0; i < 3; i++) { \
+ SCOPED_TRACE(absl::StrFormat("rpy_1(%d) = %f, rpy_2(%d) = %f", i, \
+ rpy_1(i), i, rpy_2(i))); \
+ EXPECT_NORMALIZED_ANGLES_NEAR(rpy_1(i), rpy_2(i), \
+ kOrientationEqTolerance) \
+ } \
+ }
+
+#define EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(roll, pitch, yaw) \
+ { \
+ auto rpy = Eigen::Vector3d(roll, pitch, yaw); \
+ auto converted_rpy = PoseUtils::QuaternionToEulerAngles( \
+ PoseUtils::EulerAnglesToQuaternion(rpy)); \
+ EXPECT_RPY_EQ(converted_rpy, rpy); \
+ }
+
+// Both confidence matrixes should have the same dimensions and be square
+#define EXPECT_CONFIDENCE_GT(confidence1, confidence2) \
+ { \
+ ASSERT_EQ(confidence1.rows(), confidence2.rows()); \
+ ASSERT_EQ(confidence1.rows(), confidence1.cols()); \
+ ASSERT_EQ(confidence2.rows(), confidence2.cols()); \
+ for (size_t i = 0; i < confidence1.rows(); i++) { \
+ EXPECT_GT(confidence1(i, i), confidence2(i, i)); \
+ } \
+ }
+
+TEST(PoseUtilsTest, EulerAnglesAndQuaternionConversions) {
+ // Make sure that the conversions are consistent back and forth.
+ // These angles shouldn't get changed to a different, equivalent roll pitch
+ // yaw.
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, M_PI);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, -M_PI);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, M_PI_2);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, -M_PI_2);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, 0.0);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, M_PI_4, 0.0);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, -M_PI_4, 0.0);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, -M_PI_4, M_PI_4);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(M_PI_4, -M_PI_4, M_PI_4);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(-M_PI_2, -M_PI_4, M_PI_4);
+
+ // Now, do a sweep of roll, pitch, and yaws in the normalized
+ // range.
+ // - roll: (-pi/2, pi/2)
+ // - pitch: (-pi/2, pi/2)
+ // - yaw: [-pi, pi)
+ constexpr double kThetaMaxRoll = M_PI_2 - kToleranceRadians;
+ constexpr double kThetaMaxPitch = M_PI_2 - kToleranceRadians;
+ constexpr double kThetaMaxYaw = M_PI;
+ constexpr double kDeltaTheta = M_PI / 16;
+
+ for (double roll = -kThetaMaxRoll; roll < kThetaMaxRoll;
+ roll += kDeltaTheta) {
+ for (double pitch = -kThetaMaxPitch; pitch < kThetaMaxPitch;
+ pitch += kDeltaTheta) {
+ for (double yaw = -kThetaMaxYaw; yaw < kThetaMaxYaw; yaw += kDeltaTheta) {
+ SCOPED_TRACE(
+ absl::StrFormat("roll: %f, pitch: %f, yaw: %f", roll, pitch, yaw));
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(roll, pitch, yaw);
+ }
+ }
+ }
+}
+
} // namespace frc971::vision