Add support for solving for turrets
Optionally, use turrent positions to apply a rotation around Z to model
the turret. We then need to introduce a rotation from IMU frame to
turret frame (right before the bearing), and a translation as well.
This lets us represent the turret motion.
This is actually underconstrained. We can pick arbitrary turret pivot
points along the turret rotation axis without changing the solution, and
can arbitrarily rotate 0 on the turret the same way. Freeze Z in the
translation and apply a cost to the Z component of the rotation from IMU
to the turret frame to define these.
Change-Id: I3fed35a53e8e77e2c9feca9f5c6c3896b22c6277
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/vision/calibration_accumulator.cc b/frc971/vision/calibration_accumulator.cc
index 89945b0..ac1946c 100644
--- a/frc971/vision/calibration_accumulator.cc
+++ b/frc971/vision/calibration_accumulator.cc
@@ -38,6 +38,18 @@
imu_points_.emplace_back(distributed_now, std::make_pair(gyro, accel));
}
+void CalibrationData::AddTurret(
+ aos::distributed_clock::time_point distributed_now, Eigen::Vector2d state) {
+ // We want the turret to be known too when solving. But, we don't know if we
+ // are going to have a turret until we get the first reading. In that case,
+ // blow away any camera readings from before.
+ while (!rot_trans_points_.empty() &&
+ rot_trans_points_[0].first < distributed_now) {
+ rot_trans_points_.erase(rot_trans_points_.begin());
+ }
+ turret_points_.emplace_back(distributed_now, state);
+}
+
void CalibrationData::ReviewData(CalibrationDataObserver *observer) const {
size_t next_imu_point = 0;
size_t next_camera_point = 0;
diff --git a/frc971/vision/calibration_accumulator.h b/frc971/vision/calibration_accumulator.h
index 6d42708..e4f9c8a 100644
--- a/frc971/vision/calibration_accumulator.h
+++ b/frc971/vision/calibration_accumulator.h
@@ -26,6 +26,11 @@
// 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;
+
+ // Observes a turret sample at the corresponding time t, and with the
+ // corresponding state.
+ virtual void UpdateTurret(aos::distributed_clock::time_point t,
+ Eigen::Vector2d state) = 0;
};
// Class to both accumulate and replay camera and IMU data in time order.
@@ -40,12 +45,19 @@
void AddImu(aos::distributed_clock::time_point distributed_now,
Eigen::Vector3d gyro, Eigen::Vector3d accel);
+ // Adds a turret reading (position; velocity) to the list at the provided
+ // time.
+ void AddTurret(aos::distributed_clock::time_point distributed_now,
+ Eigen::Vector2d state);
+
// Processes the data points by calling UpdateCamera and UpdateIMU in time
// order.
void ReviewData(CalibrationDataObserver *observer) const;
size_t camera_samples_size() const { return rot_trans_points_.size(); }
+ size_t turret_samples() const { return turret_points_.size(); }
+
private:
std::vector<std::pair<aos::distributed_clock::time_point,
std::pair<Eigen::Vector3d, Eigen::Vector3d>>>
@@ -56,6 +68,10 @@
std::vector<std::pair<aos::distributed_clock::time_point,
std::pair<Eigen::Vector3d, Eigen::Vector3d>>>
rot_trans_points_;
+
+ // Turret state as a timestamp and [x, v].
+ std::vector<std::pair<aos::distributed_clock::time_point, Eigen::Vector2d>>
+ turret_points_;
};
// Class to register image and IMU callbacks in AOS and route them to the
diff --git a/frc971/vision/extrinsics_calibration.cc b/frc971/vision/extrinsics_calibration.cc
index df2c4b9..a71f14d 100644
--- a/frc971/vision/extrinsics_calibration.cc
+++ b/frc971/vision/extrinsics_calibration.cc
@@ -51,11 +51,13 @@
typedef Eigen::Transform<Scalar, 3, Eigen::Affine> Affine3s;
CeresPoseFilter(Eigen::Quaternion<Scalar> initial_orientation,
- Eigen::Quaternion<Scalar> imu_to_camera,
+ Eigen::Quaternion<Scalar> pivot_to_camera,
+ Eigen::Quaternion<Scalar> pivot_to_imu,
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,
+ Eigen::Matrix<Scalar, 3, 1> pivot_to_camera_translation,
+ Eigen::Matrix<Scalar, 3, 1> pivot_to_imu_translation,
Scalar gravity_scalar,
Eigen::Matrix<Scalar, 3, 1> accelerometer_bias)
: accel_(Eigen::Matrix<double, 3, 1>::Zero()),
@@ -64,8 +66,10 @@
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),
+ pivot_to_camera_rotation_(pivot_to_camera),
+ pivot_to_camera_translation_(pivot_to_camera_translation),
+ pivot_to_imu_rotation_(pivot_to_imu),
+ pivot_to_imu_translation_(pivot_to_imu_translation),
board_to_world_(board_to_world),
gravity_scalar_(gravity_scalar),
accelerometer_bias_(accelerometer_bias) {}
@@ -78,12 +82,27 @@
Eigen::Quaternion<Scalar> /*imu_to_world_rotation*/,
Affine3s /*imu_to_world*/) {}
+ void UpdateTurret(distributed_clock::time_point t,
+ Eigen::Vector2d state) override {
+ state_ = state;
+ state_time_ = t;
+ }
+
+ Eigen::Vector2d state_ = Eigen::Vector2d::Zero();
+ distributed_clock::time_point state_time_ = distributed_clock::min_time;
+
// 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 double pivot_angle =
+ state_time_ == distributed_clock::min_time
+ ? 0.0
+ : state_(0) +
+ state_(1) * chrono::duration<double>(t - state_time_).count();
+
const Eigen::Quaternion<Scalar> board_to_camera_rotation(
frc971::controls::ToQuaternionFromRotationVector(rt.first)
.cast<Scalar>());
@@ -91,8 +110,10 @@
Eigen::Translation3d(rt.second).cast<Scalar>() *
board_to_camera_rotation;
- const Affine3s imu_to_camera =
- imu_to_camera_translation_ * imu_to_camera_rotation_;
+ const Affine3s pivot_to_camera =
+ pivot_to_camera_translation_ * pivot_to_camera_rotation_;
+ const Affine3s pivot_to_imu =
+ pivot_to_imu_translation_ * pivot_to_imu_rotation_;
// This converts us from (facing the board),
// x right, y up, z towards us -> x right, y away, z up.
@@ -102,10 +123,16 @@
// world <- board <- camera <- imu.
const Eigen::Quaternion<Scalar> imu_to_world_rotation =
board_to_world_ * board_to_camera_rotation.inverse() *
- imu_to_camera_rotation_;
+ pivot_to_camera_rotation_ *
+ Eigen::AngleAxis<Scalar>(static_cast<Scalar>(-pivot_angle),
+ Eigen::Vector3d::UnitZ().cast<Scalar>()) *
+ pivot_to_imu_rotation_.inverse();
const Affine3s imu_to_world =
- board_to_world_ * board_to_camera.inverse() * imu_to_camera;
+ board_to_world_ * board_to_camera.inverse() * pivot_to_camera *
+ Eigen::AngleAxis<Scalar>(static_cast<Scalar>(-pivot_angle),
+ Eigen::Vector3d::UnitZ().cast<Scalar>()) *
+ pivot_to_imu.inverse();
const Eigen::Matrix<Scalar, 3, 1> z =
imu_to_world * Eigen::Matrix<Scalar, 3, 1>::Zero();
@@ -261,8 +288,12 @@
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::Quaternion<Scalar> pivot_to_camera_rotation_;
+ Eigen::Translation<Scalar, 3> pivot_to_camera_translation_ =
+ Eigen::Translation3d(0, 0, 0).cast<Scalar>();
+
+ Eigen::Quaternion<Scalar> pivot_to_imu_rotation_;
+ Eigen::Translation<Scalar, 3> pivot_to_imu_translation_ =
Eigen::Translation3d(0, 0, 0).cast<Scalar>();
Eigen::Quaternion<Scalar> board_to_world_;
@@ -286,22 +317,35 @@
std::vector<Eigen::Matrix<Scalar, 3, 1>> position_errors_;
};
+// Drives the Z coordinate of the quaternion to 0.
+struct PenalizeQuaternionZ {
+ template <typename S>
+ bool operator()(const S *const pivot_to_imu_ptr, S *residual) const {
+ Eigen::Quaternion<S> pivot_to_imu(pivot_to_imu_ptr[3], pivot_to_imu_ptr[0],
+ pivot_to_imu_ptr[1], pivot_to_imu_ptr[2]);
+ residual[0] = pivot_to_imu.z();
+ return true;
+ }
+};
+
// 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::Quaternion<double> pivot_to_camera,
+ Eigen::Quaternion<double> pivot_to_imu,
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,
+ Eigen::Matrix<double, 3, 1> pivot_to_camera_translation,
+ Eigen::Matrix<double, 3, 1> pivot_to_imu_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) {}
+ : CeresPoseFilter<double>(
+ initial_orientation, pivot_to_camera, pivot_to_imu, gyro_bias,
+ initial_state, board_to_world, pivot_to_camera_translation,
+ pivot_to_imu_translation, gravity_scalar, accelerometer_bias) {}
void Plot() {
std::vector<double> rx;
@@ -496,30 +540,49 @@
const 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,
+ bool operator()(const S *const initial_orientation_ptr,
+ const S *const pivot_to_camera_ptr,
+ const S *const pivot_to_imu_ptr, const S *const gyro_bias_ptr,
+ const S *const initial_state_ptr,
+ const S *const board_to_world_ptr,
+ const S *const pivot_to_camera_translation_ptr,
+ const S *const pivot_to_imu_translation_ptr,
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::Quaternion<S> initial_orientation(
+ initial_orientation_ptr[3], initial_orientation_ptr[0],
+ initial_orientation_ptr[1], initial_orientation_ptr[2]);
+ Eigen::Quaternion<S> pivot_to_camera(
+ pivot_to_camera_ptr[3], pivot_to_camera_ptr[0], pivot_to_camera_ptr[1],
+ pivot_to_camera_ptr[2]);
+ Eigen::Quaternion<S> pivot_to_imu(pivot_to_imu_ptr[3], pivot_to_imu_ptr[0],
+ pivot_to_imu_ptr[1], pivot_to_imu_ptr[2]);
+ Eigen::Quaternion<S> board_to_world(
+ board_to_world_ptr[3], board_to_world_ptr[0], board_to_world_ptr[1],
+ board_to_world_ptr[2]);
+ Eigen::Matrix<S, 3, 1> gyro_bias(gyro_bias_ptr[0], gyro_bias_ptr[1],
+ gyro_bias_ptr[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]);
+ initial_state(0) = initial_state_ptr[0];
+ initial_state(1) = initial_state_ptr[1];
+ initial_state(2) = initial_state_ptr[2];
+ initial_state(3) = initial_state_ptr[3];
+ initial_state(4) = initial_state_ptr[4];
+ initial_state(5) = initial_state_ptr[5];
+ Eigen::Matrix<S, 3, 1> pivot_to_camera_translation(
+ pivot_to_camera_translation_ptr[0], pivot_to_camera_translation_ptr[1],
+ pivot_to_camera_translation_ptr[2]);
+ Eigen::Matrix<S, 3, 1> pivot_to_imu_translation(
+ pivot_to_imu_translation_ptr[0], pivot_to_imu_translation_ptr[1],
+ pivot_to_imu_translation_ptr[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);
+ CeresPoseFilter<S> filter(
+ initial_orientation, pivot_to_camera, pivot_to_imu, gyro_bias,
+ initial_state, board_to_world, pivot_to_camera_translation,
+ pivot_to_imu_translation, *gravity_scalar_ptr, accelerometer_bias);
data->ReviewData(&filter);
for (size_t i = 0; i < filter.num_errors(); ++i) {
@@ -547,25 +610,51 @@
// 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),
- calibration_parameters->initial_orientation.coeffs().data(),
- calibration_parameters->imu_to_camera.coeffs().data(),
- calibration_parameters->gyro_bias.data(),
- calibration_parameters->initial_state.data(),
- calibration_parameters->board_to_world.coeffs().data(),
- calibration_parameters->imu_to_camera_translation.data(),
- &calibration_parameters->gravity_scalar,
- calibration_parameters->accelerometer_bias.data());
+ {
+ ceres::CostFunction *cost_function =
+ new ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 4, 4, 4, 3,
+ 6, 4, 3, 3, 1, 3>(
+ new CostFunctor(&data), data.camera_samples_size() * 6);
+ problem.AddResidualBlock(
+ cost_function, new ceres::HuberLoss(1.0),
+ calibration_parameters->initial_orientation.coeffs().data(),
+ calibration_parameters->pivot_to_camera.coeffs().data(),
+ calibration_parameters->pivot_to_imu.coeffs().data(),
+ calibration_parameters->gyro_bias.data(),
+ calibration_parameters->initial_state.data(),
+ calibration_parameters->board_to_world.coeffs().data(),
+ calibration_parameters->pivot_to_camera_translation.data(),
+ calibration_parameters->pivot_to_imu_translation.data(),
+ &calibration_parameters->gravity_scalar,
+ calibration_parameters->accelerometer_bias.data());
+ }
+
+ {
+ ceres::CostFunction *turret_z_cost_function =
+ new ceres::AutoDiffCostFunction<PenalizeQuaternionZ, 1, 4>(
+ new PenalizeQuaternionZ());
+ problem.AddResidualBlock(
+ turret_z_cost_function, nullptr,
+ calibration_parameters->pivot_to_imu.coeffs().data());
+ }
+
+ if (calibration_parameters->has_pivot) {
+ // Constrain Z.
+ problem.SetParameterization(
+ calibration_parameters->pivot_to_imu_translation.data(),
+ new ceres::SubsetParameterization(3, {2}));
+ } else {
+ problem.SetParameterBlockConstant(
+ calibration_parameters->pivot_to_imu.coeffs().data());
+ problem.SetParameterBlockConstant(
+ calibration_parameters->pivot_to_imu_translation.data());
+ }
+
problem.SetParameterization(
calibration_parameters->initial_orientation.coeffs().data(),
quaternion_local_parameterization);
problem.SetParameterization(
- calibration_parameters->imu_to_camera.coeffs().data(),
+ calibration_parameters->pivot_to_camera.coeffs().data(),
quaternion_local_parameterization);
problem.SetParameterization(
calibration_parameters->board_to_world.coeffs().data(),
@@ -597,11 +686,17 @@
LOG(INFO) << "initial_orientation "
<< calibration_parameters->initial_orientation.coeffs().transpose();
- LOG(INFO) << "imu_to_camera "
- << calibration_parameters->imu_to_camera.coeffs().transpose();
- LOG(INFO) << "imu_to_camera(rotation) "
+ LOG(INFO) << "pivot_to_imu "
+ << calibration_parameters->pivot_to_imu.coeffs().transpose();
+ LOG(INFO) << "pivot_to_imu(rotation) "
<< frc971::controls::ToRotationVectorFromQuaternion(
- calibration_parameters->imu_to_camera)
+ calibration_parameters->pivot_to_imu)
+ .transpose();
+ LOG(INFO) << "pivot_to_camera "
+ << calibration_parameters->pivot_to_camera.coeffs().transpose();
+ LOG(INFO) << "pivot_to_camera(rotation) "
+ << frc971::controls::ToRotationVectorFromQuaternion(
+ calibration_parameters->pivot_to_camera)
.transpose();
LOG(INFO) << "gyro_bias " << calibration_parameters->gyro_bias.transpose();
LOG(INFO) << "board_to_world "
@@ -610,8 +705,10 @@
<< frc971::controls::ToRotationVectorFromQuaternion(
calibration_parameters->board_to_world)
.transpose();
- LOG(INFO) << "imu_to_camera_translation "
- << calibration_parameters->imu_to_camera_translation.transpose();
+ LOG(INFO) << "pivot_to_imu_translation "
+ << calibration_parameters->pivot_to_imu_translation.transpose();
+ LOG(INFO) << "pivot_to_camera_translation "
+ << calibration_parameters->pivot_to_camera_translation.transpose();
LOG(INFO) << "gravity " << kGravity * calibration_parameters->gravity_scalar;
LOG(INFO) << "accelerometer bias "
<< calibration_parameters->accelerometer_bias.transpose();
@@ -620,11 +717,13 @@
void Plot(const CalibrationData &data,
const CalibrationParameters &calibration_parameters) {
PoseFilter filter(calibration_parameters.initial_orientation,
- calibration_parameters.imu_to_camera,
+ calibration_parameters.pivot_to_camera,
+ calibration_parameters.pivot_to_imu,
calibration_parameters.gyro_bias,
calibration_parameters.initial_state,
calibration_parameters.board_to_world,
- calibration_parameters.imu_to_camera_translation,
+ calibration_parameters.pivot_to_camera_translation,
+ calibration_parameters.pivot_to_imu_translation,
calibration_parameters.gravity_scalar,
calibration_parameters.accelerometer_bias);
data.ReviewData(&filter);
diff --git a/frc971/vision/extrinsics_calibration.h b/frc971/vision/extrinsics_calibration.h
index 9d6c704..b24f13f 100644
--- a/frc971/vision/extrinsics_calibration.h
+++ b/frc971/vision/extrinsics_calibration.h
@@ -11,7 +11,9 @@
struct CalibrationParameters {
Eigen::Quaternion<double> initial_orientation =
Eigen::Quaternion<double>::Identity();
- Eigen::Quaternion<double> imu_to_camera =
+ Eigen::Quaternion<double> pivot_to_camera =
+ Eigen::Quaternion<double>::Identity();
+ Eigen::Quaternion<double> pivot_to_imu =
Eigen::Quaternion<double>::Identity();
Eigen::Quaternion<double> board_to_world =
Eigen::Quaternion<double>::Identity();
@@ -19,17 +21,24 @@
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> pivot_to_camera_translation =
+ Eigen::Matrix<double, 3, 1>::Zero();
+ Eigen::Matrix<double, 3, 1> pivot_to_imu_translation =
Eigen::Matrix<double, 3, 1>::Zero();
double gravity_scalar = 1.0;
Eigen::Matrix<double, 3, 1> accelerometer_bias =
Eigen::Matrix<double, 3, 1>::Zero();
+
+ bool has_pivot = false;
};
+// Solves the mounting problem given the calibration data and parameters. The
+// parameters are used as the seed to the solver.
void Solve(const CalibrationData &data,
CalibrationParameters *calibration_parameters);
+// Plots the calibrated results to help visualize the fit.
void Plot(const CalibrationData &data,
const CalibrationParameters &calibration_parameters);
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index de85213..1fd48cf 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -128,6 +128,8 @@
deps = [
"//aos:init",
"//aos/events/logging:log_reader",
+ "//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/vision:extrinsics_calibration",
+ "//y2020/control_loops/superstructure:superstructure_status_fbs",
],
)
diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index c54a5cf..d0497e0 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -1,6 +1,7 @@
+#include "frc971/vision/extrinsics_calibration.h"
+
#include "Eigen/Dense"
#include "Eigen/Geometry"
-
#include "absl/strings/str_format.h"
#include "aos/events/logging/log_reader.h"
#include "aos/init.h"
@@ -9,8 +10,8 @@
#include "aos/util/file.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/control_loops/superstructure/superstructure_status_generated.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"
@@ -18,6 +19,7 @@
DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
DEFINE_string(pi, "pi-7971-2", "Pi name to calibrate.");
DEFINE_bool(plot, false, "Whether to plot the resulting data.");
+DEFINE_bool(turret, false, "If true, the camera is on the turret");
namespace frc971 {
namespace vision {
@@ -60,6 +62,21 @@
Calibration extractor(&factory, pi_event_loop.get(),
roborio_event_loop.get(), FLAGS_pi, &data);
+ if (FLAGS_turret) {
+ aos::NodeEventLoopFactory *roborio_factory =
+ factory.GetNodeEventLoopFactory(roborio_node->name()->string_view());
+ roborio_event_loop->MakeWatcher(
+ "/superstructure",
+ [roborio_factory, roborio_event_loop = roborio_event_loop.get(),
+ &data](const y2020::control_loops::superstructure::Status &status) {
+ data.AddTurret(
+ roborio_factory->ToDistributedClock(
+ roborio_event_loop->context().monotonic_event_time),
+ Eigen::Vector2d(status.turret()->position(),
+ status.turret()->velocity()));
+ });
+ }
+
factory.Run();
reader.Deregister();
@@ -71,26 +88,26 @@
const Eigen::Quaternion<double> nominal_initial_orientation(
frc971::controls::ToQuaternionFromRotationVector(
Eigen::Vector3d(0.0, 0.0, M_PI)));
- const Eigen::Quaternion<double> nominal_imu_to_camera(
+ const Eigen::Quaternion<double> nominal_pivot_to_camera(
Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitX()));
const Eigen::Quaternion<double> nominal_board_to_world(
Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()));
CalibrationParameters calibration_parameters;
calibration_parameters.initial_orientation = nominal_initial_orientation;
- calibration_parameters.imu_to_camera = nominal_imu_to_camera;
+ calibration_parameters.pivot_to_camera = nominal_pivot_to_camera;
calibration_parameters.board_to_world = nominal_board_to_world;
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();
+ LOG(INFO) << "Nominal pivot_to_camera "
+ << nominal_pivot_to_camera.coeffs().transpose();
- LOG(INFO) << "imu_to_camera delta "
+ LOG(INFO) << "pivot_to_camera delta "
<< frc971::controls::ToRotationVectorFromQuaternion(
- calibration_parameters.imu_to_camera *
- nominal_imu_to_camera.inverse())
+ calibration_parameters.pivot_to_camera *
+ nominal_pivot_to_camera.inverse())
.transpose();
LOG(INFO) << "board_to_world delta "
<< frc971::controls::ToRotationVectorFromQuaternion(