| #include <numeric> |
| |
| #include "absl/flags/flag.h" |
| |
| #include "aos/configuration.h" |
| #include "aos/events/logging/log_reader.h" |
| #include "aos/events/simulated_event_loop.h" |
| #include "aos/init.h" |
| #include "aos/util/mcap_logger.h" |
| #include "frc971/control_loops/pose.h" |
| #include "frc971/control_loops/quaternion_utils.h" |
| #include "frc971/vision/calibration_generated.h" |
| #include "frc971/vision/charuco_lib.h" |
| #include "frc971/vision/extrinsics_calibration.h" |
| #include "frc971/vision/target_mapper.h" |
| #include "frc971/vision/vision_util_lib.h" |
| #include "frc971/vision/visualize_robot.h" |
| // clang-format off |
| // OpenCV eigen files must be included after Eigen includes |
| #include "opencv2/aruco.hpp" |
| #include "opencv2/calib3d.hpp" |
| #include "opencv2/core/eigen.hpp" |
| #include "opencv2/features2d.hpp" |
| #include "opencv2/highgui.hpp" |
| #include "opencv2/highgui/highgui.hpp" |
| #include "opencv2/imgproc.hpp" |
| // clang-format on |
| #include "frc971/constants/constants_sender_lib.h" |
| #include "frc971/vision/vision_util_lib.h" |
| #include "y2024/constants/simulated_constants_sender.h" |
| #include "y2024/vision/vision_util.h" |
| |
| ABSL_FLAG(bool, alt_view, false, |
| "If true, show visualization from field level, rather than above"); |
| ABSL_FLAG(std::string, config, "", |
| "If set, override the log's config file with this one."); |
| ABSL_FLAG(std::string, constants_path, "y2024/constants/constants.json", |
| "Path to the constant file"); |
| ABSL_FLAG(double, max_pose_error, 5e-5, |
| "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", |
| "Type of target being used [aruco, charuco, charuco_diamond]"); |
| ABSL_FLAG(int32_t, team_number, 0, |
| "Required: Use the calibration for a node with this team number"); |
| ABSL_FLAG( |
| uint64_t, wait_key, 1, |
| "Time in ms to wait between images (0 to wait indefinitely until click)"); |
| |
| 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) |
| // when a single camera sees two targets, we estimate the pose between |
| // targets, and 2) when two separate cameras each see a target, we can |
| // use the pose between targets to estimate the pose between cameras. |
| |
| // We then create the extrinsics for the robot by starting with the |
| // given extrinsic for camera 1 (between imu/robot and camera frames) |
| // and then map each subsequent camera based on the data collected and |
| // the extrinsic poses computed here. |
| |
| // TODO<Jim>: Not currently using estimate from first camera to last camera-- |
| // should do full estimation, and probably also include camera->imu extrinsics |
| // from all cameras, not just first camera |
| |
| namespace y2024::vision { |
| using frc971::vision::PoseUtils; |
| using frc971::vision::TargetMap; |
| using frc971::vision::TargetMapper; |
| using frc971::vision::VisualizeRobot; |
| namespace calibration = frc971::vision::calibration; |
| |
| 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)); |
| |
| // Change reference frame from camera to robot |
| Eigen::Affine3d CameraToRobotDetection(Eigen::Affine3d H_camera_target, |
| Eigen::Affine3d extrinsics) { |
| const Eigen::Affine3d H_robot_camera = extrinsics; |
| const Eigen::Affine3d H_robot_target = H_robot_camera * H_camera_target; |
| return H_robot_target; |
| } |
| |
| struct TimestampedCameraDetection { |
| aos::distributed_clock::time_point time; |
| // Pose of target relative to robot |
| Eigen::Affine3d H_camera_target; |
| // name of pi |
| std::string camera_name; |
| int board_id; |
| }; |
| |
| TimestampedCameraDetection last_observation; |
| std::vector<std::pair<TimestampedCameraDetection, TimestampedCameraDetection>> |
| detection_list; |
| std::vector<TimestampedCameraDetection> two_board_extrinsics_list; |
| VisualizeRobot vis_robot_; |
| |
| // 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, |
| Eigen::Vector3d *rotation_variance = nullptr) { |
| std::vector<Eigen::Vector3d> translation_list; |
| std::vector<Eigen::Vector4d> rotation_list; |
| |
| 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.x(), quat.y(), quat.z(), quat.w())); |
| } |
| 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 (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() * |
| Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX()); |
| if (absl::GetFlag(FLAGS_alt_view)) { |
| // Don't rotate -- this is like viewing from the side |
| H_world_board = Eigen::Translation3d(0.0, 0.0, 3.0); |
| } |
| |
| bool draw_vis = false; |
| CHECK_LE(target_poses.size(), 2u) |
| << "Can't handle more than two tags in field of view"; |
| if (target_poses.size() == 2) { |
| draw_vis = true; |
| VLOG(1) << "Saw two boards in same view from " << camera_name; |
| int from_index = 0; |
| int to_index = 1; |
| // Handle when we see two boards at once |
| // We'll store them referenced to the lower id board |
| if (target_poses[from_index].id > target_poses[to_index].id) { |
| std::swap<int>(from_index, to_index); |
| } |
| |
| // Create "from" (A) and "to" (B) transforms |
| Eigen::Affine3d H_camera_boardA = |
| PoseUtils::Pose3dToAffine3d(target_poses[from_index].pose); |
| Eigen::Affine3d H_camera_boardB = |
| PoseUtils::Pose3dToAffine3d(target_poses[to_index].pose); |
| |
| Eigen::Affine3d H_boardA_boardB = |
| H_camera_boardA.inverse() * H_camera_boardB; |
| |
| TimestampedCameraDetection boardA_boardB{ |
| .time = distributed_eof, |
| .H_camera_target = H_boardA_boardB, |
| .camera_name = camera_name, |
| .board_id = target_poses[from_index].id}; |
| |
| VLOG(1) << "Two boards seen by " << camera_name << ". Map from board " |
| << target_poses[from_index].id << " to " |
| << target_poses[to_index].id << " is\n" |
| << H_boardA_boardB.matrix(); |
| // 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, |
| std::string("Board ") + std::to_string(target_poses[from_index].id), |
| cv::Scalar(0, 255, 0)); |
| vis_robot_.DrawFrameAxes( |
| H_world_board * boardA_boardB.H_camera_target, |
| std::string("Board ") + std::to_string(target_poses[to_index].id), |
| cv::Scalar(255, 0, 0)); |
| vis_robot_.DrawRobotOutline(H_world_board * H_camera_boardA.inverse(), |
| camera_name, kOrinColors.at(camera_name)); |
| } |
| } else if (target_poses.size() == 1) { |
| VLOG(1) << camera_name << " saw single board " << target_poses[0].id; |
| Eigen::Affine3d H_camera2_board2 = |
| PoseUtils::Pose3dToAffine3d(target_poses[0].pose); |
| TimestampedCameraDetection new_observation{ |
| .time = distributed_eof, |
| .H_camera_target = H_camera2_board2, |
| .camera_name = camera_name, |
| .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 |
| if ((new_observation.camera_name != last_observation.camera_name) && |
| (std::abs((distributed_eof - last_observation.time).count()) < |
| kImagePeriodMs / 2.0 * 1000000.0)) { |
| // Sort by camera numbering, since this is how we will handle them |
| std::pair<TimestampedCameraDetection, TimestampedCameraDetection> |
| new_pair; |
| if (ordering_map.at(last_observation.camera_name) < |
| ordering_map.at(new_observation.camera_name)) { |
| new_pair = std::pair(last_observation, new_observation); |
| } else if (ordering_map.at(last_observation.camera_name) > |
| ordering_map.at(new_observation.camera_name)) { |
| new_pair = std::pair(new_observation, last_observation); |
| } |
| |
| detection_list.push_back(new_pair); |
| |
| // This bit is just for visualization and checking purposes-- use the |
| // 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 &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_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_boardA.camera_name << " seeing board " |
| << camera2_boardA.board_id; |
| |
| // 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(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 " |
| << new_pair.second.board_id; |
| } else if (two_board_extrinsics_list.size() == 0) { |
| VLOG(1) << "Not drawing observation yet, since we don't have a two " |
| "board estimate"; |
| } |
| } else { |
| if (new_observation.camera_name == last_observation.camera_name) { |
| VLOG(2) << "Updating repeated observation for " << camera_name; |
| } else { |
| VLOG(1) << "Storing observation for " << camera_name << " at time " |
| << distributed_eof << " since last observation was " |
| << std::abs((distributed_eof - last_observation.time).count()) / |
| 1000000.0 |
| << "ms ago"; |
| } |
| last_observation = new_observation; |
| } |
| } |
| |
| if (absl::GetFlag(FLAGS_visualize)) { |
| if (!rgb_image.empty()) { |
| std::string image_name = camera_name + " Image"; |
| cv::Mat rgb_small; |
| cv::resize(rgb_image, rgb_small, cv::Size(), 0.5, 0.5); |
| cv::imshow(image_name, rgb_small); |
| cv::waitKey(absl::GetFlag(FLAGS_wait_key)); |
| } |
| |
| 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(); |
| } |
| } |
| } |
| |
| void HandleTargetMap(const TargetMap &map, |
| aos::distributed_clock::time_point distributed_eof, |
| std::string camera_name) { |
| VLOG(1) << "Got april tag map call from camera " << camera_name; |
| // 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 |
| if (static_cast<TargetMapper::TargetId>(target_pose_fbs->id()) < |
| absl::GetFlag(FLAGS_min_target_id) || |
| static_cast<TargetMapper::TargetId>(target_pose_fbs->id()) > |
| absl::GetFlag(FLAGS_max_target_id)) { |
| VLOG(1) << "Skipping tag from " << camera_name << " with invalid id of " |
| << target_pose_fbs->id(); |
| continue; |
| } |
| |
| // Skip detections with high pose errors |
| if (target_pose_fbs->pose_error() > absl::GetFlag(FLAGS_max_pose_error)) { |
| LOG(INFO) << "Skipping tag from " << camera_name << " with id " |
| << target_pose_fbs->id() << " due to pose error of " |
| << target_pose_fbs->pose_error(); |
| continue; |
| } |
| // Skip detections with high pose error ratios |
| if (target_pose_fbs->pose_error_ratio() > |
| absl::GetFlag(FLAGS_max_pose_error_ratio)) { |
| LOG(INFO) << "Skipping tag from " << camera_name << " with id " |
| << target_pose_fbs->id() << " due to pose error ratio of " |
| << target_pose_fbs->pose_error_ratio(); |
| continue; |
| } |
| |
| const TargetMapper::TargetPose target_pose = |
| TargetMapper::TargetPoseFromFbs(*target_pose_fbs); |
| |
| target_poses.emplace_back(target_pose); |
| |
| Eigen::Affine3d H_camera_target = |
| PoseUtils::Pose3dToAffine3d(target_pose.pose); |
| 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); |
| } |
| |
| void HandleImage(aos::EventLoop *event_loop, cv::Mat rgb_image, |
| const aos::monotonic_clock::time_point eof, |
| aos::distributed_clock::time_point distributed_eof, |
| frc971::vision::CharucoExtractor &charuco_extractor, |
| std::string camera_name) { |
| std::vector<cv::Vec4i> charuco_ids; |
| std::vector<std::vector<cv::Point2f>> charuco_corners; |
| bool valid = false; |
| std::vector<Eigen::Vector3d> rvecs_eigen; |
| std::vector<Eigen::Vector3d> tvecs_eigen; |
| // Why eof vs. distributed_eof? |
| charuco_extractor.ProcessImage(rgb_image, eof, event_loop->monotonic_now(), |
| charuco_ids, charuco_corners, valid, |
| rvecs_eigen, tvecs_eigen); |
| if (rvecs_eigen.size() > 0 && !valid) { |
| LOG(WARNING) << "Charuco extractor returned not valid"; |
| return; |
| } |
| |
| std::vector<TargetMapper::TargetPose> target_poses; |
| for (size_t i = 0; i < tvecs_eigen.size(); i++) { |
| Eigen::Quaterniond rotation( |
| frc971::controls::ToQuaternionFromRotationVector(rvecs_eigen[i])); |
| ceres::examples::Pose3d pose(Eigen::Vector3d(tvecs_eigen[i]), rotation); |
| TargetMapper::TargetPose target_pose{charuco_ids[i][0], pose}; |
| target_poses.emplace_back(target_pose); |
| |
| Eigen::Affine3d H_camera_target = PoseUtils::Pose3dToAffine3d(pose); |
| VLOG(2) << camera_name << " saw target " << target_pose.id |
| << " from image at timestamp " << distributed_eof |
| << " with pose = " << H_camera_target.matrix(); |
| } |
| 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() |
| ? std::nullopt |
| : std::make_optional( |
| aos::configuration::ReadConfig(absl::GetFlag(FLAGS_config)))); |
| |
| // open logfiles |
| aos::logger::LogReader reader( |
| aos::logger::SortParts(aos::logger::FindLogs(argc, argv)), |
| config.has_value() ? &config->message() : nullptr); |
| |
| reader.RemapLoggedChannel("/imu/constants", "y2024.Constants"); |
| reader.RemapLoggedChannel("/orin1/constants", "y2024.Constants"); |
| if (absl::GetFlag(FLAGS_robot)) { |
| reader.RemapLoggedChannel("/roborio/constants", "y2024.Constants"); |
| } |
| reader.Register(); |
| |
| y2024::SendSimulationConstants(reader.event_loop_factory(), |
| absl::GetFlag(FLAGS_team_number), |
| absl::GetFlag(FLAGS_constants_path)); |
| |
| VLOG(1) << "Using target type " << absl::GetFlag(FLAGS_target_type); |
| |
| std::vector<const calibration::CameraCalibration *> calibration_list; |
| |
| std::vector<std::unique_ptr<aos::EventLoop>> detection_event_loops; |
| std::vector<frc971::vision::CharucoExtractor *> charuco_extractors; |
| std::vector<Eigen::Affine3d> default_extrinsics; |
| |
| for (const CameraNode &camera_node : node_list) { |
| const aos::Node *node = aos::configuration::GetNode( |
| reader.configuration(), camera_node.node_name.c_str()); |
| |
| detection_event_loops.emplace_back( |
| reader.event_loop_factory()->MakeEventLoop( |
| (camera_node.camera_name() + "_detection").c_str(), node)); |
| |
| aos::EventLoop *const event_loop = detection_event_loops.back().get(); |
| frc971::constants::ConstantsFetcher<y2024::Constants> constants_fetcher( |
| event_loop); |
| // Get the calibration for this orin/camera pair |
| const calibration::CameraCalibration *calibration = |
| y2024::vision::FindCameraCalibration(constants_fetcher.constants(), |
| camera_node.node_name, |
| camera_node.camera_number); |
| calibration_list.push_back(calibration); |
| |
| // Extract the extrinsics from the calibration, and save as "defaults" |
| cv::Mat extrinsics_cv = |
| frc971::vision::CameraExtrinsics(calibration).value(); |
| Eigen::Matrix4d extrinsics_matrix; |
| cv::cv2eigen(extrinsics_cv, extrinsics_matrix); |
| const auto ext_H_robot_camera = Eigen::Affine3d(extrinsics_matrix); |
| default_extrinsics.emplace_back(ext_H_robot_camera); |
| |
| VLOG(1) << "Got extrinsics for " << camera_node.camera_name() << " as\n" |
| << default_extrinsics.back().matrix(); |
| |
| event_loop->MakeWatcher( |
| camera_node.camera_name(), |
| [&reader, event_loop, camera_node](const TargetMap &map) { |
| aos::distributed_clock::time_point camera_distributed_time = |
| reader.event_loop_factory() |
| ->GetNodeEventLoopFactory(event_loop->node()) |
| ->ToDistributedClock(aos::monotonic_clock::time_point( |
| aos::monotonic_clock::duration( |
| map.monotonic_timestamp_ns()))); |
| |
| HandleTargetMap(map, camera_distributed_time, |
| camera_node.camera_name()); |
| }); |
| 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(); |
| |
| 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) { |
| CHECK_EQ(base_target_id, ext.board_id) |
| << " All boards should have same reference id"; |
| if (ext.camera_name == camera_node.camera_name()) { |
| pose_list.push_back(ext); |
| } |
| } |
| 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"; |
| } |
| |
| // Next, compute the relative camera poses |
| LOG(INFO) << "Got " << detection_list.size() << " extrinsic observations"; |
| 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\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'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; |
| // If pose1 isn't referenced to base_target_id, correct that |
| if (pose1.board_id != base_target_id) { |
| // pose1.H_camera_target references boardB, so map back to boardA |
| H_camera1_boardA = |
| pose1.H_camera_target * H_boardA_boardB_avg.inverse(); |
| } |
| |
| // Now, get the camera2->boardA map (notice it's boardA, same as |
| // camera1, so we can compute the difference between the cameras with |
| // both referenced to boardA) |
| Eigen::Affine3d H_camera2_boardA = pose2.H_camera_target; |
| // If pose2 isn't referenced to boardA (base_target_id), correct that |
| if (pose2.board_id != base_target_id) { |
| // pose2.H_camera_target references boardB, so map back to boardA |
| H_camera2_boardA = |
| pose2.H_camera_target * H_boardA_boardB_avg.inverse(); |
| } |
| |
| // Compute camera1->camera2 map |
| Eigen::Affine3d H_camera1_camera2 = |
| H_camera1_boardA * H_camera2_boardA.inverse(); |
| // 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 " |
| << pose2.camera_name << " and tag " << pose2.board_id |
| << " with observation: \n" |
| << pose2.H_camera_target.matrix() << "\ngot map as\n" |
| << H_camera1_camera2.matrix(); |
| |
| Eigen::Affine3d H_world_board; |
| H_world_board = Eigen::Translation3d::Identity() * |
| Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX()); |
| if (absl::GetFlag(FLAGS_alt_view)) { |
| H_world_board = Eigen::Translation3d(0.0, 0.0, 3.0); |
| } |
| |
| VLOG(2) << "Camera1 " << pose1.camera_name << " in world frame is \n" |
| << (H_world_board * H_camera1_boardA.inverse()).matrix(); |
| VLOG(2) << "Camera2 " << pose2.camera_name << " in world frame is \n" |
| << (H_world_board * H_camera2_boardA.inverse()).matrix(); |
| } |
| } |
| // TODO<Jim>: If we don't get any matches, we could just use default |
| // extrinsics |
| CHECK(H_camera1_camera2_list.size() > 0) |
| << "Failed with zero poses for node " << node_list.at(i).camera_name() |
| << " and " << node_list.at(i + 1).camera_name(); |
| if (H_camera1_camera2_list.size() > 0) { |
| Eigen::Affine3d H_camera1_camera2_avg = |
| ComputeAveragePose(H_camera1_camera2_list); |
| LOG(INFO) << "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(); |
| |
| 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" |
| << H_camera1_camera2_default.matrix(); |
| Eigen::Affine3d H_camera1_camera2_diff = |
| H_camera1_camera2_avg * H_camera1_camera2_default.inverse(); |
| LOG(INFO) |
| << "Difference between averaged and default delta poses " |
| "has |T| = " |
| << H_camera1_camera2_diff.translation().norm() << "m and |R| = " |
| << Eigen::AngleAxisd(H_camera1_camera2_diff.rotation()).angle() |
| << " radians (" |
| << 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; |
| updated_extrinsics.push_back(next_extrinsic); |
| LOG(INFO) << "Default Extrinsic for " << node_list.at(i + 1).camera_name() |
| << " is \n" |
| << default_extrinsics[i + 1].matrix(); |
| LOG(INFO) << "--> Updated Extrinsic for " |
| << node_list.at(i + 1).camera_name() << " is \n" |
| << next_extrinsic.matrix(); |
| |
| WriteExtrinsicFile(next_extrinsic, node_list[i + 1], |
| calibration_list[i + 1]); |
| |
| if (absl::GetFlag(FLAGS_visualize)) { |
| // Draw each of the updated extrinsic camera locations |
| vis_robot_.SetDefaultViewpoint(1000.0, 1500.0); |
| vis_robot_.DrawFrameAxes( |
| updated_extrinsics.back(), node_list.at(i + 1).camera_name(), |
| kOrinColors.at(node_list.at(i + 1).camera_name())); |
| } |
| } |
| } |
| if (absl::GetFlag(FLAGS_visualize)) { |
| // And don't forget to draw the base camera location |
| vis_robot_.DrawFrameAxes(updated_extrinsics[0], |
| node_list.at(0).camera_name(), |
| kOrinColors.at(node_list.at(0).camera_name())); |
| cv::imshow("Extrinsic visualization", vis_robot_.image_); |
| // Add a bit extra time, so that we don't go right through the extrinsics |
| cv::waitKey(3000); |
| cv::waitKey(0); |
| } |
| |
| // Cleanup |
| for (uint i = 0; i < charuco_extractors.size(); i++) { |
| delete charuco_extractors[i]; |
| } |
| } |
| } // namespace y2024::vision |
| |
| int main(int argc, char **argv) { |
| aos::InitGoogle(&argc, &argv); |
| y2024::vision::ExtrinsicsMain(argc, argv); |
| } |