Pull common extrinsics calibration code out into //frc971/vision
This sets us up to have a generic solver interface, and year specific
data munging.
Change-Id: I5cba597aa263d5061b7c71cd617706460ddb5f93
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index ae4e3f2..de85213 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -69,34 +69,6 @@
],
)
-cc_library(
- name = "charuco_lib",
- srcs = [
- "charuco_lib.cc",
- ],
- hdrs = [
- "charuco_lib.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//y2020:__subpackages__"],
- deps = [
- "//aos:flatbuffers",
- "//aos/events:event_loop",
- "//aos/network:message_bridge_server_fbs",
- "//aos/network:team_number",
- "//frc971/control_loops:quaternion_utils",
- "//frc971/vision:vision_fbs",
- "//third_party:opencv",
- "//y2020/vision/sift:sift_fbs",
- "//y2020/vision/sift:sift_training_fbs",
- "//y2020/vision/tools/python_code:sift_training_data",
- "@com_github_google_glog//:glog",
- "@com_google_absl//absl/strings:str_format",
- "@com_google_absl//absl/types:span",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
cc_binary(
name = "calibration",
srcs = [
@@ -111,10 +83,10 @@
"//y2022:__subpackages__",
],
deps = [
- ":charuco_lib",
"//aos:init",
"//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/vision:charuco_lib",
"//frc971/vision:vision_fbs",
"//frc971/wpilib:imu_batch_fbs",
"//frc971/wpilib:imu_fbs",
@@ -149,27 +121,13 @@
cc_binary(
name = "extrinsics_calibration",
srcs = [
- "calibration_accumulator.cc",
- "calibration_accumulator.h",
"extrinsics_calibration.cc",
],
- data = [
- "//y2020:aos_config",
- ],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//y2020:__subpackages__"],
deps = [
- ":charuco_lib",
"//aos:init",
- "//aos/events:shm_event_loop",
"//aos/events/logging:log_reader",
- "//frc971/analysis:in_process_plotter",
- "//frc971/control_loops/drivetrain:improved_down_estimator",
- "//frc971/wpilib:imu_batch_fbs",
- "//frc971/wpilib:imu_fbs",
- "//third_party:opencv",
- "@com_google_absl//absl/strings:str_format",
- "@com_google_ceres_solver//:ceres",
- "@org_tuxfamily_eigen//:eigen",
+ "//frc971/vision:extrinsics_calibration",
],
)
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index 0a12d70..1109b58 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -12,7 +12,7 @@
#include "aos/network/team_number.h"
#include "aos/time/time.h"
#include "aos/util/file.h"
-#include "y2020/vision/charuco_lib.h"
+#include "frc971/vision/charuco_lib.h"
DEFINE_string(calibration_folder, ".", "Folder to place calibration files.");
DEFINE_string(camera_id, "", "Camera ID in format YY-NN-- year and number.");
diff --git a/y2020/vision/calibration_accumulator.cc b/y2020/vision/calibration_accumulator.cc
deleted file mode 100644
index 9f550c5..0000000
--- a/y2020/vision/calibration_accumulator.cc
+++ /dev/null
@@ -1,179 +0,0 @@
-#include "y2020/vision/calibration_accumulator.h"
-
-#include <opencv2/aruco/charuco.hpp>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/features2d.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc.hpp>
-
-#include "Eigen/Dense"
-#include "aos/events/simulated_event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/quaternion_utils.h"
-#include "frc971/wpilib/imu_batch_generated.h"
-#include "y2020/vision/charuco_lib.h"
-
-DEFINE_bool(display_undistorted, false,
- "If true, display the undistorted image.");
-
-namespace frc971 {
-namespace vision {
-using aos::distributed_clock;
-using aos::monotonic_clock;
-namespace chrono = std::chrono;
-
-constexpr double kG = 9.807;
-
-void CalibrationData::AddCameraPose(
- distributed_clock::time_point distributed_now, Eigen::Vector3d rvec,
- Eigen::Vector3d tvec) {
- // Always start with IMU reading...
- if (!imu_points_.empty() && imu_points_[0].first < distributed_now) {
- rot_trans_points_.emplace_back(distributed_now, std::make_pair(rvec, tvec));
- }
-}
-
-void CalibrationData::AddImu(distributed_clock::time_point distributed_now,
- Eigen::Vector3d gyro, Eigen::Vector3d accel) {
- imu_points_.emplace_back(distributed_now, std::make_pair(gyro, accel));
-}
-
-void CalibrationData::ReviewData(CalibrationDataObserver *observer) {
- size_t next_imu_point = 0;
- size_t next_camera_point = 0;
- while (true) {
- if (next_imu_point != imu_points_.size()) {
- // There aren't that many combinations, so just brute force them all
- // rather than being too clever.
- if (next_camera_point != rot_trans_points_.size()) {
- if (imu_points_[next_imu_point].first >
- rot_trans_points_[next_camera_point].first) {
- // Camera!
- observer->UpdateCamera(rot_trans_points_[next_camera_point].first,
- rot_trans_points_[next_camera_point].second);
- ++next_camera_point;
- } else {
- // IMU!
- observer->UpdateIMU(imu_points_[next_imu_point].first,
- imu_points_[next_imu_point].second);
- ++next_imu_point;
- }
- } else {
- if (next_camera_point != rot_trans_points_.size()) {
- // Camera!
- observer->UpdateCamera(rot_trans_points_[next_camera_point].first,
- rot_trans_points_[next_camera_point].second);
- ++next_camera_point;
- } else {
- // Nothing left for either list of points, so we are done.
- break;
- }
- }
- }
- }
-}
-
-Calibration::Calibration(aos::SimulatedEventLoopFactory *event_loop_factory,
- aos::EventLoop *image_event_loop,
- aos::EventLoop *imu_event_loop, std::string_view pi,
- CalibrationData *data)
- : image_event_loop_(image_event_loop),
- image_factory_(event_loop_factory->GetNodeEventLoopFactory(
- image_event_loop_->node())),
- imu_event_loop_(imu_event_loop),
- imu_factory_(
- event_loop_factory->GetNodeEventLoopFactory(imu_event_loop_->node())),
- charuco_extractor_(
- image_event_loop_, pi,
- [this](cv::Mat rgb_image, monotonic_clock::time_point eof,
- std::vector<int> charuco_ids,
- std::vector<cv::Point2f> charuco_corners, bool valid,
- Eigen::Vector3d rvec_eigen, Eigen::Vector3d tvec_eigen) {
- HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
- rvec_eigen, tvec_eigen);
- }),
- data_(data) {
- imu_factory_->OnShutdown([]() { cv::destroyAllWindows(); });
-
- imu_event_loop_->MakeWatcher(
- "/drivetrain", [this](const frc971::IMUValuesBatch &imu) {
- if (!imu.has_readings()) {
- return;
- }
- for (const frc971::IMUValues *value : *imu.readings()) {
- HandleIMU(value);
- }
- });
-}
-
-void Calibration::HandleCharuco(cv::Mat rgb_image,
- const monotonic_clock::time_point eof,
- std::vector<int> /*charuco_ids*/,
- std::vector<cv::Point2f> /*charuco_corners*/,
- bool valid, Eigen::Vector3d rvec_eigen,
- Eigen::Vector3d tvec_eigen) {
- if (valid) {
- data_->AddCameraPose(image_factory_->ToDistributedClock(eof), rvec_eigen,
- tvec_eigen);
-
- // TODO(austin): Need a gravity vector input.
- //
- // TODO(austin): Need a state, covariance, and model.
- //
- // TODO(austin): Need to record all the values out of a log and run it
- // as a batch run so we can feed it into ceres.
-
- // Z -> up
- // Y -> away from cameras 2 and 3
- // X -> left
- Eigen::Vector3d imu(last_value_.accelerometer_x,
- last_value_.accelerometer_y,
- last_value_.accelerometer_z);
-
- Eigen::Quaternion<double> imu_to_camera(
- Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitX()));
-
- Eigen::Quaternion<double> board_to_world(
- Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()));
-
- Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ",\n", "[", "]",
- "[", "]");
-
- const double age_double =
- std::chrono::duration_cast<std::chrono::duration<double>>(
- image_event_loop_->monotonic_now() - eof)
- .count();
- LOG(INFO) << std::fixed << std::setprecision(6) << "Age: " << age_double
- << ", Pose is R:" << rvec_eigen.transpose().format(HeavyFmt)
- << " T:" << tvec_eigen.transpose().format(HeavyFmt);
- }
-
- cv::imshow("Display", rgb_image);
-
- if (FLAGS_display_undistorted) {
- const cv::Size image_size(rgb_image.cols, rgb_image.rows);
- cv::Mat undistorted_rgb_image(image_size, CV_8UC3);
- cv::undistort(rgb_image, undistorted_rgb_image,
- charuco_extractor_.camera_matrix(),
- charuco_extractor_.dist_coeffs());
-
- cv::imshow("Display undist", undistorted_rgb_image);
- }
-}
-
-void Calibration::HandleIMU(const frc971::IMUValues *imu) {
- VLOG(1) << "IMU " << imu;
- imu->UnPackTo(&last_value_);
- Eigen::Vector3d gyro(last_value_.gyro_x, last_value_.gyro_y,
- last_value_.gyro_z);
- Eigen::Vector3d accel(last_value_.accelerometer_x,
- last_value_.accelerometer_y,
- last_value_.accelerometer_z);
-
- data_->AddImu(imu_factory_->ToDistributedClock(monotonic_clock::time_point(
- chrono::nanoseconds(imu->monotonic_timestamp_ns()))),
- gyro, accel * kG);
-}
-
-} // namespace vision
-} // namespace frc971
diff --git a/y2020/vision/calibration_accumulator.h b/y2020/vision/calibration_accumulator.h
deleted file mode 100644
index 7bff9f0..0000000
--- a/y2020/vision/calibration_accumulator.h
+++ /dev/null
@@ -1,95 +0,0 @@
-#ifndef Y2020_VISION_CALIBRATION_ACCUMULATOR_H_
-#define Y2020_VISION_CALIBRATION_ACCUMULATOR_H_
-
-#include <vector>
-
-#include "Eigen/Dense"
-#include "aos/events/simulated_event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/quaternion_utils.h"
-#include "frc971/wpilib/imu_batch_generated.h"
-#include "y2020/vision/charuco_lib.h"
-
-namespace frc971 {
-namespace vision {
-
-// This class provides an interface for an application to be notified of all
-// camera and IMU samples in order with the correct timestamps.
-class CalibrationDataObserver {
- public:
- // Observes a camera sample at the corresponding time t, and with the
- // corresponding rotation and translation vectors rt.
- virtual void UpdateCamera(aos::distributed_clock::time_point t,
- std::pair<Eigen::Vector3d, Eigen::Vector3d> rt) = 0;
-
- // Observes an IMU sample at the corresponding time t, and with the
- // corresponding angular velocity and linear acceleration vectors wa.
- virtual void UpdateIMU(aos::distributed_clock::time_point t,
- std::pair<Eigen::Vector3d, Eigen::Vector3d> wa) = 0;
-};
-
-// Class to both accumulate and replay camera and IMU data in time order.
-class CalibrationData {
- public:
- // Adds a camera/charuco detection to the list at the provided time.
- // This has only been tested with a charuco board.
- void AddCameraPose(aos::distributed_clock::time_point distributed_now,
- Eigen::Vector3d rvec, Eigen::Vector3d tvec);
-
- // Adds an IMU point to the list at the provided time.
- void AddImu(aos::distributed_clock::time_point distributed_now,
- Eigen::Vector3d gyro, Eigen::Vector3d accel);
-
- // Processes the data points by calling UpdateCamera and UpdateIMU in time
- // order.
- void ReviewData(CalibrationDataObserver *observer);
-
- size_t camera_samples_size() const { return rot_trans_points_.size(); }
-
- private:
- std::vector<std::pair<aos::distributed_clock::time_point,
- std::pair<Eigen::Vector3d, Eigen::Vector3d>>>
- imu_points_;
-
- // Store pose samples as timestamp, along with
- // pair of rotation, translation vectors
- std::vector<std::pair<aos::distributed_clock::time_point,
- std::pair<Eigen::Vector3d, Eigen::Vector3d>>>
- rot_trans_points_;
-};
-
-// Class to register image and IMU callbacks in AOS and route them to the
-// corresponding CalibrationData class.
-class Calibration {
- public:
- Calibration(aos::SimulatedEventLoopFactory *event_loop_factory,
- aos::EventLoop *image_event_loop, aos::EventLoop *imu_event_loop,
- std::string_view pi, CalibrationData *data);
-
- // Processes a charuco detection.
- void HandleCharuco(cv::Mat rgb_image,
- const aos::monotonic_clock::time_point eof,
- std::vector<int> /*charuco_ids*/,
- std::vector<cv::Point2f> /*charuco_corners*/, bool valid,
- Eigen::Vector3d rvec_eigen, Eigen::Vector3d tvec_eigen);
-
- // Processes an IMU reading.
- void HandleIMU(const frc971::IMUValues *imu);
-
- private:
- aos::EventLoop *image_event_loop_;
- aos::NodeEventLoopFactory *image_factory_;
- aos::EventLoop *imu_event_loop_;
- aos::NodeEventLoopFactory *imu_factory_;
-
- CharucoExtractor charuco_extractor_;
-
- CalibrationData *data_;
-
- frc971::IMUValuesT last_value_;
-};
-
-} // namespace vision
-} // namespace frc971
-
-#endif // Y2020_VISION_CALIBRATION_ACCUMULATOR_H_
diff --git a/y2020/vision/charuco_lib.cc b/y2020/vision/charuco_lib.cc
deleted file mode 100644
index 8677cbb..0000000
--- a/y2020/vision/charuco_lib.cc
+++ /dev/null
@@ -1,289 +0,0 @@
-#include "y2020/vision/charuco_lib.h"
-
-#include <chrono>
-#include <functional>
-#include <string_view>
-
-#include <opencv2/core/eigen.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc.hpp>
-#include "aos/events/event_loop.h"
-#include "aos/flatbuffers.h"
-#include "aos/network/team_number.h"
-#include "frc971/control_loops/quaternion_utils.h"
-#include "frc971/vision/vision_generated.h"
-#include "glog/logging.h"
-#include "y2020/vision/sift/sift_generated.h"
-#include "y2020/vision/sift/sift_training_generated.h"
-#include "y2020/vision/tools/python_code/sift_training_data.h"
-
-DEFINE_uint32(min_targets, 10,
- "The mininum number of targets required to match.");
-DEFINE_bool(large_board, true, "If true, use the large calibration board.");
-DEFINE_bool(coarse_pattern, true, "If true, use coarse arucos; else, use fine");
-DEFINE_string(board_template_path, "",
- "If specified, write an image to the specified path for the "
- "charuco board pattern.");
-
-namespace frc971 {
-namespace vision {
-namespace chrono = std::chrono;
-using aos::monotonic_clock;
-
-CameraCalibration::CameraCalibration(
- const absl::Span<const uint8_t> training_data_bfbs, std::string_view pi) {
- const aos::FlatbufferSpan<sift::TrainingData> training_data(
- training_data_bfbs);
- CHECK(training_data.Verify());
- camera_calibration_ = FindCameraCalibration(&training_data.message(), pi);
-}
-
-cv::Mat CameraCalibration::CameraIntrinsics() const {
- const cv::Mat result(3, 3, CV_32F,
- const_cast<void *>(static_cast<const void *>(
- camera_calibration_->intrinsics()->data())));
- CHECK_EQ(result.total(), camera_calibration_->intrinsics()->size());
- return result;
-}
-
-Eigen::Matrix3d CameraCalibration::CameraIntrinsicsEigen() const {
- cv::Mat camera_intrinsics = CameraIntrinsics();
- Eigen::Matrix3d result;
- cv::cv2eigen(camera_intrinsics, result);
- return result;
-}
-
-cv::Mat CameraCalibration::CameraDistCoeffs() const {
- const cv::Mat result(5, 1, CV_32F,
- const_cast<void *>(static_cast<const void *>(
- camera_calibration_->dist_coeffs()->data())));
- CHECK_EQ(result.total(), camera_calibration_->dist_coeffs()->size());
- return result;
-}
-
-const sift::CameraCalibration *CameraCalibration::FindCameraCalibration(
- const sift::TrainingData *const training_data, std::string_view pi) const {
- std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(pi);
- std::optional<uint16_t> team_number =
- aos::network::team_number_internal::ParsePiTeamNumber(pi);
- CHECK(pi_number);
- CHECK(team_number);
- const std::string node_name = absl::StrFormat("pi%d", pi_number.value());
- LOG(INFO) << "Looking for node name " << node_name << " team number "
- << team_number.value();
- for (const sift::CameraCalibration *candidate :
- *training_data->camera_calibrations()) {
- if (candidate->node_name()->string_view() != node_name) {
- continue;
- }
- if (candidate->team_number() != team_number.value()) {
- continue;
- }
- return candidate;
- }
- LOG(FATAL) << ": Failed to find camera calibration for " << node_name
- << " on " << team_number.value();
-}
-
-ImageCallback::ImageCallback(
- aos::EventLoop *event_loop, std::string_view channel,
- std::function<void(cv::Mat, monotonic_clock::time_point)> &&fn)
- : event_loop_(event_loop),
- server_fetcher_(
- event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
- "/aos")),
- source_node_(aos::configuration::GetNode(
- event_loop_->configuration(),
- event_loop_->GetChannel<CameraImage>(channel)
- ->source_node()
- ->string_view())),
- handle_image_(std::move(fn)) {
- event_loop_->MakeWatcher(channel, [this](const CameraImage &image) {
- const monotonic_clock::time_point eof_source_node =
- monotonic_clock::time_point(
- chrono::nanoseconds(image.monotonic_timestamp_ns()));
- chrono::nanoseconds offset{0};
- if (source_node_ != event_loop_->node()) {
- server_fetcher_.Fetch();
- if (!server_fetcher_.get()) {
- return;
- }
-
- // If we are viewing this image from another node, convert to our
- // monotonic clock.
- const aos::message_bridge::ServerConnection *server_connection = nullptr;
-
- for (const aos::message_bridge::ServerConnection *connection :
- *server_fetcher_->connections()) {
- CHECK(connection->has_node());
- if (connection->node()->name()->string_view() ==
- source_node_->name()->string_view()) {
- server_connection = connection;
- break;
- }
- }
-
- CHECK(server_connection != nullptr) << ": Failed to find client";
- if (!server_connection->has_monotonic_offset()) {
- VLOG(1) << "No offset yet.";
- return;
- }
- offset = chrono::nanoseconds(server_connection->monotonic_offset());
- }
-
- const monotonic_clock::time_point eof = eof_source_node - offset;
-
- const monotonic_clock::duration age = event_loop_->monotonic_now() - eof;
- const double age_double =
- std::chrono::duration_cast<std::chrono::duration<double>>(age).count();
- if (age > std::chrono::milliseconds(100)) {
- VLOG(2) << "Age: " << age_double << ", getting behind, skipping";
- return;
- }
- // Create color image:
- cv::Mat image_color_mat(cv::Size(image.cols(), image.rows()), CV_8UC2,
- (void *)image.data()->data());
- const cv::Size image_size(image.cols(), image.rows());
- cv::Mat rgb_image(image_size, CV_8UC3);
- cv::cvtColor(image_color_mat, rgb_image, cv::COLOR_YUV2BGR_YUYV);
- handle_image_(rgb_image, eof);
- });
-}
-
-CharucoExtractor::CharucoExtractor(
- aos::EventLoop *event_loop, std::string_view pi,
- std::function<void(cv::Mat, monotonic_clock::time_point, std::vector<int>,
- std::vector<cv::Point2f>, bool, Eigen::Vector3d,
- Eigen::Vector3d)> &&fn)
- : event_loop_(event_loop),
- calibration_(SiftTrainingData(), pi),
- dictionary_(cv::aruco::getPredefinedDictionary(
- FLAGS_large_board ? cv::aruco::DICT_5X5_250
- : cv::aruco::DICT_6X6_250)),
- board_(
- FLAGS_large_board
- ? (FLAGS_coarse_pattern ? cv::aruco::CharucoBoard::create(
- 12, 9, 0.06, 0.04666, dictionary_)
- : cv::aruco::CharucoBoard::create(
- 25, 18, 0.03, 0.0233, dictionary_))
- : (FLAGS_coarse_pattern ? cv::aruco::CharucoBoard::create(
- 7, 5, 0.04, 0.025, dictionary_)
- // TODO(jim): Need to figure out what size
- // is for small board, fine pattern
- : cv::aruco::CharucoBoard::create(
- 7, 5, 0.03, 0.0233, dictionary_))),
- camera_matrix_(calibration_.CameraIntrinsics()),
- eigen_camera_matrix_(calibration_.CameraIntrinsicsEigen()),
- dist_coeffs_(calibration_.CameraDistCoeffs()),
- pi_number_(aos::network::ParsePiNumber(pi)),
- image_callback_(
- event_loop,
- absl::StrCat("/pi", std::to_string(pi_number_.value()), "/camera"),
- [this](cv::Mat rgb_image, const monotonic_clock::time_point eof) {
- HandleImage(rgb_image, eof);
- }),
- handle_charuco_(std::move(fn)) {
- LOG(INFO) << "Using " << (FLAGS_large_board ? "large" : "small")
- << " board with " << (FLAGS_coarse_pattern ? "coarse" : "fine")
- << " pattern";
- if (!FLAGS_board_template_path.empty()) {
- cv::Mat board_image;
- board_->draw(cv::Size(600, 500), board_image, 10, 1);
- cv::imwrite(FLAGS_board_template_path, board_image);
- }
-
- LOG(INFO) << "Camera matrix " << camera_matrix_;
- LOG(INFO) << "Distortion Coefficients " << dist_coeffs_;
-
- CHECK(pi_number_) << ": Invalid pi number " << pi
- << ", failed to parse pi number";
-
- LOG(INFO) << "Connecting to channel /pi" << pi_number_.value() << "/camera";
-}
-
-void CharucoExtractor::HandleImage(cv::Mat rgb_image,
- const monotonic_clock::time_point eof) {
- const double age_double =
- std::chrono::duration_cast<std::chrono::duration<double>>(
- event_loop_->monotonic_now() - eof)
- .count();
- std::vector<int> marker_ids;
- std::vector<std::vector<cv::Point2f>> marker_corners;
-
- cv::aruco::detectMarkers(rgb_image, board_->dictionary, marker_corners,
- marker_ids);
-
- std::vector<cv::Point2f> charuco_corners;
- std::vector<int> charuco_ids;
- bool valid = false;
- Eigen::Vector3d rvec_eigen = Eigen::Vector3d::Zero();
- Eigen::Vector3d tvec_eigen = Eigen::Vector3d::Zero();
-
- // If at least one marker detected
- if (marker_ids.size() >= FLAGS_min_targets) {
- // Run everything twice, once with the calibration, and once
- // without. This lets us both calibrate, and also print out the pose
- // real time with the previous calibration.
- cv::aruco::interpolateCornersCharuco(marker_corners, marker_ids, rgb_image,
- board_, charuco_corners, charuco_ids);
-
- std::vector<cv::Point2f> charuco_corners_with_calibration;
- std::vector<int> charuco_ids_with_calibration;
-
- cv::aruco::interpolateCornersCharuco(
- marker_corners, marker_ids, rgb_image, board_,
- charuco_corners_with_calibration, charuco_ids_with_calibration,
- camera_matrix_, dist_coeffs_);
-
- cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
-
- if (charuco_ids.size() >= FLAGS_min_targets) {
- cv::aruco::drawDetectedCornersCharuco(rgb_image, charuco_corners,
- charuco_ids, cv::Scalar(255, 0, 0));
-
- cv::Vec3d rvec, tvec;
- valid = cv::aruco::estimatePoseCharucoBoard(
- charuco_corners_with_calibration, charuco_ids_with_calibration,
- board_, camera_matrix_, dist_coeffs_, rvec, tvec);
-
- // if charuco pose is valid
- if (valid) {
- cv::cv2eigen(rvec, rvec_eigen);
- cv::cv2eigen(tvec, tvec_eigen);
-
- Eigen::Quaternion<double> rotation(
- frc971::controls::ToQuaternionFromRotationVector(rvec_eigen));
- Eigen::Translation3d translation(tvec_eigen);
-
- const Eigen::Affine3d board_to_camera = translation * rotation;
-
- Eigen::Matrix<double, 3, 4> camera_projection =
- Eigen::Matrix<double, 3, 4>::Identity();
- Eigen::Vector3d result = eigen_camera_matrix_ * camera_projection *
- board_to_camera * Eigen::Vector3d::Zero();
-
- result /= result.z();
- cv::circle(rgb_image, cv::Point(result.x(), result.y()), 4,
- cv::Scalar(255, 255, 255), 0, cv::LINE_8);
-
- cv::aruco::drawAxis(rgb_image, camera_matrix_, dist_coeffs_, rvec, tvec,
- 0.1);
- } else {
- LOG(INFO) << "Age: " << age_double << ", invalid pose";
- }
- } else {
- LOG(INFO) << "Age: " << age_double << ", not enough charuco IDs, got "
- << charuco_ids.size() << ", needed " << FLAGS_min_targets;
- }
- } else {
- LOG(INFO) << "Age: " << age_double << ", not enough marker IDs, got "
- << marker_ids.size() << ", needed " << FLAGS_min_targets;
- cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
- }
-
- handle_charuco_(rgb_image, eof, charuco_ids, charuco_corners, valid,
- rvec_eigen, tvec_eigen);
-}
-
-} // namespace vision
-} // namespace frc971
diff --git a/y2020/vision/charuco_lib.h b/y2020/vision/charuco_lib.h
deleted file mode 100644
index a54bfca..0000000
--- a/y2020/vision/charuco_lib.h
+++ /dev/null
@@ -1,117 +0,0 @@
-#ifndef Y2020_VISION_CHARUCO_LIB_H_
-#define Y2020_VISION_CHARUCO_LIB_H_
-
-#include <functional>
-#include <string_view>
-
-#include <opencv2/aruco/charuco.hpp>
-#include <opencv2/calib3d.hpp>
-#include "Eigen/Dense"
-#include "Eigen/Geometry"
-
-#include "absl/types/span.h"
-#include "aos/events/event_loop.h"
-#include "aos/network/message_bridge_server_generated.h"
-#include "y2020/vision/sift/sift_generated.h"
-#include "y2020/vision/sift/sift_training_generated.h"
-
-namespace frc971 {
-namespace vision {
-
-// Class to find extrinsics for a specified pi's camera using the provided
-// training data.
-class CameraCalibration {
- public:
- CameraCalibration(const absl::Span<const uint8_t> training_data_bfbs,
- std::string_view pi);
-
- // Intrinsics for the located camera.
- cv::Mat CameraIntrinsics() const;
- Eigen::Matrix3d CameraIntrinsicsEigen() const;
-
- // Distortion coefficients for the located camera.
- cv::Mat CameraDistCoeffs() const;
-
- private:
- // Finds the camera specific calibration flatbuffer.
- const sift::CameraCalibration *FindCameraCalibration(
- const sift::TrainingData *const training_data, std::string_view pi) const;
-
- // Pointer to this camera's calibration parameters.
- const sift::CameraCalibration *camera_calibration_;
-};
-
-// Class to call a function with a cv::Mat and age when an image shows up on the
-// provided channel. This hides all the conversions and wrangling needed to
-// view the image.
-class ImageCallback {
- public:
- ImageCallback(
- aos::EventLoop *event_loop, std::string_view channel,
- std::function<void(cv::Mat, aos::monotonic_clock::time_point)> &&fn);
-
- private:
- aos::EventLoop *event_loop_;
- aos::Fetcher<aos::message_bridge::ServerStatistics> server_fetcher_;
- const aos::Node *source_node_;
- std::function<void(cv::Mat, aos::monotonic_clock::time_point)> handle_image_;
-};
-
-// Class which calls a callback each time an image arrives with the information
-// extracted from it.
-class CharucoExtractor {
- public:
- // The callback takes the following arguments:
- // cv::Mat -> image with overlays drawn on it.
- // monotonic_clock::time_point -> Time on this node when this image was
- // captured.
- // std::vector<int> -> charuco_ids
- // std::vector<cv::Point2f> -> charuco_corners
- // bool -> true if rvec/tvec is valid.
- // Eigen::Vector3d -> rvec
- // Eigen::Vector3d -> tvec
- CharucoExtractor(
- aos::EventLoop *event_loop, std::string_view pi,
- std::function<void(cv::Mat, aos::monotonic_clock::time_point,
- std::vector<int>, std::vector<cv::Point2f>, bool,
- Eigen::Vector3d, Eigen::Vector3d)> &&fn);
-
- // Returns the aruco dictionary in use.
- cv::Ptr<cv::aruco::Dictionary> dictionary() const { return dictionary_; }
- // Returns the aruco board in use.
- cv::Ptr<cv::aruco::CharucoBoard> board() const { return board_; }
-
- // Returns the camera matrix for this camera.
- const cv::Mat camera_matrix() const { return camera_matrix_; }
- // Returns the distortion coefficients for this camera.
- const cv::Mat dist_coeffs() const { return dist_coeffs_; }
-
- private:
- // Handles the image by detecting the charuco board in it.
- void HandleImage(cv::Mat rgb_image, aos::monotonic_clock::time_point eof);
-
- aos::EventLoop *event_loop_;
- CameraCalibration calibration_;
-
- cv::Ptr<cv::aruco::Dictionary> dictionary_;
- cv::Ptr<cv::aruco::CharucoBoard> board_;
-
- const cv::Mat camera_matrix_;
- const Eigen::Matrix3d eigen_camera_matrix_;
- const cv::Mat dist_coeffs_;
-
- const std::optional<uint16_t> pi_number_;
-
- ImageCallback image_callback_;
-
- // Function to call.
- std::function<void(cv::Mat, aos::monotonic_clock::time_point,
- std::vector<int>, std::vector<cv::Point2f>, bool,
- Eigen::Vector3d, Eigen::Vector3d)>
- handle_charuco_;
-};
-
-} // namespace vision
-} // namespace frc971
-
-#endif // Y2020_VISION_CHARUCO_LIB_H_
diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index 5f08527..c54a5cf 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -3,19 +3,14 @@
#include "absl/strings/str_format.h"
#include "aos/events/logging/log_reader.h"
-#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/network/team_number.h"
#include "aos/time/time.h"
#include "aos/util/file.h"
-#include "ceres/ceres.h"
-#include "frc971/analysis/in_process_plotter.h"
-#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/vision/vision_generated.h"
+#include "frc971/vision/extrinsics_calibration.h"
#include "frc971/wpilib/imu_batch_generated.h"
-#include "y2020/vision/calibration_accumulator.h"
-#include "y2020/vision/charuco_lib.h"
#include "y2020/vision/sift/sift_generated.h"
#include "y2020/vision/sift/sift_training_generated.h"
#include "y2020/vision/tools/python_code/sift_training_data.h"
@@ -30,529 +25,6 @@
using aos::distributed_clock;
using aos::monotonic_clock;
-constexpr double kGravity = 9.8;
-
-// The basic ideas here are taken from Kalibr.
-// (https://github.com/ethz-asl/kalibr), but adapted to work with AOS, and to be
-// simpler.
-//
-// Camera readings and IMU readings come in at different times, on different
-// time scales. Our first problem is to align them in time so we can actually
-// compute an error. This is done in the calibration accumulator code. The
-// kalibr paper uses splines, while this uses kalman filters to solve the same
-// interpolation problem so we can get the expected vs actual pose at the time
-// each image arrives.
-//
-// The cost function is then fed the computed angular and positional error for
-// each camera sample before the kalman filter update. Intuitively, the smaller
-// the corrections to the kalman filter each step, the better the estimate
-// should be.
-//
-// We don't actually implement the angular kalman filter because the IMU is so
-// good. We give the solver an initial position and bias, and let it solve from
-// there. This lets us represent drift that is linear in time, which should be
-// good enough for ~1 minute calibration.
-//
-// TODO(austin): Kalman smoother ala
-// https://stanford.edu/~boyd/papers/pdf/auto_ks.pdf should allow for better
-// parallelism, and since we aren't causal, will take that into account a lot
-// better.
-
-// This class takes the initial parameters and biases, and computes the error
-// between the measured and expected camera readings. When optimized, this
-// gives us a cost function to minimize.
-template <typename Scalar>
-class CeresPoseFilter : public CalibrationDataObserver {
- public:
- typedef Eigen::Transform<Scalar, 3, Eigen::Affine> Affine3s;
-
- CeresPoseFilter(Eigen::Quaternion<Scalar> initial_orientation,
- Eigen::Quaternion<Scalar> imu_to_camera,
- Eigen::Matrix<Scalar, 3, 1> gyro_bias,
- Eigen::Matrix<Scalar, 6, 1> initial_state,
- Eigen::Quaternion<Scalar> board_to_world,
- Eigen::Matrix<Scalar, 3, 1> imu_to_camera_translation,
- Scalar gravity_scalar,
- Eigen::Matrix<Scalar, 3, 1> accelerometer_bias)
- : accel_(Eigen::Matrix<double, 3, 1>::Zero()),
- omega_(Eigen::Matrix<double, 3, 1>::Zero()),
- imu_bias_(gyro_bias),
- orientation_(initial_orientation),
- x_hat_(initial_state),
- p_(Eigen::Matrix<Scalar, 6, 6>::Zero()),
- imu_to_camera_rotation_(imu_to_camera),
- imu_to_camera_translation_(imu_to_camera_translation),
- board_to_world_(board_to_world),
- gravity_scalar_(gravity_scalar),
- accelerometer_bias_(accelerometer_bias) {}
-
- Scalar gravity_scalar() { return gravity_scalar_; }
-
- virtual void ObserveCameraUpdate(
- distributed_clock::time_point /*t*/,
- Eigen::Vector3d /*board_to_camera_rotation*/,
- Eigen::Quaternion<Scalar> /*imu_to_world_rotation*/,
- Affine3s /*imu_to_world*/) {}
-
- // Observes a camera measurement by applying a kalman filter correction and
- // accumulating up the error associated with the step.
- void UpdateCamera(distributed_clock::time_point t,
- std::pair<Eigen::Vector3d, Eigen::Vector3d> rt) override {
- Integrate(t);
-
- const Eigen::Quaternion<Scalar> board_to_camera_rotation(
- frc971::controls::ToQuaternionFromRotationVector(rt.first)
- .cast<Scalar>());
- const Affine3s board_to_camera =
- Eigen::Translation3d(rt.second).cast<Scalar>() *
- board_to_camera_rotation;
-
- const Affine3s imu_to_camera =
- imu_to_camera_translation_ * imu_to_camera_rotation_;
-
- // This converts us from (facing the board),
- // x right, y up, z towards us -> x right, y away, z up.
- // Confirmed to be right.
-
- // Want world -> imu rotation.
- // world <- board <- camera <- imu.
- const Eigen::Quaternion<Scalar> imu_to_world_rotation =
- board_to_world_ * board_to_camera_rotation.inverse() *
- imu_to_camera_rotation_;
-
- const Affine3s imu_to_world =
- board_to_world_ * board_to_camera.inverse() * imu_to_camera;
-
- const Eigen::Matrix<Scalar, 3, 1> z =
- imu_to_world * Eigen::Matrix<Scalar, 3, 1>::Zero();
-
- Eigen::Matrix<Scalar, 3, 6> H = Eigen::Matrix<Scalar, 3, 6>::Zero();
- H(0, 0) = static_cast<Scalar>(1.0);
- H(1, 1) = static_cast<Scalar>(1.0);
- H(2, 2) = static_cast<Scalar>(1.0);
- const Eigen::Matrix<Scalar, 3, 1> y = z - H * x_hat_;
-
- const Eigen::Matrix<double, 3, 3> R =
- (::Eigen::DiagonalMatrix<double, 3>().diagonal() << ::std::pow(0.01, 2),
- ::std::pow(0.01, 2), ::std::pow(0.01, 2))
- .finished()
- .asDiagonal();
-
- const Eigen::Matrix<Scalar, 3, 3> S =
- H * p_ * H.transpose() + R.cast<Scalar>();
- const Eigen::Matrix<Scalar, 6, 3> K = p_ * H.transpose() * S.inverse();
-
- x_hat_ += K * y;
- p_ = (Eigen::Matrix<Scalar, 6, 6>::Identity() - K * H) * p_;
-
- const Eigen::Quaternion<Scalar> error(imu_to_world_rotation.inverse() *
- orientation());
-
- errors_.emplace_back(
- Eigen::Matrix<Scalar, 3, 1>(error.x(), error.y(), error.z()));
- position_errors_.emplace_back(y);
-
- ObserveCameraUpdate(t, rt.first, imu_to_world_rotation, imu_to_world);
- }
-
- virtual void ObserveIMUUpdate(
- distributed_clock::time_point /*t*/,
- std::pair<Eigen::Vector3d, Eigen::Vector3d> /*wa*/) {}
-
- void UpdateIMU(distributed_clock::time_point t,
- std::pair<Eigen::Vector3d, Eigen::Vector3d> wa) override {
- Integrate(t);
- omega_ = wa.first;
- accel_ = wa.second;
-
- ObserveIMUUpdate(t, wa);
- }
-
- const Eigen::Quaternion<Scalar> &orientation() const { return orientation_; }
-
- size_t num_errors() const { return errors_.size(); }
- Scalar errorx(size_t i) const { return errors_[i].x(); }
- Scalar errory(size_t i) const { return errors_[i].y(); }
- Scalar errorz(size_t i) const { return errors_[i].z(); }
-
- size_t num_perrors() const { return position_errors_.size(); }
- Scalar errorpx(size_t i) const { return position_errors_[i].x(); }
- Scalar errorpy(size_t i) const { return position_errors_[i].y(); }
- Scalar errorpz(size_t i) const { return position_errors_[i].z(); }
-
- private:
- Eigen::Matrix<Scalar, 46, 1> Pack(Eigen::Quaternion<Scalar> q,
- Eigen::Matrix<Scalar, 6, 1> x_hat,
- Eigen::Matrix<Scalar, 6, 6> p) {
- Eigen::Matrix<Scalar, 46, 1> result = Eigen::Matrix<Scalar, 46, 1>::Zero();
- result.template block<4, 1>(0, 0) = q.coeffs();
- result.template block<6, 1>(4, 0) = x_hat;
- result.template block<36, 1>(10, 0) =
- Eigen::Map<Eigen::Matrix<Scalar, 36, 1>>(p.data(), p.size());
-
- return result;
- }
-
- std::tuple<Eigen::Quaternion<Scalar>, Eigen::Matrix<Scalar, 6, 1>,
- Eigen::Matrix<Scalar, 6, 6>>
- UnPack(Eigen::Matrix<Scalar, 46, 1> input) {
- Eigen::Quaternion<Scalar> q(input.template block<4, 1>(0, 0));
- Eigen::Matrix<Scalar, 6, 1> x_hat(input.template block<6, 1>(4, 0));
- Eigen::Matrix<Scalar, 6, 6> p =
- Eigen::Map<Eigen::Matrix<Scalar, 6, 6>>(input.data() + 10, 6, 6);
- return std::make_tuple(q, x_hat, p);
- }
-
- Eigen::Matrix<Scalar, 46, 1> Derivative(
- const Eigen::Matrix<Scalar, 46, 1> &input) {
- auto [q, x_hat, p] = UnPack(input);
-
- Eigen::Quaternion<Scalar> omega_q;
- omega_q.w() = Scalar(0.0);
- omega_q.vec() = 0.5 * (omega_.cast<Scalar>() - imu_bias_);
- Eigen::Matrix<Scalar, 4, 1> q_dot = (q * omega_q).coeffs();
-
- Eigen::Matrix<double, 6, 6> A = Eigen::Matrix<double, 6, 6>::Zero();
- A(0, 3) = 1.0;
- A(1, 4) = 1.0;
- A(2, 5) = 1.0;
-
- Eigen::Matrix<Scalar, 6, 1> x_hat_dot = A * x_hat;
- x_hat_dot.template block<3, 1>(3, 0) =
- orientation() * (accel_.cast<Scalar>() - accelerometer_bias_) -
- Eigen::Vector3d(0, 0, kGravity).cast<Scalar>() * gravity_scalar_;
-
- // Initialize the position noise to 0. If the solver is going to back-solve
- // for the most likely starting position, let's just say that the noise is
- // small.
- constexpr double kPositionNoise = 0.0;
- constexpr double kAccelerometerNoise = 2.3e-6 * 9.8;
- constexpr double kIMUdt = 5.0e-4;
- Eigen::Matrix<double, 6, 6> Q_dot(
- (::Eigen::DiagonalMatrix<double, 6>().diagonal()
- << ::std::pow(kPositionNoise, 2) / kIMUdt,
- ::std::pow(kPositionNoise, 2) / kIMUdt,
- ::std::pow(kPositionNoise, 2) / kIMUdt,
- ::std::pow(kAccelerometerNoise, 2) / kIMUdt,
- ::std::pow(kAccelerometerNoise, 2) / kIMUdt,
- ::std::pow(kAccelerometerNoise, 2) / kIMUdt)
- .finished()
- .asDiagonal());
- Eigen::Matrix<Scalar, 6, 6> p_dot = A.cast<Scalar>() * p +
- p * A.transpose().cast<Scalar>() +
- Q_dot.cast<Scalar>();
-
- return Pack(Eigen::Quaternion<Scalar>(q_dot), x_hat_dot, p_dot);
- }
-
- virtual void ObserveIntegrated(distributed_clock::time_point /*t*/,
- Eigen::Matrix<Scalar, 6, 1> /*x_hat*/,
- Eigen::Quaternion<Scalar> /*orientation*/,
- Eigen::Matrix<Scalar, 6, 6> /*p*/) {}
-
- void Integrate(distributed_clock::time_point t) {
- if (last_time_ != distributed_clock::min_time) {
- Eigen::Matrix<Scalar, 46, 1> next = control_loops::RungeKutta(
- [this](auto r) { return Derivative(r); },
- Pack(orientation_, x_hat_, p_),
- aos::time::DurationInSeconds(t - last_time_));
-
- std::tie(orientation_, x_hat_, p_) = UnPack(next);
-
- // Normalize q so it doesn't drift.
- orientation_.normalize();
- }
-
- last_time_ = t;
- ObserveIntegrated(t, x_hat_, orientation_, p_);
- }
-
- Eigen::Matrix<double, 3, 1> accel_;
- Eigen::Matrix<double, 3, 1> omega_;
- Eigen::Matrix<Scalar, 3, 1> imu_bias_;
-
- // IMU -> world quaternion
- Eigen::Quaternion<Scalar> orientation_;
- Eigen::Matrix<Scalar, 6, 1> x_hat_;
- Eigen::Matrix<Scalar, 6, 6> p_;
- distributed_clock::time_point last_time_ = distributed_clock::min_time;
-
- Eigen::Quaternion<Scalar> imu_to_camera_rotation_;
- Eigen::Translation<Scalar, 3> imu_to_camera_translation_ =
- Eigen::Translation3d(0, 0, 0).cast<Scalar>();
-
- Eigen::Quaternion<Scalar> board_to_world_;
- Scalar gravity_scalar_;
- Eigen::Matrix<Scalar, 3, 1> accelerometer_bias_;
- // States:
- // xyz position
- // xyz velocity
- //
- // Inputs
- // xyz accel
- //
- // Measurement:
- // xyz position from camera.
- //
- // Since the gyro is so good, we can just solve for the bias and initial
- // position with the solver and see what it learns.
-
- // Returns the angular errors for each camera sample.
- std::vector<Eigen::Matrix<Scalar, 3, 1>> errors_;
- std::vector<Eigen::Matrix<Scalar, 3, 1>> position_errors_;
-};
-
-// Subclass of the filter above which has plotting. This keeps debug code and
-// actual code separate.
-class PoseFilter : public CeresPoseFilter<double> {
- public:
- PoseFilter(Eigen::Quaternion<double> initial_orientation,
- Eigen::Quaternion<double> imu_to_camera,
- Eigen::Matrix<double, 3, 1> gyro_bias,
- Eigen::Matrix<double, 6, 1> initial_state,
- Eigen::Quaternion<double> board_to_world,
- Eigen::Matrix<double, 3, 1> imu_to_camera_translation,
- double gravity_scalar,
- Eigen::Matrix<double, 3, 1> accelerometer_bias)
- : CeresPoseFilter<double>(initial_orientation, imu_to_camera, gyro_bias,
- initial_state, board_to_world,
- imu_to_camera_translation, gravity_scalar,
- accelerometer_bias) {}
-
- void Plot() {
- std::vector<double> rx;
- std::vector<double> ry;
- std::vector<double> rz;
- std::vector<double> x;
- std::vector<double> y;
- std::vector<double> z;
- std::vector<double> vx;
- std::vector<double> vy;
- std::vector<double> vz;
- for (const Eigen::Quaternion<double> &q : orientations_) {
- Eigen::Matrix<double, 3, 1> rotation_vector =
- frc971::controls::ToRotationVectorFromQuaternion(q);
- rx.emplace_back(rotation_vector(0, 0));
- ry.emplace_back(rotation_vector(1, 0));
- rz.emplace_back(rotation_vector(2, 0));
- }
- for (const Eigen::Matrix<double, 6, 1> &x_hat : x_hats_) {
- x.emplace_back(x_hat(0));
- y.emplace_back(x_hat(1));
- z.emplace_back(x_hat(2));
- vx.emplace_back(x_hat(3));
- vy.emplace_back(x_hat(4));
- vz.emplace_back(x_hat(5));
- }
-
- frc971::analysis::Plotter plotter;
- plotter.AddFigure("position");
- plotter.AddLine(times_, rx, "x_hat(0)");
- plotter.AddLine(times_, ry, "x_hat(1)");
- plotter.AddLine(times_, rz, "x_hat(2)");
- plotter.AddLine(ct, cx, "Camera x");
- plotter.AddLine(ct, cy, "Camera y");
- plotter.AddLine(ct, cz, "Camera z");
- plotter.AddLine(ct, cerrx, "Camera error x");
- plotter.AddLine(ct, cerry, "Camera error y");
- plotter.AddLine(ct, cerrz, "Camera error z");
- plotter.Publish();
-
- plotter.AddFigure("error");
- plotter.AddLine(times_, rx, "x_hat(0)");
- plotter.AddLine(times_, ry, "x_hat(1)");
- plotter.AddLine(times_, rz, "x_hat(2)");
- plotter.AddLine(ct, cerrx, "Camera error x");
- plotter.AddLine(ct, cerry, "Camera error y");
- plotter.AddLine(ct, cerrz, "Camera error z");
- plotter.Publish();
-
- plotter.AddFigure("imu");
- plotter.AddLine(ct, world_gravity_x, "world_gravity(0)");
- plotter.AddLine(ct, world_gravity_y, "world_gravity(1)");
- plotter.AddLine(ct, world_gravity_z, "world_gravity(2)");
- plotter.AddLine(imut, imu_x, "imu x");
- plotter.AddLine(imut, imu_y, "imu y");
- plotter.AddLine(imut, imu_z, "imu z");
- plotter.AddLine(times_, rx, "rotation x");
- plotter.AddLine(times_, ry, "rotation y");
- plotter.AddLine(times_, rz, "rotation z");
- plotter.Publish();
-
- plotter.AddFigure("raw");
- plotter.AddLine(imut, imu_x, "imu x");
- plotter.AddLine(imut, imu_y, "imu y");
- plotter.AddLine(imut, imu_z, "imu z");
- plotter.AddLine(imut, imu_ratex, "omega x");
- plotter.AddLine(imut, imu_ratey, "omega y");
- plotter.AddLine(imut, imu_ratez, "omega z");
- plotter.AddLine(ct, raw_cx, "Camera x");
- plotter.AddLine(ct, raw_cy, "Camera y");
- plotter.AddLine(ct, raw_cz, "Camera z");
- plotter.Publish();
-
- plotter.AddFigure("xyz vel");
- plotter.AddLine(times_, x, "x");
- plotter.AddLine(times_, y, "y");
- plotter.AddLine(times_, z, "z");
- plotter.AddLine(times_, vx, "vx");
- plotter.AddLine(times_, vy, "vy");
- plotter.AddLine(times_, vz, "vz");
- plotter.AddLine(ct, camera_position_x, "Camera x");
- plotter.AddLine(ct, camera_position_y, "Camera y");
- plotter.AddLine(ct, camera_position_z, "Camera z");
- plotter.Publish();
-
- plotter.Spin();
- }
-
- void ObserveIntegrated(distributed_clock::time_point t,
- Eigen::Matrix<double, 6, 1> x_hat,
- Eigen::Quaternion<double> orientation,
- Eigen::Matrix<double, 6, 6> p) override {
- VLOG(1) << t << " -> " << p;
- VLOG(1) << t << " xhat -> " << x_hat.transpose();
- times_.emplace_back(chrono::duration<double>(t.time_since_epoch()).count());
- x_hats_.emplace_back(x_hat);
- orientations_.emplace_back(orientation);
- }
-
- void ObserveIMUUpdate(
- distributed_clock::time_point t,
- std::pair<Eigen::Vector3d, Eigen::Vector3d> wa) override {
- imut.emplace_back(chrono::duration<double>(t.time_since_epoch()).count());
- imu_ratex.emplace_back(wa.first.x());
- imu_ratey.emplace_back(wa.first.y());
- imu_ratez.emplace_back(wa.first.z());
- imu_x.emplace_back(wa.second.x());
- imu_y.emplace_back(wa.second.y());
- imu_z.emplace_back(wa.second.z());
-
- last_accel_ = wa.second;
- }
-
- void ObserveCameraUpdate(distributed_clock::time_point t,
- Eigen::Vector3d board_to_camera_rotation,
- Eigen::Quaternion<double> imu_to_world_rotation,
- Eigen::Affine3d imu_to_world) override {
- raw_cx.emplace_back(board_to_camera_rotation(0, 0));
- raw_cy.emplace_back(board_to_camera_rotation(1, 0));
- raw_cz.emplace_back(board_to_camera_rotation(2, 0));
-
- Eigen::Matrix<double, 3, 1> rotation_vector =
- frc971::controls::ToRotationVectorFromQuaternion(imu_to_world_rotation);
- ct.emplace_back(chrono::duration<double>(t.time_since_epoch()).count());
-
- Eigen::Matrix<double, 3, 1> cerr =
- frc971::controls::ToRotationVectorFromQuaternion(
- imu_to_world_rotation.inverse() * orientation());
-
- cx.emplace_back(rotation_vector(0, 0));
- cy.emplace_back(rotation_vector(1, 0));
- cz.emplace_back(rotation_vector(2, 0));
-
- cerrx.emplace_back(cerr(0, 0));
- cerry.emplace_back(cerr(1, 0));
- cerrz.emplace_back(cerr(2, 0));
-
- const Eigen::Vector3d world_gravity =
- imu_to_world_rotation * last_accel_ -
- Eigen::Vector3d(0, 0, kGravity) * gravity_scalar();
-
- const Eigen::Vector3d camera_position =
- imu_to_world * Eigen::Vector3d::Zero();
-
- world_gravity_x.emplace_back(world_gravity.x());
- world_gravity_y.emplace_back(world_gravity.y());
- world_gravity_z.emplace_back(world_gravity.z());
-
- camera_position_x.emplace_back(camera_position.x());
- camera_position_y.emplace_back(camera_position.y());
- camera_position_z.emplace_back(camera_position.z());
- }
-
- std::vector<double> ct;
- std::vector<double> cx;
- std::vector<double> cy;
- std::vector<double> cz;
- std::vector<double> raw_cx;
- std::vector<double> raw_cy;
- std::vector<double> raw_cz;
- std::vector<double> cerrx;
- std::vector<double> cerry;
- std::vector<double> cerrz;
-
- std::vector<double> world_gravity_x;
- std::vector<double> world_gravity_y;
- std::vector<double> world_gravity_z;
- std::vector<double> imu_x;
- std::vector<double> imu_y;
- std::vector<double> imu_z;
- std::vector<double> camera_position_x;
- std::vector<double> camera_position_y;
- std::vector<double> camera_position_z;
-
- std::vector<double> imut;
- std::vector<double> imu_ratex;
- std::vector<double> imu_ratey;
- std::vector<double> imu_ratez;
-
- std::vector<double> times_;
- std::vector<Eigen::Matrix<double, 6, 1>> x_hats_;
- std::vector<Eigen::Quaternion<double>> orientations_;
-
- Eigen::Matrix<double, 3, 1> last_accel_ = Eigen::Matrix<double, 3, 1>::Zero();
-};
-
-// Adapter class from the KF above to a Ceres cost function.
-struct CostFunctor {
- CostFunctor(CalibrationData *d) : data(d) {}
-
- CalibrationData *data;
-
- template <typename S>
- bool operator()(const S *const q1, const S *const q2, const S *const v,
- const S *const p, const S *const btw, const S *const itc,
- const S *const gravity_scalar_ptr,
- const S *const accelerometer_bias_ptr, S *residual) const {
- Eigen::Quaternion<S> initial_orientation(q1[3], q1[0], q1[1], q1[2]);
- Eigen::Quaternion<S> mounting_orientation(q2[3], q2[0], q2[1], q2[2]);
- Eigen::Quaternion<S> board_to_world(btw[3], btw[0], btw[1], btw[2]);
- Eigen::Matrix<S, 3, 1> gyro_bias(v[0], v[1], v[2]);
- Eigen::Matrix<S, 6, 1> initial_state;
- initial_state(0) = p[0];
- initial_state(1) = p[1];
- initial_state(2) = p[2];
- initial_state(3) = p[3];
- initial_state(4) = p[4];
- initial_state(5) = p[5];
- Eigen::Matrix<S, 3, 1> imu_to_camera_translation(itc[0], itc[1], itc[2]);
- Eigen::Matrix<S, 3, 1> accelerometer_bias(accelerometer_bias_ptr[0],
- accelerometer_bias_ptr[1],
- accelerometer_bias_ptr[2]);
-
- CeresPoseFilter<S> filter(initial_orientation, mounting_orientation,
- gyro_bias, initial_state, board_to_world,
- imu_to_camera_translation, *gravity_scalar_ptr,
- accelerometer_bias);
- data->ReviewData(&filter);
-
- for (size_t i = 0; i < filter.num_errors(); ++i) {
- residual[3 * i + 0] = filter.errorx(i);
- residual[3 * i + 1] = filter.errory(i);
- residual[3 * i + 2] = filter.errorz(i);
- }
-
- for (size_t i = 0; i < filter.num_perrors(); ++i) {
- residual[3 * filter.num_errors() + 3 * i + 0] = filter.errorpx(i);
- residual[3 * filter.num_errors() + 3 * i + 1] = filter.errorpy(i);
- residual[3 * filter.num_errors() + 3 * i + 2] = filter.errorpz(i);
- }
-
- return true;
- }
-};
-
void Main(int argc, char **argv) {
CalibrationData data;
@@ -604,104 +76,30 @@
const Eigen::Quaternion<double> nominal_board_to_world(
Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()));
- Eigen::Quaternion<double> initial_orientation = nominal_initial_orientation;
- // Eigen::Quaternion<double>::Identity();
- Eigen::Quaternion<double> imu_to_camera = nominal_imu_to_camera;
- // Eigen::Quaternion<double>::Identity();
- Eigen::Quaternion<double> board_to_world = nominal_board_to_world;
- // Eigen::Quaternion<double>::Identity();
- Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero();
- Eigen::Matrix<double, 6, 1> initial_state =
- Eigen::Matrix<double, 6, 1>::Zero();
- Eigen::Matrix<double, 3, 1> imu_to_camera_translation =
- Eigen::Matrix<double, 3, 1>::Zero();
+ CalibrationParameters calibration_parameters;
+ calibration_parameters.initial_orientation = nominal_initial_orientation;
+ calibration_parameters.imu_to_camera = nominal_imu_to_camera;
+ calibration_parameters.board_to_world = nominal_board_to_world;
- double gravity_scalar = 1.0;
- Eigen::Matrix<double, 3, 1> accelerometer_bias =
- Eigen::Matrix<double, 3, 1>::Zero();
+ Solve(data, &calibration_parameters);
+ LOG(INFO) << "Nominal initial_orientation "
+ << nominal_initial_orientation.coeffs().transpose();
+ LOG(INFO) << "Nominal imu_to_camera "
+ << nominal_imu_to_camera.coeffs().transpose();
- {
- ceres::Problem problem;
+ LOG(INFO) << "imu_to_camera delta "
+ << frc971::controls::ToRotationVectorFromQuaternion(
+ calibration_parameters.imu_to_camera *
+ nominal_imu_to_camera.inverse())
+ .transpose();
+ LOG(INFO) << "board_to_world delta "
+ << frc971::controls::ToRotationVectorFromQuaternion(
+ calibration_parameters.board_to_world *
+ nominal_board_to_world.inverse())
+ .transpose();
- ceres::EigenQuaternionParameterization *quaternion_local_parameterization =
- new ceres::EigenQuaternionParameterization();
- // Set up the only cost function (also known as residual). This uses
- // auto-differentiation to obtain the derivative (jacobian).
-
- ceres::CostFunction *cost_function =
- new ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 4, 4, 3, 6,
- 4, 3, 1, 3>(
- new CostFunctor(&data), data.camera_samples_size() * 6);
- problem.AddResidualBlock(
- cost_function, new ceres::HuberLoss(1.0),
- initial_orientation.coeffs().data(), imu_to_camera.coeffs().data(),
- gyro_bias.data(), initial_state.data(), board_to_world.coeffs().data(),
- imu_to_camera_translation.data(), &gravity_scalar,
- accelerometer_bias.data());
- problem.SetParameterization(initial_orientation.coeffs().data(),
- quaternion_local_parameterization);
- problem.SetParameterization(imu_to_camera.coeffs().data(),
- quaternion_local_parameterization);
- problem.SetParameterization(board_to_world.coeffs().data(),
- quaternion_local_parameterization);
- for (int i = 0; i < 3; ++i) {
- problem.SetParameterLowerBound(gyro_bias.data(), i, -0.05);
- problem.SetParameterUpperBound(gyro_bias.data(), i, 0.05);
- problem.SetParameterLowerBound(accelerometer_bias.data(), i, -0.05);
- problem.SetParameterUpperBound(accelerometer_bias.data(), i, 0.05);
- }
- problem.SetParameterLowerBound(&gravity_scalar, 0, 0.95);
- problem.SetParameterUpperBound(&gravity_scalar, 0, 1.05);
-
- // Run the solver!
- ceres::Solver::Options options;
- options.minimizer_progress_to_stdout = true;
- options.gradient_tolerance = 1e-12;
- options.function_tolerance = 1e-16;
- options.parameter_tolerance = 1e-12;
- ceres::Solver::Summary summary;
- Solve(options, &problem, &summary);
- LOG(INFO) << summary.FullReport();
-
- LOG(INFO) << "Nominal initial_orientation "
- << nominal_initial_orientation.coeffs().transpose();
- LOG(INFO) << "Nominal imu_to_camera "
- << nominal_imu_to_camera.coeffs().transpose();
-
- LOG(INFO) << "initial_orientation "
- << initial_orientation.coeffs().transpose();
- LOG(INFO) << "imu_to_camera " << imu_to_camera.coeffs().transpose();
- LOG(INFO) << "imu_to_camera(rotation) "
- << frc971::controls::ToRotationVectorFromQuaternion(imu_to_camera)
- .transpose();
- LOG(INFO) << "imu_to_camera delta "
- << frc971::controls::ToRotationVectorFromQuaternion(
- imu_to_camera * nominal_imu_to_camera.inverse())
- .transpose();
- LOG(INFO) << "gyro_bias " << gyro_bias.transpose();
- LOG(INFO) << "board_to_world " << board_to_world.coeffs().transpose();
- LOG(INFO) << "board_to_world(rotation) "
- << frc971::controls::ToRotationVectorFromQuaternion(
- board_to_world)
- .transpose();
- LOG(INFO) << "board_to_world delta "
- << frc971::controls::ToRotationVectorFromQuaternion(
- board_to_world * nominal_board_to_world.inverse())
- .transpose();
- LOG(INFO) << "imu_to_camera_translation "
- << imu_to_camera_translation.transpose();
- LOG(INFO) << "gravity " << kGravity * gravity_scalar;
- LOG(INFO) << "accelerometer bias " << accelerometer_bias.transpose();
- }
-
- {
- PoseFilter filter(initial_orientation, imu_to_camera, gyro_bias,
- initial_state, board_to_world, imu_to_camera_translation,
- gravity_scalar, accelerometer_bias);
- data.ReviewData(&filter);
- if (FLAGS_plot) {
- filter.Plot();
- }
+ if (FLAGS_plot) {
+ Plot(data, calibration_parameters);
}
}