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.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);