Solve for mounting position as well as angle
This gives us the full extrinsics of our camera!
Change-Id: Ia8c09fe6eefc361837fd7c525fb1fbaed6c4a184
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2020/vision/calibration_accumulator.cc b/y2020/vision/calibration_accumulator.cc
index e77c74b..9f550c5 100644
--- a/y2020/vision/calibration_accumulator.cc
+++ b/y2020/vision/calibration_accumulator.cc
@@ -22,10 +22,15 @@
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) {
- rot_trans_points_.emplace_back(distributed_now, std::make_pair(rvec, 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,
@@ -167,7 +172,7 @@
data_->AddImu(imu_factory_->ToDistributedClock(monotonic_clock::time_point(
chrono::nanoseconds(imu->monotonic_timestamp_ns()))),
- gyro, accel);
+ gyro, accel * kG);
}
} // namespace vision