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/y2024/vision/calibrate_multi_cameras.cc b/y2024/vision/calibrate_multi_cameras.cc
index 7a9f412..b0fa33b 100644
--- a/y2024/vision/calibrate_multi_cameras.cc
+++ b/y2024/vision/calibrate_multi_cameras.cc
@@ -40,6 +40,9 @@
"Throw out target poses with a higher pose error than this");
ABSL_FLAG(double, max_pose_error_ratio, 0.4,
"Throw out target poses with a higher pose error ratio than this");
+ABSL_FLAG(bool, robot, false,
+ "If true we're calibrating extrinsics for the robot, use the "
+ "correct node path for the robot.");
ABSL_FLAG(std::string, output_folder, "/tmp",
"Directory in which to store the updated calibration files");
ABSL_FLAG(std::string, target_type, "charuco_diamond",
@@ -49,12 +52,10 @@
ABSL_FLAG(
uint64_t, wait_key, 1,
"Time in ms to wait between images (0 to wait indefinitely until click)");
-ABSL_FLAG(bool, robot, false,
- "If true we're calibrating extrinsics for the robot, use the "
- "correct node path for the robot.");
ABSL_DECLARE_FLAG(int32_t, min_target_id);
ABSL_DECLARE_FLAG(int32_t, max_target_id);
+ABSL_DECLARE_FLAG(double, outlier_std_devs);
// Calibrate extrinsic relationship between cameras using two targets
// seen jointly between cameras. Uses two types of information: 1)
@@ -72,8 +73,6 @@
// from all cameras, not just first camera
namespace y2024::vision {
-using frc971::vision::DataAdapter;
-using frc971::vision::ImageCallback;
using frc971::vision::PoseUtils;
using frc971::vision::TargetMap;
using frc971::vision::TargetMapper;
@@ -83,8 +82,8 @@
static constexpr double kImagePeriodMs =
1.0 / 60.0 * 1000.0; // Image capture period in ms
+// Set up our camera naming data
std::vector<CameraNode> node_list(y2024::vision::CreateNodeList());
-
std::map<std::string, int> ordering_map(
y2024::vision::CreateOrderingMap(node_list));
@@ -111,57 +110,8 @@
std::vector<TimestampedCameraDetection> two_board_extrinsics_list;
VisualizeRobot vis_robot_;
-// TODO<jim>: Implement variance calcs
-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) {
- Eigen::Vector3d avg_translation =
- std::accumulate(translation_list.begin(), translation_list.end(),
- Eigen::Vector3d{0, 0, 0}) /
- translation_list.size();
-
- // TODO<Jim>: Use QuaternionMean from quaternion_utils.cc (but this
- // requires a fixed number of quaternions to be averaged
- Eigen::Vector4d avg_rotation =
- std::accumulate(rotation_list.begin(), rotation_list.end(),
- Eigen::Vector4d{0, 0, 0, 0}) /
- rotation_list.size();
- // Normalize, so it's a valid quaternion
- avg_rotation = avg_rotation / avg_rotation.norm();
- Eigen::Quaterniond avg_rotation_q{avg_rotation[0], avg_rotation[1],
- avg_rotation[2], avg_rotation[3]};
- Eigen::Translation3d translation(avg_translation);
- Eigen::Affine3d return_pose = translation * avg_rotation_q;
- if (translation_variance != nullptr) {
- *translation_variance = Eigen::Vector3d();
- }
- if (rotation_variance != nullptr) {
- *rotation_variance = Eigen::Vector3d();
- }
-
- return return_pose;
-}
-
-Eigen::Affine3d ComputeAveragePose(
- std::vector<Eigen::Affine3d> &pose_list,
- Eigen::Vector3d *translation_variance = nullptr,
- Eigen::Vector3d *rotation_variance = nullptr) {
- 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.w(), quat.x(), quat.y(), quat.z()));
- }
-
- return ComputeAveragePose(translation_list, rotation_list,
- translation_variance, rotation_variance);
-}
-
+// Helper function to compute average pose when supplied with list
+// of TimestampedCameraDetection's
Eigen::Affine3d ComputeAveragePose(
std::vector<TimestampedCameraDetection> &pose_list,
Eigen::Vector3d *translation_variance = nullptr,
@@ -172,18 +122,95 @@
for (TimestampedCameraDetection pose : pose_list) {
translation_list.push_back(pose.H_camera_target.translation());
Eigen::Quaterniond quat(pose.H_camera_target.rotation().matrix());
+ // NOTE: Eigen initialies as (x,y,z,w) from Vector4d's
rotation_list.push_back(
- Eigen::Vector4d(quat.w(), quat.x(), quat.y(), quat.z()));
+ Eigen::Vector4d(quat.x(), quat.y(), quat.z(), quat.w()));
}
- return ComputeAveragePose(translation_list, rotation_list,
- translation_variance, rotation_variance);
+ return frc971::vision::ComputeAveragePose(
+ translation_list, rotation_list, translation_variance, rotation_variance);
}
+// Do outlier rejection. Given a list of poses, compute the
+// mean and standard deviation, and throw out those more than
+// FLAGS_outlier_std_devs standard deviations away from the mean.
+// Repeat for the desired number of iterations or until we don't throw
+// out any more outliers
+void RemoveOutliers(std::vector<TimestampedCameraDetection> &pose_list,
+ int num_iterations = 1) {
+ for (int i = 1; i <= num_iterations; i++) {
+ Eigen::Vector3d translation_variance, rotation_variance;
+ Eigen::Affine3d avg_pose = ComputeAveragePose(
+ pose_list, &translation_variance, &rotation_variance);
+
+ size_t original_size = pose_list.size();
+ // Create a lambda to identify the outliers to be removed
+ auto remove_lambda = [translation_variance, rotation_variance,
+ avg_pose](const TimestampedCameraDetection &pose) {
+ Eigen::Affine3d H_delta = avg_pose * pose.H_camera_target.inverse();
+ // Compute the z-score for each dimension, and use the max to
+ // decide on outliers. This is similar to the Mahalanobis
+ // distance (scale by inverse of the covariance matrix), but we're
+ // treating each dimension independently
+ Eigen::Matrix3d translation_sigma = translation_variance.asDiagonal();
+ Eigen::Matrix3d rotation_sigma = rotation_variance.asDiagonal();
+ Eigen::Vector3d delta_rotation =
+ PoseUtils::RotationMatrixToEulerAngles(H_delta.rotation().matrix());
+ double max_translation_z_score = std::sqrt(
+ H_delta.translation()
+ .cwiseProduct(translation_sigma.inverse() * H_delta.translation())
+ .maxCoeff());
+ double max_rotation_z_score = std::sqrt(static_cast<double>(
+ (delta_rotation.array() *
+ (rotation_sigma.inverse() * delta_rotation).array())
+ .maxCoeff()));
+ double z_score = std::max(max_translation_z_score, max_rotation_z_score);
+ // Remove observations that vary significantly from the mean
+ if (z_score > absl::GetFlag(FLAGS_outlier_std_devs)) {
+ VLOG(1) << "Removing outlier with z_score " << z_score
+ << " relative to std dev = "
+ << absl::GetFlag(FLAGS_outlier_std_devs);
+ return true;
+ }
+ return false;
+ };
+ pose_list.erase(
+ std::remove_if(pose_list.begin(), pose_list.end(), remove_lambda),
+ pose_list.end());
+
+ VLOG(1) << "Iteration #" << i << ": removed "
+ << (original_size - pose_list.size())
+ << " outlier constraints out of " << original_size
+ << " total\nStd Dev's are: "
+ << translation_variance.array().sqrt().transpose() << "m and "
+ << rotation_variance.array().sqrt().transpose() * 180.0 / M_PI
+ << "deg";
+ if (original_size - pose_list.size() == 0) {
+ VLOG(1) << "At step " << i
+ << ", ending outlier rejection early due to convergence at "
+ << pose_list.size() << " elements.\nStd Dev's are: "
+ << translation_variance.array().sqrt().transpose() << "m and "
+ << rotation_variance.array().sqrt().transpose() * 180.0 / M_PI
+ << "deg";
+ break;
+ }
+ }
+}
+
+static std::map<std::string, aos::distributed_clock::time_point>
+ last_eofs_debug;
+int display_count = 1;
+
+// Take in list of poses from a camera observation and add to running list
+// One of two options:
+// 1) We see two boards in one view-- store this to get an estimate of
+// the offset between the two boards
+// 2) We see just one board-- save this and try to pair it with a previous
+// observation from another camera
void HandlePoses(cv::Mat rgb_image,
std::vector<TargetMapper::TargetPose> target_poses,
aos::distributed_clock::time_point distributed_eof,
std::string camera_name) {
- // This is used to transform points for visualization
+ // This (H_world_board) is used to transform points for visualization
// Assumes targets are aligned with x->right, y->up, z->out of board
Eigen::Affine3d H_world_board;
H_world_board = Eigen::Translation3d::Identity() *
@@ -229,6 +256,15 @@
// Store this observation of the transform between two boards
two_board_extrinsics_list.push_back(boardA_boardB);
+ // Also, store one of the observations, so we can potentially
+ // match against the next single-target observation that comes in
+ TimestampedCameraDetection new_observation{
+ .time = distributed_eof,
+ .H_camera_target = H_camera_boardA,
+ .camera_name = camera_name,
+ .board_id = target_poses[from_index].id};
+ last_observation = new_observation;
+
if (absl::GetFlag(FLAGS_visualize)) {
vis_robot_.DrawFrameAxes(
H_world_board,
@@ -252,8 +288,9 @@
.board_id = target_poses[0].id};
// Only take two observations if they're within half an image cycle of
- // each other (i.e., as close in time as possible) And, if two consecutive
- // observations are from the same camera, just replace with the newest one
+ // each other (i.e., as close in time as possible). And, if two
+ // consecutive observations are from the same camera, just replace with
+ // the newest one
if ((new_observation.camera_name != last_observation.camera_name) &&
(std::abs((distributed_eof - last_observation.time).count()) <
kImagePeriodMs / 2.0 * 1000000.0)) {
@@ -271,44 +308,51 @@
detection_list.push_back(new_pair);
// This bit is just for visualization and checking purposes-- use the
- // last two-board observation to figure out the current estimate
- // between the two cameras
+ // first two-board observation to figure out the current estimate
+ // between the two cameras (this could be incorrect, but it keeps it
+ // constant)
if (absl::GetFlag(FLAGS_visualize) &&
two_board_extrinsics_list.size() > 0) {
draw_vis = true;
- TimestampedCameraDetection &last_two_board_ext =
- two_board_extrinsics_list.back();
- Eigen::Affine3d &H_boardA_boardB = last_two_board_ext.H_camera_target;
- int boardA_boardB_id = last_two_board_ext.board_id;
+ TimestampedCameraDetection &first_two_board_ext =
+ two_board_extrinsics_list.front();
+ Eigen::Affine3d &H_boardA_boardB = first_two_board_ext.H_camera_target;
+ int boardA_id = first_two_board_ext.board_id;
TimestampedCameraDetection camera1_boardA = new_pair.first;
- TimestampedCameraDetection camera2_boardB = new_pair.second;
- // If camera1_boardA doesn't point to the first target, then swap
- // these two
- if (camera1_boardA.board_id != boardA_boardB_id) {
- camera1_boardA = new_pair.second;
- camera2_boardB = new_pair.first;
+ TimestampedCameraDetection camera2_boardA = new_pair.second;
+ Eigen::Affine3d H_camera1_boardA = camera1_boardA.H_camera_target;
+ Eigen::Affine3d H_camera2_boardA = camera2_boardA.H_camera_target;
+ // If camera1_boardA doesn't currently point to boardA, then fix it
+ if (camera1_boardA.board_id != boardA_id) {
+ H_camera1_boardA = H_camera1_boardA * H_boardA_boardB.inverse();
+ }
+ // If camera2_boardA doesn't currently point to boardA, then fix it
+ if (camera2_boardA.board_id != boardA_id) {
+ H_camera2_boardA = H_camera2_boardA * H_boardA_boardB.inverse();
}
VLOG(1) << "Camera " << camera1_boardA.camera_name << " seeing board "
<< camera1_boardA.board_id << " and camera "
- << camera2_boardB.camera_name << " seeing board "
- << camera2_boardB.board_id;
+ << camera2_boardA.camera_name << " seeing board "
+ << camera2_boardA.board_id;
- vis_robot_.DrawRobotOutline(
- H_world_board * camera1_boardA.H_camera_target.inverse(),
- camera1_boardA.camera_name,
- kOrinColors.at(camera1_boardA.camera_name));
- vis_robot_.DrawRobotOutline(
- H_world_board * H_boardA_boardB *
- camera2_boardB.H_camera_target.inverse(),
- camera2_boardB.camera_name,
- kOrinColors.at(camera2_boardB.camera_name));
+ // Draw the two poses of the cameras, and the locations of the
+ // boards We use "Board A" as the origin (with everything relative
+ // to H_world_board)
+ vis_robot_.DrawRobotOutline(H_world_board * H_camera1_boardA.inverse(),
+ camera1_boardA.camera_name,
+ kOrinColors.at(camera1_boardA.camera_name));
+ vis_robot_.DrawRobotOutline(H_world_board * H_camera2_boardA.inverse(),
+ camera2_boardA.camera_name,
+ kOrinColors.at(camera2_boardA.camera_name));
vis_robot_.DrawFrameAxes(
H_world_board,
- std::string("Board ") + std::to_string(last_two_board_ext.board_id),
+ std::string("Board ") +
+ std::to_string(first_two_board_ext.board_id),
cv::Scalar(0, 255, 0));
vis_robot_.DrawFrameAxes(H_world_board * H_boardA_boardB, "Board B",
cv::Scalar(255, 0, 0));
+
VLOG(1) << "Storing observation between " << new_pair.first.camera_name
<< ", target " << new_pair.first.board_id << " and "
<< new_pair.second.camera_name << ", target "
@@ -341,6 +385,10 @@
}
if (draw_vis) {
+ cv::putText(vis_robot_.image_,
+ "Display #" + std::to_string(display_count++),
+ cv::Point(600, 10), cv::FONT_HERSHEY_PLAIN, 1.0,
+ cv::Scalar(255, 255, 255));
cv::imshow("Overhead View", vis_robot_.image_);
cv::waitKey(absl::GetFlag(FLAGS_wait_key));
vis_robot_.ClearImage();
@@ -355,6 +403,14 @@
// Create empty RGB image in this case
cv::Mat rgb_image;
std::vector<TargetMapper::TargetPose> target_poses;
+ VLOG(1) << ": Diff since last image from " << camera_name << " is "
+ << (distributed_eof - last_eofs_debug.at(camera_name)).count() /
+ 1000000.0
+ << "ms";
+
+ if (last_eofs_debug.find(camera_name) == last_eofs_debug.end()) {
+ last_eofs_debug[camera_name] = distributed_eof;
+ }
for (const auto *target_pose_fbs : *map.target_poses()) {
// Skip detections with invalid ids
@@ -384,7 +440,7 @@
}
const TargetMapper::TargetPose target_pose =
- PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+ TargetMapper::TargetPoseFromFbs(*target_pose_fbs);
target_poses.emplace_back(target_pose);
@@ -393,7 +449,13 @@
VLOG(1) << camera_name << " saw target " << target_pose.id
<< " from TargetMap at timestamp " << distributed_eof
<< " with pose = " << H_camera_target.matrix();
+ LOG(INFO) << "pose info for target " << target_pose_fbs->id()
+ << ": \nconfidence: " << target_pose_fbs->confidence()
+ << ", pose_error: " << target_pose_fbs->pose_error()
+ << ", pose_error_ratio: " << target_pose_fbs->pose_error_ratio()
+ << ", dist_factor: " << target_pose_fbs->distortion_factor();
}
+ last_eofs_debug[camera_name] = distributed_eof;
HandlePoses(rgb_image, target_poses, distributed_eof, camera_name);
}
@@ -432,12 +494,59 @@
HandlePoses(rgb_image, target_poses, distributed_eof, camera_name);
}
+void WriteExtrinsicFile(Eigen::Affine3d extrinsic, CameraNode camera_node,
+ const calibration::CameraCalibration *original_cal) {
+ // Write out this extrinsic to a file
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::Offset<flatbuffers::Vector<float>> data_offset =
+ fbb.CreateVector(frc971::vision::MatrixToVector(extrinsic.matrix()));
+ calibration::TransformationMatrix::Builder matrix_builder(fbb);
+ matrix_builder.add_data(data_offset);
+ flatbuffers::Offset<calibration::TransformationMatrix>
+ extrinsic_calibration_offset = matrix_builder.Finish();
+
+ calibration::CameraCalibration::Builder calibration_builder(fbb);
+ calibration_builder.add_fixed_extrinsics(extrinsic_calibration_offset);
+ const aos::realtime_clock::time_point realtime_now =
+ aos::realtime_clock::now();
+ calibration_builder.add_calibration_timestamp(
+ realtime_now.time_since_epoch().count());
+ fbb.Finish(calibration_builder.Finish());
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ solved_extrinsics = fbb.Release();
+
+ aos::FlatbufferDetachedBuffer<frc971::vision::calibration::CameraCalibration>
+ cal_copy = aos::RecursiveCopyFlatBuffer(original_cal);
+ cal_copy.mutable_message()->clear_fixed_extrinsics();
+ cal_copy.mutable_message()->clear_calibration_timestamp();
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ merged_calibration = aos::MergeFlatBuffers(&cal_copy.message(),
+ &solved_extrinsics.message());
+
+ std::stringstream time_ss;
+ time_ss << realtime_now;
+
+ const std::string calibration_filename = frc971::vision::CalibrationFilename(
+ absl::GetFlag(FLAGS_output_folder), camera_node.node_name,
+ absl::GetFlag(FLAGS_team_number), camera_node.camera_number,
+ cal_copy.message().camera_id()->data(), time_ss.str());
+ LOG(INFO) << calibration_filename << " -> "
+ << aos::FlatbufferToJson(merged_calibration, {.multi_line = true});
+
+ aos::util::WriteStringToFileOrDie(
+ calibration_filename,
+ aos::FlatbufferToJson(merged_calibration, {.multi_line = true}));
+}
+
void ExtrinsicsMain(int argc, char *argv[]) {
vis_robot_ = VisualizeRobot(cv::Size(1000, 1000));
vis_robot_.ClearImage();
const double kFocalLength = 1000.0;
const int kImageWidth = 1000;
vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength);
+ LOG(INFO) << "COPYTHIS, count, camera_name, target_id, timestamp, mag_T, "
+ "mag_R_deg, "
+ "confidence, pose_error, pose_error_ratio, distortion_factor";
std::optional<aos::FlatbufferDetachedBuffer<aos::Configuration>> config =
(absl::GetFlag(FLAGS_config).empty()
@@ -467,7 +576,6 @@
std::vector<std::unique_ptr<aos::EventLoop>> detection_event_loops;
std::vector<frc971::vision::CharucoExtractor *> charuco_extractors;
- std::vector<frc971::vision::ImageCallback *> image_callbacks;
std::vector<Eigen::Affine3d> default_extrinsics;
for (const CameraNode &camera_node : node_list) {
@@ -514,17 +622,50 @@
});
VLOG(1) << "Created watcher for using the detection event loop for "
<< camera_node.camera_name();
+
+ // Display images, if they exist
+ std::string camera_name = camera_node.camera_name();
+ detection_event_loops.back()->MakeWatcher(
+ camera_name.c_str(),
+ [camera_name](const frc971::vision::CameraImage &image) {
+ cv::Mat image_color_mat(cv::Size(image.cols(), image.rows()), CV_8UC2,
+ (void *)image.data()->data());
+ cv::Mat bgr_image(cv::Size(image.cols(), image.rows()), CV_8UC3);
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
+ cv::resize(bgr_image, bgr_image, cv::Size(500, 500));
+ cv::imshow(camera_name.c_str(), bgr_image);
+ cv::waitKey(1);
+ });
}
reader.event_loop_factory()->Run();
- // Do quick check to see what averaged two-board pose for
- // each camera is individually
CHECK_GT(two_board_extrinsics_list.size(), 0u)
<< "Must have at least one view of both boards";
int base_target_id = two_board_extrinsics_list[0].board_id;
VLOG(1) << "Base id for two_board_extrinsics_list is " << base_target_id;
+ int pre_outlier_num = two_board_extrinsics_list.size();
+ RemoveOutliers(two_board_extrinsics_list);
+
+ LOG(INFO) << "Started with " << pre_outlier_num
+ << " observations. After OUTLIER rejection, "
+ << two_board_extrinsics_list.size() << " observations remaining";
+ Eigen::Affine3d H_boardA_boardB_avg =
+ ComputeAveragePose(two_board_extrinsics_list);
+ LOG(INFO) << "Estimate of two board pose using all nodes with "
+ << two_board_extrinsics_list.size() << " observations is:\n"
+ << H_boardA_boardB_avg.matrix() << "\nOr translation of "
+ << H_boardA_boardB_avg.translation().transpose()
+ << " (m) and rotation (r,p,y) of "
+ << PoseUtils::RotationMatrixToEulerAngles(
+ H_boardA_boardB_avg.rotation().matrix())
+ .transpose() *
+ 180.0 / M_PI
+ << " (deg)";
+
+ // Do quick check to see what averaged two-board pose for
+ // each camera is individually, and compare with overall average
for (auto camera_node : node_list) {
std::vector<TimestampedCameraDetection> pose_list;
for (auto ext : two_board_extrinsics_list) {
@@ -534,32 +675,74 @@
pose_list.push_back(ext);
}
}
- Eigen::Affine3d avg_pose_from_camera = ComputeAveragePose(pose_list);
- VLOG(1) << "Estimate from " << camera_node.camera_name() << " with "
- << pose_list.size() << " observations is:\n"
- << avg_pose_from_camera.matrix();
+ CHECK(pose_list.size() > 0)
+ << "Didn't get any two_board extrinsics for camera "
+ << camera_node.camera_name();
+ Eigen::Vector3d translation_variance, rotation_variance;
+ Eigen::Affine3d avg_pose_from_camera = ComputeAveragePose(
+ pose_list, &translation_variance, &rotation_variance);
+
+ Eigen::Vector3d translation_std_dev = translation_variance.array().sqrt();
+ LOG(INFO) << camera_node.camera_name() << " has average pose from "
+ << pose_list.size() << " views of two targets of \n"
+ << avg_pose_from_camera.matrix()
+ << "\nTranslation standard deviation is "
+ << translation_std_dev.transpose();
+ double stdev_norm = translation_std_dev.norm();
+ double threshold = 0.03; // 3 cm threshold on translation variation
+ if (stdev_norm > threshold) {
+ LOG(INFO) << "WARNING: |STD_DEV| is " << stdev_norm * 100 << " > "
+ << threshold * 100 << " cm!!!!\nStd dev vector (in m) is "
+ << translation_std_dev.transpose();
+ }
+
+ Eigen::Vector3d rotation_std_dev = rotation_variance.array().sqrt();
+ LOG(INFO) << camera_node.camera_name()
+ << " with rotational standard deviation of: "
+ << rotation_std_dev.transpose() << " (radians)";
+ double rot_stdev_norm = rotation_std_dev.norm();
+ double rot_threshold = 3 * M_PI / 180.0; // Warn if more than 3 degrees
+ if (rot_stdev_norm > rot_threshold) {
+ LOG(INFO) << "WARNING: ROTATIONAL STD DEV is "
+ << rot_stdev_norm * 180.0 / M_PI << " > "
+ << rot_threshold * 180.0 / M_PI
+ << " degrees!!!!\nStd dev vector (in deg) is "
+ << (rotation_std_dev * 180.0 / M_PI).transpose();
+ }
+ // Check if a particular camera deviates significantly from the overall
+ // average Any of these factors could indicate a problem with that camera
+ Eigen::Affine3d delta_from_overall =
+ H_boardA_boardB_avg * avg_pose_from_camera.inverse();
+ LOG(INFO) << camera_node.camera_name()
+ << " had estimate different from pooled average of\n"
+ << "|dT| = " << delta_from_overall.translation().norm()
+ << "m and |dR| = "
+ << (PoseUtils::RotationMatrixToEulerAngles(
+ delta_from_overall.rotation().matrix()) *
+ 180.0 / M_PI)
+ .norm()
+ << " deg";
}
- Eigen::Affine3d H_boardA_boardB_avg =
- ComputeAveragePose(two_board_extrinsics_list);
- // TODO: Should probably do some outlier rejection
- VLOG(1) << "Estimate of two board pose using all nodes with "
- << two_board_extrinsics_list.size() << " observations is:\n"
- << H_boardA_boardB_avg.matrix() << "\n";
// Next, compute the relative camera poses
LOG(INFO) << "Got " << detection_list.size() << " extrinsic observations";
- std::vector<Eigen::Affine3d> H_camera1_camera2_list;
+ std::vector<TimestampedCameraDetection> H_camera1_camera2_list;
std::vector<Eigen::Affine3d> updated_extrinsics;
// Use the first node's extrinsics as our base, and fix from there
updated_extrinsics.push_back(default_extrinsics[0]);
LOG(INFO) << "Default extrinsic for camera " << node_list.at(0).camera_name()
- << " is " << default_extrinsics[0].matrix();
+ << " is\n"
+ << default_extrinsics[0].matrix();
for (uint i = 0; i < node_list.size() - 1; i++) {
H_camera1_camera2_list.clear();
// Go through the list, and find successive pairs of
- // cameras We'll be calculating and writing the second
- // of the pair (the i+1'th camera)
+ // cameras. We'll be calculating and writing the second
+ // of the pair's (the i+1'th camera) extrinsics
for (auto [pose1, pose2] : detection_list) {
+ // Note that this assumes our poses are ordered by the node_list
+ CHECK(!((pose1.camera_name == node_list.at(i + 1).camera_name()) &&
+ (pose2.camera_name == node_list.at(i).camera_name())))
+ << "The camera ordering on our detections is incorrect!";
if ((pose1.camera_name == node_list.at(i).camera_name()) &&
(pose2.camera_name == node_list.at(i + 1).camera_name())) {
Eigen::Affine3d H_camera1_boardA = pose1.H_camera_target;
@@ -584,7 +767,12 @@
// Compute camera1->camera2 map
Eigen::Affine3d H_camera1_camera2 =
H_camera1_boardA * H_camera2_boardA.inverse();
- H_camera1_camera2_list.push_back(H_camera1_camera2);
+ // Set the list up as TimestampedCameraDetection's to use the outlier
+ // removal function
+ TimestampedCameraDetection camera1_camera2{
+ .H_camera_target = H_camera1_camera2,
+ .camera_name = node_list.at(i + 1).camera_name()};
+ H_camera1_camera2_list.push_back(camera1_camera2);
VLOG(1) << "Map from camera " << pose1.camera_name << " and tag "
<< pose1.board_id << " with observation: \n"
<< pose1.H_camera_target.matrix() << "\n to camera "
@@ -619,6 +807,17 @@
<< H_camera1_camera2_list.size()
<< " observations, and the average pose is:\n"
<< H_camera1_camera2_avg.matrix();
+
+ RemoveOutliers(H_camera1_camera2_list);
+
+ H_camera1_camera2_avg = ComputeAveragePose(H_camera1_camera2_list);
+ LOG(INFO) << "After outlier rejection, from "
+ << node_list.at(i).camera_name() << " to "
+ << node_list.at(i + 1).camera_name() << " found "
+ << H_camera1_camera2_list.size()
+ << " observations, and the average pose is:\n"
+ << H_camera1_camera2_avg.matrix();
+
Eigen::Affine3d H_camera1_camera2_default =
default_extrinsics[i].inverse() * default_extrinsics[i + 1];
LOG(INFO) << "Compare this to that from default values:\n"
@@ -634,6 +833,7 @@
<< Eigen::AngleAxisd(H_camera1_camera2_diff.rotation()).angle() *
180.0 / M_PI
<< " degrees)";
+
// Next extrinsic is just previous one * avg_delta_pose
Eigen::Affine3d next_extrinsic =
updated_extrinsics.back() * H_camera1_camera2_avg;
@@ -645,51 +845,8 @@
<< node_list.at(i + 1).camera_name() << " is \n"
<< next_extrinsic.matrix();
- // Write out this extrinsic to a file
- flatbuffers::FlatBufferBuilder fbb;
- flatbuffers::Offset<flatbuffers::Vector<float>> data_offset =
- fbb.CreateVector(
- frc971::vision::MatrixToVector(next_extrinsic.matrix()));
- calibration::TransformationMatrix::Builder matrix_builder(fbb);
- matrix_builder.add_data(data_offset);
- flatbuffers::Offset<calibration::TransformationMatrix>
- extrinsic_calibration_offset = matrix_builder.Finish();
-
- calibration::CameraCalibration::Builder calibration_builder(fbb);
- calibration_builder.add_fixed_extrinsics(extrinsic_calibration_offset);
- const aos::realtime_clock::time_point realtime_now =
- aos::realtime_clock::now();
- calibration_builder.add_calibration_timestamp(
- realtime_now.time_since_epoch().count());
- fbb.Finish(calibration_builder.Finish());
- aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
- solved_extrinsics = fbb.Release();
-
- aos::FlatbufferDetachedBuffer<
- frc971::vision::calibration::CameraCalibration>
- cal_copy = aos::RecursiveCopyFlatBuffer(calibration_list[i + 1]);
- cal_copy.mutable_message()->clear_fixed_extrinsics();
- cal_copy.mutable_message()->clear_calibration_timestamp();
- aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
- merged_calibration = aos::MergeFlatBuffers(
- &cal_copy.message(), &solved_extrinsics.message());
-
- std::stringstream time_ss;
- time_ss << realtime_now;
-
- CameraNode &camera_node = node_list[i + 1];
- const std::string calibration_filename =
- frc971::vision::CalibrationFilename(
- absl::GetFlag(FLAGS_output_folder), camera_node.node_name,
- absl::GetFlag(FLAGS_team_number), camera_node.camera_number,
- cal_copy.message().camera_id()->data(), time_ss.str());
- LOG(INFO) << calibration_filename << " -> "
- << aos::FlatbufferToJson(merged_calibration,
- {.multi_line = true});
-
- aos::util::WriteStringToFileOrDie(
- calibration_filename,
- aos::FlatbufferToJson(merged_calibration, {.multi_line = true}));
+ WriteExtrinsicFile(next_extrinsic, node_list[i + 1],
+ calibration_list[i + 1]);
if (absl::GetFlag(FLAGS_visualize)) {
// Draw each of the updated extrinsic camera locations
@@ -712,9 +869,8 @@
}
// Cleanup
- for (uint i = 0; i < image_callbacks.size(); i++) {
+ for (uint i = 0; i < charuco_extractors.size(); i++) {
delete charuco_extractors[i];
- delete image_callbacks[i];
}
}
} // namespace y2024::vision