Convert extrinsics_calibration to an observer model
Rather than coupling the logic for estimation with extraction, add an
observer interface and put the filter there. This makes it much easier
conceptually to reprocess data multiple times.
Change-Id: I70c6f36d2c2069ce86a45a4859894188a470d003
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index b43829d..00dd791 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -164,6 +164,8 @@
cc_binary(
name = "extrinsics_calibration",
srcs = [
+ "calibration_accumulator.cc",
+ "calibration_accumulator.h",
"extrinsics_calibration.cc",
],
data = [
diff --git a/y2020/vision/calibration_accumulator.cc b/y2020/vision/calibration_accumulator.cc
new file mode 100644
index 0000000..e77c74b
--- /dev/null
+++ b/y2020/vision/calibration_accumulator.cc
@@ -0,0 +1,174 @@
+#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;
+
+void CalibrationData::AddCameraPose(
+ distributed_clock::time_point distributed_now, Eigen::Vector3d rvec,
+ Eigen::Vector3d tvec) {
+ 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);
+}
+
+} // namespace vision
+} // namespace frc971
diff --git a/y2020/vision/calibration_accumulator.h b/y2020/vision/calibration_accumulator.h
new file mode 100644
index 0000000..0f1bff7
--- /dev/null
+++ b/y2020/vision/calibration_accumulator.h
@@ -0,0 +1,89 @@
+#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 {
+
+class CalibrationDataObserver {
+ public:
+ virtual void UpdateCamera(aos::distributed_clock::time_point t,
+ std::pair<Eigen::Vector3d, Eigen::Vector3d> rt) = 0;
+
+ 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/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index e775e3e..be64efb 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -6,10 +6,7 @@
#include "Eigen/Dense"
#include "Eigen/Geometry"
-#include "frc971/wpilib/imu_batch_generated.h"
#include "absl/strings/str_format.h"
-#include "frc971/control_loops/quaternion_utils.h"
-#include "y2020/vision/charuco_lib.h"
#include "aos/events/logging/log_reader.h"
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
@@ -17,16 +14,17 @@
#include "aos/time/time.h"
#include "aos/util/file.h"
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "frc971/control_loops/quaternion_utils.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"
#include "y2020/vision/vision_generated.h"
-#include "y2020/vision/charuco_lib.h"
DEFINE_string(config, "config.json", "Path to the config file to use.");
DEFINE_string(pi, "pi-7971-2", "Pi name to calibrate.");
-DEFINE_bool(display_undistorted, false,
- "If true, display the undistorted image.");
namespace frc971 {
namespace vision {
@@ -34,74 +32,37 @@
using aos::distributed_clock;
using aos::monotonic_clock;
-// Class to both accumulate and replay camera and IMU data in time order.
-class CalibrationData {
+class PoseFilter : public CalibrationDataObserver {
public:
- CalibrationData()
- : x_hat_(Eigen::Matrix<double, 9, 1>::Zero()),
+ PoseFilter()
+ : accel_(Eigen::Matrix<double, 3, 1>::Zero()),
+ omega_(Eigen::Matrix<double, 3, 1>::Zero()),
+ x_hat_(Eigen::Matrix<double, 9, 1>::Zero()),
q_(Eigen::Matrix<double, 9, 9>::Zero()) {}
- // Adds an IMU point to the list at the provided time.
- void AddImu(distributed_clock::time_point distributed_now,
- Eigen::Vector3d gyro, Eigen::Vector3d accel) {
- imu_points_.emplace_back(distributed_now, std::make_pair(gyro, accel));
- }
-
- // Adds a camera/charuco detection to the list at the provided time.
- void AddCharuco(distributed_clock::time_point distributed_now,
- Eigen::Vector3d rvec, Eigen::Vector3d tvec) {
- rot_trans_points_.emplace_back(distributed_now, std::make_pair(rvec, tvec));
- }
-
- // Processes the data points by calling UpdateCamera and UpdateIMU in time
- // order.
- void ReviewData() {
- 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!
- UpdateCamera(rot_trans_points_[next_camera_point].first,
- rot_trans_points_[next_camera_point].second);
- ++next_camera_point;
- } else {
- // IMU!
- 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!
- 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;
- }
- }
- }
- }
- }
-
void UpdateCamera(distributed_clock::time_point t,
- std::pair<Eigen::Vector3d, Eigen::Vector3d> rt) {
+ std::pair<Eigen::Vector3d, Eigen::Vector3d> rt) override {
+ Integrate(t);
+ // TODO(austin): take the sample.
LOG(INFO) << t << " Camera " << rt.second.transpose();
}
void UpdateIMU(distributed_clock::time_point t,
- std::pair<Eigen::Vector3d, Eigen::Vector3d> wa) {
+ std::pair<Eigen::Vector3d, Eigen::Vector3d> wa) override {
+ Integrate(t);
+ omega_ = wa.first;
+ accel_ = wa.second;
LOG(INFO) << t << " IMU " << wa.first.transpose();
}
private:
- // TODO(austin): Actually use these. Or make a new "callback" object which has these.
+ void Integrate(distributed_clock::time_point t) { LOG(INFO) << t; }
+
+ Eigen::Matrix<double, 3, 1> accel_;
+ Eigen::Matrix<double, 3, 1> omega_;
+
+ // TODO(austin): Actually use these. Or make a new "callback" object which
+ // has these.
Eigen::Matrix<double, 9, 1> x_hat_;
Eigen::Matrix<double, 9, 9> q_;
@@ -118,160 +79,6 @@
// Measurement:
// xyz position
// orientation rotation vector
-
- std::vector<std::pair<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<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)
- : 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_event_loop_->MakeWatcher(
- "/drivetrain", [this](const frc971::IMUValuesBatch &imu) {
- if (!imu.has_readings()) {
- return;
- }
- for (const frc971::IMUValues *value : *imu.readings()) {
- HandleIMU(value);
- }
- });
- }
-
- // Processes a charuco detection.
- void 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) {
- Eigen::Quaternion<double> rotation(
- frc971::controls::ToQuaternionFromRotationVector(rvec_eigen));
- Eigen::Translation3d translation(tvec_eigen);
-
- const Eigen::Affine3d board_to_camera = translation * rotation;
- (void)board_to_camera;
-
- // 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.
-
- // LOG(INFO) << "rotation " << rotation.matrix();
- // LOG(INFO) << "translation " << translation.matrix();
- // 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);
-
- // For cameras 2 and 3...
- 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", "[", "]",
- "[", "]");
-
- LOG(INFO);
- LOG(INFO) << "World Gravity "
- << (board_to_world * rotation.inverse() * imu_to_camera * imu)
- .transpose()
- .format(HeavyFmt);
- LOG(INFO) << "Board Gravity "
- << (rotation.inverse() * imu_to_camera * imu)
- .transpose()
- .format(HeavyFmt);
- LOG(INFO) << "Camera Gravity "
- << (imu_to_camera * imu).transpose().format(HeavyFmt);
- LOG(INFO) << "IMU Gravity " << imu.transpose().format(HeavyFmt);
-
- 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);
-
- data_->AddCharuco(image_factory_->ToDistributedClock(eof), rvec_eigen,
- tvec_eigen);
- }
-
- 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);
- }
-
- int keystroke = cv::waitKey(1);
- if ((keystroke & 0xFF) == static_cast<int>('q')) {
- // image_event_loop_->Exit();
- }
- }
-
- // Processes an IMU reading.
- void 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);
- }
-
- frc971::IMUValuesT last_value_;
-
- private:
- aos::EventLoop *image_event_loop_;
- aos::NodeEventLoopFactory *image_factory_;
- aos::EventLoop *imu_event_loop_;
- aos::NodeEventLoopFactory *imu_factory_;
-
- CharucoExtractor charuco_extractor_;
-
- CalibrationData *data_;
};
void Main(int argc, char **argv) {
@@ -316,7 +123,11 @@
LOG(INFO) << "Done with event_loop running";
// And now we have it, we can start processing it.
- data.ReviewData();
+
+ {
+ PoseFilter filter;
+ data.ReviewData(&filter);
+ }
}
} // namespace vision