From 752a32bcd3d8385909a336072c4323f8aaed19fb Mon Sep 17 00:00:00 2001 From: mason turner Date: Wed, 10 Jul 2019 02:40:17 +0000 Subject: [PATCH 01/13] Rewrite Servo Driver --- embedded/servo_bq/servo_balsaq.cc | 14 +++--- embedded/servo_bq/servo_balsaq.hh | 4 +- embedded/servo_driver/cfg/servo_configs.yaml | 24 +++++----- embedded/servo_driver/servo_calibrate.cc | 12 ++--- embedded/servo_driver/servo_driver.cc | 46 ++++++++------------ embedded/servo_driver/servo_driver.hh | 27 ++++++++---- 6 files changed, 64 insertions(+), 63 deletions(-) diff --git a/embedded/servo_bq/servo_balsaq.cc b/embedded/servo_bq/servo_balsaq.cc index 3df4d18..903988e 100644 --- a/embedded/servo_bq/servo_balsaq.cc +++ b/embedded/servo_bq/servo_balsaq.cc @@ -16,10 +16,10 @@ namespace jet { void ServoBq::init(const Config& config) { - subscriber = make_subscriber("servo_command_channel"); + subscriber_ = make_subscriber("servo_command_channel"); for (auto& servo_config : config) { ServoDriver servo = ServoDriver(servo_config.second); - servos.push_back(servo); + servos_.push_back(servo); } } @@ -28,7 +28,7 @@ void ServoBq::loop() { // subscribe_latest proxy bool got_msg = false; - while (subscriber->read(message, 1)) { + while (subscriber_->read(message, 1)) { got_msg = true; } @@ -36,10 +36,10 @@ void ServoBq::loop() { gonogo().go(); last_msg_recvd_timestamp_ = get_current_time(); for (uint i = 0; i < message.servo_indices.size(); i++) { - auto servo_index = message.servo_indices.at(i); + uint servo_index = message.servo_indices.at(i); auto target_radian = message.target_radians.at(i); - assert(servo_index < servos.size()); - servos.at(servo_index).set_angle_radians(target_radian); + assert(servo_index < servos_.size()); + servos_.at(servo_index).set_angle_radians(target_radian); } } if (last_msg_recvd_timestamp_ < get_current_time() - Duration::from_seconds(1)) { @@ -48,7 +48,7 @@ void ServoBq::loop() { } void ServoBq::shutdown() { std::cout << "Servo process shutting down." << std::endl; - servos.at(0).shutdown_pwm(); + servos_.at(0).shutdown_pwm(); } } // namespace jet diff --git a/embedded/servo_bq/servo_balsaq.hh b/embedded/servo_bq/servo_balsaq.hh index 3a31368..8737ba5 100644 --- a/embedded/servo_bq/servo_balsaq.hh +++ b/embedded/servo_bq/servo_balsaq.hh @@ -15,8 +15,8 @@ class ServoBq : public BalsaQ { void shutdown(); private: - std::vector servos; - SubscriberPtr subscriber; + std::vector servos_; + SubscriberPtr subscriber_; Timestamp last_msg_recvd_timestamp_; }; diff --git a/embedded/servo_driver/cfg/servo_configs.yaml b/embedded/servo_driver/cfg/servo_configs.yaml index 724b04d..d54ad2a 100644 --- a/embedded/servo_driver/cfg/servo_configs.yaml +++ b/embedded/servo_driver/cfg/servo_configs.yaml @@ -1,20 +1,20 @@ 0: index: 0 - calibrated_center: 50 - calibrated_max: 71 - max_angle: 20 + calibrated_max_pwm_count: 2740 + calibrated_min_pwm_count: 1320 + max_angle_radians: 0.174533 1: index: 1 - calibrated_center: 50 - calibrated_max: 71 - max_angle: 20 + calibrated_max_pwm_count: 2740 + calibrated_min_pwm_count: 1320 + max_angle_radians: 0.174533 2: index: 2 - calibrated_center: 50 - calibrated_max: 71 - max_angle: 20 + calibrated_max_pwm_count: 2730 + calibrated_min_pwm_count: 1328 + max_angle_radians: 0.174533 3: index: 3 - calibrated_center: 50 - calibrated_max: 71 - max_angle: 20 + calibrated_max_pwm_count: 2740 + calibrated_min_pwm_count: 1320 + max_angle_radians: 0.174533 diff --git a/embedded/servo_driver/servo_calibrate.cc b/embedded/servo_driver/servo_calibrate.cc index 5a967b3..e336a9e 100644 --- a/embedded/servo_driver/servo_calibrate.cc +++ b/embedded/servo_driver/servo_calibrate.cc @@ -22,13 +22,13 @@ void clear_screen() { void print_help(const std::vector &servos) { std::cout << "Servo 0: Press '1' for up, '2' for down -- current percentage is " - << servos[0].get_percentage() << std::endl; + << servos[0].get_percentage() << " count is: " << servos[0].get_pwm_count() << std::endl; std::cout << "Servo 1: Press '3' for up, '4' for down -- current percentage is " - << servos[1].get_percentage() << std::endl; + << servos[1].get_percentage() << " count is: " << servos[1].get_pwm_count() << std::endl; std::cout << "Servo 2: Press '5' for up, '6' for down -- current percentage is " - << servos[2].get_percentage() << std::endl; + << servos[2].get_percentage() << " count is: " << servos[2].get_pwm_count() << std::endl; std::cout << "Servo 3: Press '7' for up, '8' for down -- current percentage is " - << servos[3].get_percentage() << std::endl; + << servos[3].get_percentage() << " count is: " << servos[3].get_pwm_count() << std::endl; std::cout << std::endl; std::cout << "Press Enter to start reading currents from the current sensor" @@ -41,9 +41,9 @@ int keypress(int count, int key) { ServoDriver &servo = servos[(num - 1) / 2]; Direction direction = num % 2 == 1 ? UP : DOWN; if (direction == DOWN) { - servo.set_percentage(std::max(0.0f, servo.get_percentage() - 1.0f)); + servo.set_percentage(std::max(0.0, servo.get_percentage() - 0.005)); } else { - servo.set_percentage(std::min(100.0f, servo.get_percentage() + 1.0f)); + servo.set_percentage(std::min(1.0, servo.get_percentage() + 0.005)); } print_help(servos); return 0; diff --git a/embedded/servo_driver/servo_driver.cc b/embedded/servo_driver/servo_driver.cc index b7557b6..2e1440f 100644 --- a/embedded/servo_driver/servo_driver.cc +++ b/embedded/servo_driver/servo_driver.cc @@ -10,16 +10,6 @@ #include #include -namespace { -// 850 to 2150 microseconds for the PWM width, specified by the servo -constexpr int MIN_COUNTS = 1160; -constexpr int MAX_COUNTS = 2935; -constexpr int PWM_FREQUENCY = 330; -float rad_to_deg(float radians) { - return radians * (180.0 / M_PI); -} -} // namespace - ServoDriver::ServoDriver(const Config& config) { int i2cHandle = i2c_open("/dev/i2c-1"); // TODO make configurable if (i2cHandle == -1) { @@ -32,9 +22,9 @@ ServoDriver::ServoDriver(const Config& config) { pwm_driver_->enable_auto_increment(true); try { - max_angle_ = config["max_angle"].as(); - calibrated_center_ = config["calibrated_center"].as(); - calibrated_max_ = config["calibrated_max"].as(); + max_angle_radians_ = config["max_angle_radians"].as(); + calibrated_min_pwm_count_ = config["calibrated_min_pwm_count"].as(); + calibrated_max_pwm_count_ = config["calibrated_max_pwm_count"].as(); servo_index_ = config["index"].as(); assert(servo_index_ >= 0); } catch (YAML::BadFile e) { @@ -42,36 +32,38 @@ ServoDriver::ServoDriver(const Config& config) { throw std::runtime_error(err); } - percentage_ = calibrated_center_; - set_percentage(percentage_); + set_angle_radians(0); } -void ServoDriver::set_percentage(float unchecked_percentage) { - if (unchecked_percentage > 100 || unchecked_percentage < 0) { +void ServoDriver::set_percentage(double unchecked_percentage, uint max_pwm_count, uint min_pwm_count) { + if (unchecked_percentage > 1 || unchecked_percentage < 0) { std::cout << "Desired servo percentage out of range: " << unchecked_percentage << std::endl; } - percentage_ = std::max(std::min(100.0f, unchecked_percentage), 0.0f); + percentage_ = std::max(std::min(1.0, unchecked_percentage), 0.0); - const float counts_range = MAX_COUNTS - MIN_COUNTS; - int counts = round(counts_range * percentage_ / 100 + MIN_COUNTS); + const uint counts_range = max_pwm_count - min_pwm_count; + const uint counts = counts_range * percentage_ + min_pwm_count; + pwm_count_ = counts; pwm_driver_->set_pwm(servo_index_, 0, counts); } -float ServoDriver::get_percentage() const { +double ServoDriver::get_percentage() const { return percentage_; } +uint ServoDriver::get_pwm_count() const { + return pwm_count_; +} + int ServoDriver::get_servo_index() const { return servo_index_; } -void ServoDriver::set_angle_radians( - float angle_radians) { // TODO noble to give units to angle - float angle_degrees = rad_to_deg(angle_radians); - float angleFraction = static_cast(angle_degrees) / max_angle_; - float half_range = (calibrated_max_ - calibrated_center_); - set_percentage(calibrated_center_ + half_range * angleFraction); +void ServoDriver::set_angle_radians(double angle_radians) { + const double slope = 1.0 / (2*max_angle_radians_); + const double percentage = slope * (angle_radians + max_angle_radians_); + set_percentage(percentage, calibrated_max_pwm_count_, calibrated_min_pwm_count_); } void ServoDriver::shutdown_pwm() { diff --git a/embedded/servo_driver/servo_driver.hh b/embedded/servo_driver/servo_driver.hh index 19dd314..0190ea7 100644 --- a/embedded/servo_driver/servo_driver.hh +++ b/embedded/servo_driver/servo_driver.hh @@ -17,20 +17,29 @@ class ServoDriver { public: ServoDriver(const Config& config); - void set_percentage(float percentage); - float get_percentage() const; + void set_percentage(double percentage, uint max_pwm_count = MAX_PWM_COUNTS, uint min_pwm_count = MIN_PWM_COUNTS); + double get_percentage() const; + uint get_pwm_count() const; int get_servo_index() const; - void set_angle_radians(float angle); + void set_angle_radians(double angle); void shutdown_pwm(); private: + + // 850 to 2150 microseconds for the PWM width, specified by the servo + // Values checked on Oscilloscope + static constexpr uint MIN_PWM_COUNTS = 1160; + static constexpr uint MAX_PWM_COUNTS = 2935; + static constexpr uint PWM_FREQUENCY = 330; int servo_index_ = -1; std::shared_ptr pwm_driver_; std::string config_path_; - // The percentage corresponding to max angle. - float calibrated_max_; - // The percentage corresponding to zero angle. - float calibrated_center_; - float max_angle_; - float percentage_; + // The count corresponding to max angle. + uint calibrated_max_pwm_count_; + // The count corresponding to zero angle. + uint calibrated_min_pwm_count_; + // Max angle in radians that the servo can achieve. + double max_angle_radians_; + double percentage_; + uint pwm_count_; }; From 70f304920d73ca01c95efcc072499ed4df11727b Mon Sep 17 00:00:00 2001 From: mason turner Date: Wed, 10 Jul 2019 02:42:55 +0000 Subject: [PATCH 02/13] Add pwm to comment --- embedded/servo_driver/servo_calibrate.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/embedded/servo_driver/servo_calibrate.cc b/embedded/servo_driver/servo_calibrate.cc index e336a9e..42ed38b 100644 --- a/embedded/servo_driver/servo_calibrate.cc +++ b/embedded/servo_driver/servo_calibrate.cc @@ -22,13 +22,13 @@ void clear_screen() { void print_help(const std::vector &servos) { std::cout << "Servo 0: Press '1' for up, '2' for down -- current percentage is " - << servos[0].get_percentage() << " count is: " << servos[0].get_pwm_count() << std::endl; + << servos[0].get_percentage() << " pwm count is: " << servos[0].get_pwm_count() << std::endl; std::cout << "Servo 1: Press '3' for up, '4' for down -- current percentage is " - << servos[1].get_percentage() << " count is: " << servos[1].get_pwm_count() << std::endl; + << servos[1].get_percentage() << " pwm count is: " << servos[1].get_pwm_count() << std::endl; std::cout << "Servo 2: Press '5' for up, '6' for down -- current percentage is " - << servos[2].get_percentage() << " count is: " << servos[2].get_pwm_count() << std::endl; + << servos[2].get_percentage() << " pwm count is: " << servos[2].get_pwm_count() << std::endl; std::cout << "Servo 3: Press '7' for up, '8' for down -- current percentage is " - << servos[3].get_percentage() << " count is: " << servos[3].get_pwm_count() << std::endl; + << servos[3].get_percentage() << " pwm count is: " << servos[3].get_pwm_count() << std::endl; std::cout << std::endl; std::cout << "Press Enter to start reading currents from the current sensor" From f843ecf6ef188ec3a2577eb167a2f0b5a997283e Mon Sep 17 00:00:00 2001 From: mason turner Date: Wed, 10 Jul 2019 03:11:05 +0000 Subject: [PATCH 03/13] Finish calibrations --- embedded/servo_driver/cfg/servo_configs.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/embedded/servo_driver/cfg/servo_configs.yaml b/embedded/servo_driver/cfg/servo_configs.yaml index d54ad2a..0abafd7 100644 --- a/embedded/servo_driver/cfg/servo_configs.yaml +++ b/embedded/servo_driver/cfg/servo_configs.yaml @@ -1,12 +1,12 @@ 0: index: 0 - calibrated_max_pwm_count: 2740 - calibrated_min_pwm_count: 1320 + calibrated_max_pwm_count: 2713 + calibrated_min_pwm_count: 1239 max_angle_radians: 0.174533 1: index: 1 - calibrated_max_pwm_count: 2740 - calibrated_min_pwm_count: 1320 + calibrated_max_pwm_count: 2819 + calibrated_min_pwm_count: 1275 max_angle_radians: 0.174533 2: index: 2 @@ -15,6 +15,6 @@ max_angle_radians: 0.174533 3: index: 3 - calibrated_max_pwm_count: 2740 - calibrated_min_pwm_count: 1320 + calibrated_max_pwm_count: 2739 + calibrated_min_pwm_count: 1257 max_angle_radians: 0.174533 From bbd06c29975e5902527694a2b5f6c32b2aa1ca3a Mon Sep 17 00:00:00 2001 From: mason turner Date: Wed, 10 Jul 2019 03:28:56 +0000 Subject: [PATCH 04/13] Rename to reflect target angles --- embedded/servo_bq/servo_balsaq.cc | 2 +- embedded/servo_driver/servo_driver.cc | 4 ++-- embedded/servo_driver/servo_driver.hh | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/embedded/servo_bq/servo_balsaq.cc b/embedded/servo_bq/servo_balsaq.cc index 903988e..b308a29 100644 --- a/embedded/servo_bq/servo_balsaq.cc +++ b/embedded/servo_bq/servo_balsaq.cc @@ -39,7 +39,7 @@ void ServoBq::loop() { uint servo_index = message.servo_indices.at(i); auto target_radian = message.target_radians.at(i); assert(servo_index < servos_.size()); - servos_.at(servo_index).set_angle_radians(target_radian); + servos_.at(servo_index).set_vane_angle_radians(target_radian); } } if (last_msg_recvd_timestamp_ < get_current_time() - Duration::from_seconds(1)) { diff --git a/embedded/servo_driver/servo_driver.cc b/embedded/servo_driver/servo_driver.cc index 2e1440f..43bb528 100644 --- a/embedded/servo_driver/servo_driver.cc +++ b/embedded/servo_driver/servo_driver.cc @@ -32,7 +32,7 @@ ServoDriver::ServoDriver(const Config& config) { throw std::runtime_error(err); } - set_angle_radians(0); + set_vane_angle_radians(0); } void ServoDriver::set_percentage(double unchecked_percentage, uint max_pwm_count, uint min_pwm_count) { @@ -60,7 +60,7 @@ int ServoDriver::get_servo_index() const { return servo_index_; } -void ServoDriver::set_angle_radians(double angle_radians) { +void ServoDriver::set_vane_angle_radians(double angle_radians) { const double slope = 1.0 / (2*max_angle_radians_); const double percentage = slope * (angle_radians + max_angle_radians_); set_percentage(percentage, calibrated_max_pwm_count_, calibrated_min_pwm_count_); diff --git a/embedded/servo_driver/servo_driver.hh b/embedded/servo_driver/servo_driver.hh index 0190ea7..5188e31 100644 --- a/embedded/servo_driver/servo_driver.hh +++ b/embedded/servo_driver/servo_driver.hh @@ -21,7 +21,7 @@ class ServoDriver { double get_percentage() const; uint get_pwm_count() const; int get_servo_index() const; - void set_angle_radians(double angle); + void set_vane_angle_radians(double angle); void shutdown_pwm(); private: From ea219cb34105340d8e5ad6c0141b3dab7606a3a6 Mon Sep 17 00:00:00 2001 From: mason turner Date: Wed, 10 Jul 2019 03:56:18 +0000 Subject: [PATCH 05/13] Address Jake Comments --- embedded/servo_driver/servo_calibrate.cc | 1 + embedded/servo_driver/servo_driver.cc | 14 +++++++------- embedded/servo_driver/servo_driver.hh | 14 ++++++++------ 3 files changed, 16 insertions(+), 13 deletions(-) diff --git a/embedded/servo_driver/servo_calibrate.cc b/embedded/servo_driver/servo_calibrate.cc index 42ed38b..f2c9476 100644 --- a/embedded/servo_driver/servo_calibrate.cc +++ b/embedded/servo_driver/servo_calibrate.cc @@ -40,6 +40,7 @@ int keypress(int count, int key) { int num = key - '0'; ServoDriver &servo = servos[(num - 1) / 2]; Direction direction = num % 2 == 1 ? UP : DOWN; + // Increment the percentage set-point by 0.5% if (direction == DOWN) { servo.set_percentage(std::max(0.0, servo.get_percentage() - 0.005)); } else { diff --git a/embedded/servo_driver/servo_driver.cc b/embedded/servo_driver/servo_driver.cc index 43bb528..281de03 100644 --- a/embedded/servo_driver/servo_driver.cc +++ b/embedded/servo_driver/servo_driver.cc @@ -35,15 +35,15 @@ ServoDriver::ServoDriver(const Config& config) { set_vane_angle_radians(0); } -void ServoDriver::set_percentage(double unchecked_percentage, uint max_pwm_count, uint min_pwm_count) { - if (unchecked_percentage > 1 || unchecked_percentage < 0) { +void ServoDriver::set_percentage(const double unchecked_percentage, const int max_pwm_count, const int min_pwm_count) { + if (unchecked_percentage > 1.0 || unchecked_percentage < 0.0) { std::cout << "Desired servo percentage out of range: " << unchecked_percentage << std::endl; } percentage_ = std::max(std::min(1.0, unchecked_percentage), 0.0); - const uint counts_range = max_pwm_count - min_pwm_count; - const uint counts = counts_range * percentage_ + min_pwm_count; + const int counts_range = max_pwm_count - min_pwm_count; + const int counts = counts_range * percentage_ + min_pwm_count; pwm_count_ = counts; pwm_driver_->set_pwm(servo_index_, 0, counts); } @@ -52,7 +52,7 @@ double ServoDriver::get_percentage() const { return percentage_; } -uint ServoDriver::get_pwm_count() const { +int ServoDriver::get_pwm_count() const { return pwm_count_; } @@ -60,8 +60,8 @@ int ServoDriver::get_servo_index() const { return servo_index_; } -void ServoDriver::set_vane_angle_radians(double angle_radians) { - const double slope = 1.0 / (2*max_angle_radians_); +void ServoDriver::set_vane_angle_radians(const double angle_radians) { + const double slope = 1.0 / (2.0*max_angle_radians_); const double percentage = slope * (angle_radians + max_angle_radians_); set_percentage(percentage, calibrated_max_pwm_count_, calibrated_min_pwm_count_); } diff --git a/embedded/servo_driver/servo_driver.hh b/embedded/servo_driver/servo_driver.hh index 5188e31..6f7aae6 100644 --- a/embedded/servo_driver/servo_driver.hh +++ b/embedded/servo_driver/servo_driver.hh @@ -17,13 +17,15 @@ class ServoDriver { public: ServoDriver(const Config& config); - void set_percentage(double percentage, uint max_pwm_count = MAX_PWM_COUNTS, uint min_pwm_count = MIN_PWM_COUNTS); + void set_percentage(const double percentage, const int max_pwm_count = MAX_PWM_COUNTS, const int min_pwm_count = MIN_PWM_COUNTS); double get_percentage() const; - uint get_pwm_count() const; + int get_pwm_count() const; int get_servo_index() const; - void set_vane_angle_radians(double angle); void shutdown_pwm(); + // Sets the jet-vane angle. Angles are symmetric around zero. + void set_vane_angle_radians(const double angle_radians); + private: // 850 to 2150 microseconds for the PWM width, specified by the servo @@ -35,11 +37,11 @@ class ServoDriver { std::shared_ptr pwm_driver_; std::string config_path_; // The count corresponding to max angle. - uint calibrated_max_pwm_count_; + int calibrated_max_pwm_count_; // The count corresponding to zero angle. - uint calibrated_min_pwm_count_; + int calibrated_min_pwm_count_; // Max angle in radians that the servo can achieve. double max_angle_radians_; double percentage_; - uint pwm_count_; + int pwm_count_; }; From 31387bb53a48a2d6bfd6819225c06ba0ada3145e Mon Sep 17 00:00:00 2001 From: mason turner Date: Wed, 10 Jul 2019 04:25:50 +0000 Subject: [PATCH 06/13] Add publisher for commanded PWM set points --- embedded/servo_bq/servo_balsaq.cc | 19 ++++++++++++++----- embedded/servo_bq/servo_balsaq.hh | 2 +- .../servo_bq/servo_pwm_setting_message.hh | 16 ++++++++++++++++ 3 files changed, 31 insertions(+), 6 deletions(-) create mode 100644 embedded/servo_bq/servo_pwm_setting_message.hh diff --git a/embedded/servo_bq/servo_balsaq.cc b/embedded/servo_bq/servo_balsaq.cc index b308a29..b357acb 100644 --- a/embedded/servo_bq/servo_balsaq.cc +++ b/embedded/servo_bq/servo_balsaq.cc @@ -1,6 +1,8 @@ //%bin(servo_balsaq_main) #include "embedded/servo_bq/servo_balsaq.hh" #include "infrastructure/balsa_queue/bq_main_macro.hh" +#include "embedded/servo_bq/set_servo_message.hh" +#include "embedded/servo_bq/servo_pwm_setting_message.hh" #include #include @@ -17,6 +19,7 @@ namespace jet { void ServoBq::init(const Config& config) { subscriber_ = make_subscriber("servo_command_channel"); + publisher_ = make_publisher("servo_pwm_commands_channel"); for (auto& servo_config : config) { ServoDriver servo = ServoDriver(servo_config.second); servos_.push_back(servo); @@ -24,24 +27,30 @@ void ServoBq::init(const Config& config) { } void ServoBq::loop() { - SetServoMessage message; + SetServoMessage rx_message; + ServoPwmSettingMessage tx_message; // subscribe_latest proxy bool got_msg = false; - while (subscriber_->read(message, 1)) { + while (subscriber_->read(rx_message, 1)) { got_msg = true; } if (got_msg) { gonogo().go(); last_msg_recvd_timestamp_ = get_current_time(); - for (uint i = 0; i < message.servo_indices.size(); i++) { - uint servo_index = message.servo_indices.at(i); - auto target_radian = message.target_radians.at(i); + for (uint i = 0; i < rx_message.servo_indices.size(); i++) { + uint servo_index = rx_message.servo_indices.at(i); + auto target_radian = rx_message.target_radians.at(i); assert(servo_index < servos_.size()); servos_.at(servo_index).set_vane_angle_radians(target_radian); + + // Record the servo set points for posterity + tx_message.servo_indices.push_back(servo_index); + tx_message.pwm_set_points.push_back(servos_.at(servo_index).get_pwm_count()); } } + publisher_->publish(tx_message); if (last_msg_recvd_timestamp_ < get_current_time() - Duration::from_seconds(1)) { gonogo().nogo("More than 1 second since last servo command"); } diff --git a/embedded/servo_bq/servo_balsaq.hh b/embedded/servo_bq/servo_balsaq.hh index 8737ba5..7b8e873 100644 --- a/embedded/servo_bq/servo_balsaq.hh +++ b/embedded/servo_bq/servo_balsaq.hh @@ -1,6 +1,5 @@ #pragma once -#include "embedded/servo_bq/set_servo_message.hh" #include "embedded/servo_driver/servo_driver.hh" #include "infrastructure/balsa_queue/balsa_queue.hh" #include "infrastructure/comms/mqtt_comms_factory.hh" @@ -17,6 +16,7 @@ class ServoBq : public BalsaQ { private: std::vector servos_; SubscriberPtr subscriber_; + PublisherPtr publisher_; Timestamp last_msg_recvd_timestamp_; }; diff --git a/embedded/servo_bq/servo_pwm_setting_message.hh b/embedded/servo_bq/servo_pwm_setting_message.hh new file mode 100644 index 0000000..e756c49 --- /dev/null +++ b/embedded/servo_bq/servo_pwm_setting_message.hh @@ -0,0 +1,16 @@ +#pragma once + +#include "infrastructure/comms/schemas/message.hh" +#include + +namespace jet { + +struct ServoPwmSettingMessage : Message { + std::vector servo_indices; + std::vector pwm_set_points; + + MESSAGE(ServoPwmSettingMessage, servo_indices, pwm_set_points); +}; + +} + \ No newline at end of file From e9eddb4666ba7d950226aac258c4157914997980 Mon Sep 17 00:00:00 2001 From: Mason Turner Date: Wed, 10 Jul 2019 22:19:06 -0400 Subject: [PATCH 07/13] Fix Servo Test Target --- embedded/servo_driver/servo_test.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/embedded/servo_driver/servo_test.cc b/embedded/servo_driver/servo_test.cc index 96c9c89..8d2c6fa 100644 --- a/embedded/servo_driver/servo_test.cc +++ b/embedded/servo_driver/servo_test.cc @@ -15,15 +15,15 @@ int main() { // Sweep through servos and set to -max_angle, 0, max_angle for (auto &servo : servos) { - servo.set_angle_radians(-20.0f); + servo.set_vane_angle_radians(0.174533); } usleep(1000000); for (auto &servo : servos) { - servo.set_angle_radians(-0.f); + servo.set_vane_angle_radians(0.0); } usleep(1000000); - for (auto &servo : servos) { - servo.set_angle_radians(20.f); + for (auto &servo : servos) { + servo.set_vane_angle_radians(-0.174533); } return 0; } From 9e8965dd60745938427f7bdf56a3e58c94fb10d3 Mon Sep 17 00:00:00 2001 From: Jacob Panikulam Date: Thu, 14 Feb 2019 22:54:17 -0500 Subject: [PATCH 08/13] Updates to filtering --- camera/camera_manager.cc | 4 + camera/camera_manager.hh | 5 + camera/cfg/camera_config_1.yaml | 5 +- camera/cfg/camera_config_2.yaml | 6 +- camera/cfg/camera_config_jake.yaml | 4 +- camera/cfg/camera_jet.yaml | 24 +- config/fiducial_map/fiducial_map.yaml | 8 +- control/controller_balsaq.cc | 5 +- control/quadraframe_model.hh | 3 +- control/servo_interface.cc | 16 +- embedded/imu_driver/imu_definitions.hh | 37 +- embedded/servo_driver/cfg/servo_cfg2.yaml | 4 +- filtering/calibrate_camera_from_log.cc | 172 ++++++++ filtering/calibrate_jet.cc | 387 +++++++++++++++++ filtering/extract_data_from_log.cc | 157 +++++++ filtering/extract_data_from_log.hh | 39 ++ filtering/filter_balsaq.cc | 48 ++- filtering/filter_demo.cc | 502 ---------------------- filtering/filter_viz_balsaq.cc | 55 ++- filtering/filter_viz_balsaq.hh | 3 +- filtering/to_time_point.cc | 10 + filtering/to_time_point.hh | 6 + infrastructure/gonogo/gonogo_bq.cc | 2 + third_party/experiments | 2 +- vision/fiducial_detection_and_pose.cc | 18 +- vision/fiducial_detection_and_pose.hh | 9 +- vision/fiducial_detection_message.cc | 11 +- 27 files changed, 941 insertions(+), 601 deletions(-) create mode 100644 filtering/calibrate_camera_from_log.cc create mode 100644 filtering/calibrate_jet.cc create mode 100644 filtering/extract_data_from_log.cc create mode 100644 filtering/extract_data_from_log.hh delete mode 100644 filtering/filter_demo.cc create mode 100644 filtering/to_time_point.cc create mode 100644 filtering/to_time_point.hh diff --git a/camera/camera_manager.cc b/camera/camera_manager.cc index ec9da2d..6c27f2c 100644 --- a/camera/camera_manager.cc +++ b/camera/camera_manager.cc @@ -27,6 +27,10 @@ Calibration extract_calibration(const YAML::Node& cfg) { calibration.camera_matrix = cv::Mat(3, 3, CV_64F, camera_matrix_values.data()).clone(); std::vector distortion_coefficient_values = cfg["distortion_coefficients"].as>(); calibration.distortion_coefficients = cv::Mat(1, 5, CV_64F, distortion_coefficient_values.data()).clone(); + + const std::vector resolution = cfg["resolution"].as>(); + calibration.cols = resolution.at(0); + calibration.rows = resolution.at(1); return calibration; } diff --git a/camera/camera_manager.hh b/camera/camera_manager.hh index f2d5ba7..f7d0230 100644 --- a/camera/camera_manager.hh +++ b/camera/camera_manager.hh @@ -12,6 +12,11 @@ namespace jet { struct Calibration { cv::Mat camera_matrix; cv::Mat distortion_coefficients; + + // The calibration is fixed for some resolution + // (The focal length is in units of pixels!) + int cols; + int rows; }; struct Camera { diff --git a/camera/cfg/camera_config_1.yaml b/camera/cfg/camera_config_1.yaml index d2b4d67..044cb39 100644 --- a/camera/cfg/camera_config_1.yaml +++ b/camera/cfg/camera_config_1.yaml @@ -2,7 +2,8 @@ serial_number: 31E7DB5E v4l_path: usb-046d_Logitech_Webcam_C930e_31E7DB5E-video-index0 note: these cal values were copied from camera 0 camera_matrix: [499.7749869454186, 0, 309.3792706235992, - 0, 496.9300965132637, 241.6934416030273, + 0, 496.9300965132637, 241.6934416030273, 0, 0, 1] distortion_coefficients: [0.003861115403120386, 0.09541932181851349, 0.001898991151152847, - -0.003082742498836169, -0.2932184860155891] \ No newline at end of file + -0.003082742498836169, -0.2932184860155891] +resolution: [640, 480] \ No newline at end of file diff --git a/camera/cfg/camera_config_2.yaml b/camera/cfg/camera_config_2.yaml index 9945fb7..ec34181 100644 --- a/camera/cfg/camera_config_2.yaml +++ b/camera/cfg/camera_config_2.yaml @@ -2,7 +2,9 @@ serial_number: CA38DB5E v4l_path: usb-046d_Logitech_Webcam_C930e_CA38DB5E-video-index0 note: these cal values were copied from camera 0 camera_matrix: [499.7749869454186, 0, 309.3792706235992, - 0, 496.9300965132637, 241.6934416030273, + 0, 496.9300965132637, 241.6934416030273, 0, 0, 1] distortion_coefficients: [0.003861115403120386, 0.09541932181851349, 0.001898991151152847, - -0.003082742498836169, -0.2932184860155891] \ No newline at end of file + -0.003082742498836169, -0.2932184860155891] + +resolution: [640, 480] \ No newline at end of file diff --git a/camera/cfg/camera_config_jake.yaml b/camera/cfg/camera_config_jake.yaml index 35a79ea..d5d2866 100644 --- a/camera/cfg/camera_config_jake.yaml +++ b/camera/cfg/camera_config_jake.yaml @@ -5,4 +5,6 @@ camera_matrix: [499.7749869454186, 0, 309.3792706235992, 0, 496.9300965132637, 241.6934416030273, 0, 0, 1] distortion_coefficients: [0.003861115403120386, 0.09541932181851349, 0.001898991151152847, - -0.003082742498836169, -0.2932184860155891] \ No newline at end of file + -0.003082742498836169, -0.2932184860155891] + +resolution: [640, 480] \ No newline at end of file diff --git a/camera/cfg/camera_jet.yaml b/camera/cfg/camera_jet.yaml index 45cecb9..f0b582a 100644 --- a/camera/cfg/camera_jet.yaml +++ b/camera/cfg/camera_jet.yaml @@ -1,7 +1,21 @@ +distortion_coefficients: + - 0.06693324518972132 + - -0.2241512297832522 + - 0.009935840339065886 + - 0.008486955527599985 + - 0.1017895523815406 serial_number: 5CFD076E +camera_matrix: + - 279.7596429981606 + - 0 + - 235.9919590561939 + - 0 + - 280.0340888659648 + - 141.9506134614783 + - 0 + - 0 + - 1 +resolution: + - 480 + - 270 v4l_path: usb-046d_Logitech_Webcam_C930e_5CFD076E-video-index0 -camera_matrix: [499.7749869454186, 0, 309.3792706235992, - 0, 496.9300965132637, 241.6934416030273, - 0, 0, 1] -distortion_coefficients: [0.003861115403120386, 0.09541932181851349, 0.001898991151152847, - -0.003082742498836169, -0.2932184860155891] \ No newline at end of file diff --git a/config/fiducial_map/fiducial_map.yaml b/config/fiducial_map/fiducial_map.yaml index 0589126..9f8b7b3 100644 --- a/config/fiducial_map/fiducial_map.yaml +++ b/config/fiducial_map/fiducial_map.yaml @@ -1,14 +1,14 @@ fiducial_4x4: artag_squares: 4 dict_num: 250 - log_translation_tag_from_world: [0.182163, 0.129157, 0.898972] - log_rotation_tag_from_world: [-1.0712, 0.978419, 1.12526] + log_translation_tag_from_world: [-0.285517, -0.0842155, 1.2411] + log_rotation_tag_from_world: [0.495738, 1.39608, 0.0704547] artag_width_mm: 99.0 # measured by Matt feb 10 2019 artag_gap_mm: 50.0 # measured by Matt feb 10 2019 fiducial_5x5: artag_squares: 5 dict_num: 250 - log_translation_tag_from_world: [0.182163, 0.129157, 0.898972] - log_rotation_tag_from_world: [-1.0712, 0.978419, 1.12526] + log_translation_tag_from_world: [-0.285517, -0.0842155, 1.2411] + log_rotation_tag_from_world: [0.495738, 1.39608, 0.0704547] artag_width_mm: 100.0 # measured by Matt feb 10 2019 artag_gap_mm: 50.0 # measured by Matt feb 10 2019 diff --git a/control/controller_balsaq.cc b/control/controller_balsaq.cc index 0ed4200..1d82787 100644 --- a/control/controller_balsaq.cc +++ b/control/controller_balsaq.cc @@ -35,6 +35,8 @@ jcc::Vec3 sigmoid(const jcc::Vec3& v) { QuadraframeStatus generate_control(const SO3& world_from_target, const Pose& pose, const JetStatus& jet_status) { JetVaneMapper mapper_; + const MatNd<3, 3> K = jcc::Vec3(0.3, 0.3, 0.4).asDiagonal(); + // // Compute the current expected jet force (All servos zero'd) // @@ -50,9 +52,8 @@ QuadraframeStatus generate_control(const SO3& world_from_target, const Pose& pos const jcc::Vec3 desired_force_jet_frame = wrench_for_zero.force_N; - const double gain_p = 0.1; const SO3 target_from_jet = world_from_target.inverse() * pose.world_from_jet.so3(); - const jcc::Vec3 desired_torque_jet_frame = gain_p * sigmoid(target_from_jet.log()); + const jcc::Vec3 desired_torque_jet_frame = -K * target_from_jet.log(); Wrench target_wrench; target_wrench.torque_Nm = desired_torque_jet_frame; diff --git a/control/quadraframe_model.hh b/control/quadraframe_model.hh index 20a836a..0342e62 100644 --- a/control/quadraframe_model.hh +++ b/control/quadraframe_model.hh @@ -1,8 +1,9 @@ #pragma once #include "control/jet_vane_model.hh" -#include "control/vanes_generated.hh" #include "control/wrench.hh" +#include "control/vanes_generated.hh" + namespace jet { namespace control { diff --git a/control/servo_interface.cc b/control/servo_interface.cc index 62d0d37..70d83f4 100644 --- a/control/servo_interface.cc +++ b/control/servo_interface.cc @@ -9,16 +9,16 @@ SetServoMessage create_servo_command(const QuadraframeStatus& qframe_status) { SetServoMessage servo_msg; servo_msg.servo_indices.push_back(0); - servo_msg.target_radians.push_back(qframe_status.servo_0_angle_rad); + servo_msg.target_radians.push_back(-qframe_status.servo_0_angle_rad); servo_msg.servo_indices.push_back(1); - servo_msg.target_radians.push_back(qframe_status.servo_1_angle_rad); + servo_msg.target_radians.push_back(-qframe_status.servo_1_angle_rad); servo_msg.servo_indices.push_back(2); - servo_msg.target_radians.push_back(qframe_status.servo_2_angle_rad); + servo_msg.target_radians.push_back(-qframe_status.servo_2_angle_rad); servo_msg.servo_indices.push_back(3); - servo_msg.target_radians.push_back(qframe_status.servo_3_angle_rad); + servo_msg.target_radians.push_back(-qframe_status.servo_3_angle_rad); return servo_msg; } @@ -32,10 +32,10 @@ QuadraframeStatus create_quadraframe_status(const SetServoMessage& servo_msg) { } QuadraframeStatus qframe_status; - qframe_status.servo_0_angle_rad = servo_msg.target_radians[0]; - qframe_status.servo_1_angle_rad = servo_msg.target_radians[1]; - qframe_status.servo_2_angle_rad = servo_msg.target_radians[2]; - qframe_status.servo_3_angle_rad = servo_msg.target_radians[3]; + qframe_status.servo_0_angle_rad = -servo_msg.target_radians[0]; + qframe_status.servo_1_angle_rad = -servo_msg.target_radians[1]; + qframe_status.servo_2_angle_rad = -servo_msg.target_radians[2]; + qframe_status.servo_3_angle_rad = -servo_msg.target_radians[3]; return qframe_status; } diff --git a/embedded/imu_driver/imu_definitions.hh b/embedded/imu_driver/imu_definitions.hh index 27ba317..ed26384 100644 --- a/embedded/imu_driver/imu_definitions.hh +++ b/embedded/imu_driver/imu_definitions.hh @@ -7,35 +7,38 @@ namespace jet { namespace embedded { +// clang-format off struct GyroConfig0 { - static constexpr uint8_t DPS_2000 = 0b000; - static constexpr uint8_t DPS_1000 = 0b001; - static constexpr uint8_t DPS_500 = 0b010; - static constexpr uint8_t DPS_250 = 0b011; - static constexpr uint8_t DPS_125 = 0b100; - static constexpr uint8_t HZ_523 = 0b000000; - static constexpr uint8_t HZ_230 = 0b001000; - static constexpr uint8_t HZ_116 = 0b010000; + static constexpr uint8_t DPS_2000 = 0b000; + static constexpr uint8_t DPS_1000 = 0b001; + static constexpr uint8_t DPS_500 = 0b010; + static constexpr uint8_t DPS_250 = 0b011; + static constexpr uint8_t DPS_125 = 0b100; + static constexpr uint8_t HZ_523 = 0b000000; + static constexpr uint8_t HZ_230 = 0b001000; + static constexpr uint8_t HZ_116 = 0b010000; }; struct AccConfig { - static constexpr uint8_t g_2 = 0b000; - static constexpr uint8_t g_4 = 0b001; - static constexpr uint8_t g_8 = 0b010; - static constexpr uint8_t g_16 = 0b011; + static constexpr uint8_t g_2 = 0b00; + static constexpr uint8_t g_4 = 0b01; + static constexpr uint8_t g_8 = 0b10; + static constexpr uint8_t g_16 = 0b11; - static constexpr uint8_t HZ_7_81 = 0b00000; + static constexpr uint8_t HZ_7_81 = 0b00000; static constexpr uint8_t HZ_15_63 = 0b00100; - static constexpr uint8_t HZ_62_5 = 0b01100; - static constexpr uint8_t HZ_125 = 0b10000; + static constexpr uint8_t HZ_62_5 = 0b01100; + static constexpr uint8_t HZ_125 = 0b10000; + static constexpr uint8_t HZ_250 = 0b10100; }; struct ConfigAdresses { static constexpr uint8_t GYR_CONFIG_0 = 0x38; static constexpr uint8_t GYR_CONFIG_1 = 0x00; - static constexpr uint8_t ACC_CONFIG = 0x0D; - static constexpr uint8_t MAG_CONFIG = 0x6D; + static constexpr uint8_t ACC_CONFIG = 0x0D; + static constexpr uint8_t MAG_CONFIG = 0x6D; }; +// clang-format on struct ImuMeasurements { jcc::Vec3 accel_mpss; diff --git a/embedded/servo_driver/cfg/servo_cfg2.yaml b/embedded/servo_driver/cfg/servo_cfg2.yaml index cb05c85..3afcb7f 100644 --- a/embedded/servo_driver/cfg/servo_cfg2.yaml +++ b/embedded/servo_driver/cfg/servo_cfg2.yaml @@ -1,4 +1,4 @@ index: 2 calibrated_center: 50 -calibrated_max: 100 -max_angle: 14 +calibrated_max: 71 +max_angle: 20 diff --git a/filtering/calibrate_camera_from_log.cc b/filtering/calibrate_camera_from_log.cc new file mode 100644 index 0000000..de19e0b --- /dev/null +++ b/filtering/calibrate_camera_from_log.cc @@ -0,0 +1,172 @@ +//%deps(simple_geometry) +//%deps(ui2d) +//%deps(window_3d) +//%deps(time_point) +#include "third_party/experiments/estimation/time_point.hh" +#include "third_party/experiments/logging/log.hh" +#include "third_party/experiments/viewer/interaction/ui2d.hh" +#include "third_party/experiments/viewer/primitives/simple_geometry.hh" +#include "third_party/experiments/viewer/window_3d.hh" + +#include "filtering/extract_data_from_log.hh" + +#include "vision/fiducial_detection_and_pose.hh" + +//%deps(yaml-cpp) +#include + +namespace jet { +namespace filtering { + +namespace { + +bool visualize() { + return true; +} + +void setup() { + if (visualize()) { + const auto view = viewer::get_window3d("Calibration"); + view->set_continue_time_ms(1); + const auto background = view->add_primitive(); + const geometry::shapes::Plane ground{jcc::Vec3::UnitZ(), 0.0}; + background->add_plane({ground, 0.1}); + background->flip(); + } +} +} // namespace + +void calibrate_camera(const std::string& log_path) { + jcc::Success() << "Preparing to calibrate" << std::endl; + + // + // Set up the viewer + // + + setup(); + + // + // Grab images + // + + jcc::Success() << "[Camera] Validating fiducial measurements" << std::endl; + const TimeRange range{}; + ImageStream image_stream(log_path, range); + jcc::Success() << "[Camera] Parsing images" << std::endl; + + // + // View stuff + // + + const auto view = viewer::get_window3d("Calibration"); + const auto geo = view->add_primitive(); + const auto ui2d = view->add_primitive(); + + std::vector> object_points; + std::vector> image_points; + + std::string serial_number = ""; + int cols, rows; + + constexpr int DECIMATION = 25; + + int i = 0; + while (true) { + const auto image = image_stream.next(); + ++i; + if (i % DECIMATION != 0) { + continue; + } + if (!image) { + break; + } + + const auto obj_pt_associations = obj_points_img_points_from_image(image->image); + + serial_number = image->serial_number; + cols = image->image.cols; + rows = image->image.rows; + + if (visualize()) { + ui2d->clear(); + geo->clear(); + ui2d->add_image(image->image, 1.0); + + if (!obj_pt_associations.empty()) { + // + // Make OpenCV points for calibration + // + + std::vector this_object_points; + std::vector this_image_points; + for (const auto& assoc : obj_pt_associations) { + const jcc::Vec2 pt_board_surface = jcc::Vec2(assoc.point_board_space); + const jcc::Vec3 obj_pt_dbl = jcc::Vec3(pt_board_surface.x(), pt_board_surface.y(), 0.0); + const jcc::Vec2 image_pt_dbl = assoc.point_image_space; + + const Eigen::Vector2f image_pt = image_pt_dbl.cast(); + const Eigen::Vector3f obj_pt = obj_pt_dbl.cast(); + this_image_points.push_back({image_pt.x(), image_pt.y()}); + this_object_points.push_back({obj_pt.x(), obj_pt.y(), obj_pt.z()}); + + const jcc::Vec4 blue(0.0, 0.0, 1.0, 1.0); + constexpr double PT_SIZE = 5.0; + ui2d->add_point({image_pt_dbl / image->image.rows, blue, PT_SIZE}); + } + image_points.push_back(this_image_points); + object_points.push_back(this_object_points); + } + + ui2d->flip(); + geo->flip(); + + view->spin_until_step(); + } + } + + cv::Mat K; + cv::Mat D; + std::vector rvecs, tvecs; + jcc::Warning() << "Calibrating camera, this might take a while..." << std::endl; + int flag = 0; + const auto t0 = jcc::now(); + cv::calibrateCamera(object_points, image_points, cv::Size(480, 270), K, D, rvecs, tvecs, flag); + const auto t1 = jcc::now(); + jcc::Success() << "Done. Took: " << estimation::to_seconds(t1 - t0) << std::endl; + jcc::Success() << "Generating yaml..." << std::endl; + + YAML::Node node; // starts out as null + node["serial_number"] = serial_number; // it now is a map node + + { + std::stringstream ss; + ss << "usb-046d_Logitech_Webcam_C930e_" << serial_number << "-video-index0"; + node["v4l_path"] = ss.str(); + } + + for (int i = 0; i < D.cols; ++i) { + node["distortion_coefficients"].push_back(D.at(0, i)); + } + for (int i = 0; i < K.cols; ++i) { + for (int j = 0; j < K.rows; ++j) { + node["camera_matrix"].push_back(K.at(i, j)); + } + } + node["resolution"].push_back(cols); + node["resolution"].push_back(rows); + + node["source_log"] = log_path; + + std::cout << "\n\n\n" << std::endl; + std::cout << node << std::endl; + std::cout << "\n\n\n" << std::endl; +} + +} // namespace filtering +} // namespace jet + +int main(int argc, char** argv) { + assert(argc > 1); + const std::string path(argv[1]); + jet::filtering::calibrate_camera(path); +} \ No newline at end of file diff --git a/filtering/calibrate_jet.cc b/filtering/calibrate_jet.cc new file mode 100644 index 0000000..3538868 --- /dev/null +++ b/filtering/calibrate_jet.cc @@ -0,0 +1,387 @@ +//%deps(jet_filter) +//%deps(jet_optimizer) +//%deps(nonlinear_camera_model) +//%deps(warn_sensor_rates) +//%deps(make_interpolator) +#include "third_party/experiments/estimation/sensors/make_interpolator.hh" + +#include "third_party/experiments/estimation/calibration/nonlinear_camera_model.hh" +#include "third_party/experiments/estimation/calibration/warn_sensor_rates.hh" +#include "third_party/experiments/estimation/jet/jet_filter.hh" +#include "third_party/experiments/estimation/jet/jet_optimizer.hh" + +//%deps(calibrate_single_imu) +//%deps(robust_pnp) +//%deps(visualize_calibration) +//%deps(visualize_camera_calibration) +#include "third_party/experiments/estimation/calibration/calibrate_single_imu.hh" +#include "third_party/experiments/estimation/vision/robust_pnp.hh" +#include "third_party/experiments/estimation/visualization/visualize_calibration.hh" +#include "third_party/experiments/estimation/visualization/visualize_camera_calibration.hh" + +//%deps(form_coordinate_frame) +#include "third_party/experiments/geometry/spatial/form_coordinate_frame.hh" + +//%deps(simple_geometry) +//%deps(ui2d) +//%deps(window_3d) +#include "third_party/experiments/viewer/interaction/ui2d.hh" +#include "third_party/experiments/viewer/primitives/simple_geometry.hh" +#include "third_party/experiments/viewer/window_3d.hh" + +//%deps(jet_model) +#include "third_party/experiments/planning/jet/jet_model.hh" +//%deps(log) +#include "third_party/experiments/logging/log.hh" + +#include "filtering/extract_data_from_log.hh" + +#include "camera/camera_manager.hh" +#include "vision/fiducial_detection_and_pose.hh" + +// - Validate reprojection of fiducial detection using the packaged calibration +// - Validate opencv calibration +// x - Estimate IMU intrinsics +// x - Estimate direction of gravity +// - 2D imageview + +namespace jet { +namespace filtering { + +namespace { +constexpr int IMU_1 = 78; +constexpr int IMU_2 = 36; + +estimation::ProjectionCoefficients proj_coeffs_from_opencv(const Calibration& cal) { + estimation::ProjectionCoefficients model; + model.fx = cal.camera_matrix.at(0, 0); + model.fy = cal.camera_matrix.at(1, 1); + model.cx = cal.camera_matrix.at(0, 2); + model.cy = cal.camera_matrix.at(1, 2); + model.k1 = cal.distortion_coefficients.at(0); + model.k2 = cal.distortion_coefficients.at(1); + model.p1 = cal.distortion_coefficients.at(2); + model.p2 = cal.distortion_coefficients.at(3); + model.k3 = cal.distortion_coefficients.at(4); + + model.rows = cal.rows; + model.cols = cal.cols; + + return model; +} + +std::vector object_image_assoc_from_board_point( + const std::vector& obj_pt_associations) { + std::vector obj_image; + + for (const auto& assoc : obj_pt_associations) { + const jcc::Vec2 pt_board_surface = jcc::Vec2(assoc.point_board_space); + const jcc::Vec3 observed_pt_board_frame = jcc::Vec3(pt_board_surface.x(), pt_board_surface.y(), 0.0); + const jcc::Vec2 observed_pt_image = assoc.point_image_space; + obj_image.push_back({observed_pt_image, observed_pt_board_frame}); + } + return obj_image; +} + +void align_crappy_jake(const std::shared_ptr& geo, + const std::shared_ptr& ui2d, + const estimation::NonlinearCameraModel& model, + const SE3& fiducial_from_camera_init, + const std::vector& obj_pt_associations) { + std::vector observed; + std::vector object; + + const auto associations = object_image_assoc_from_board_point(obj_pt_associations); + + const auto pnp_visitor = estimation::make_pnp_visitor(model, object, observed); + const auto pnp_result = estimation::robust_pnp(model, fiducial_from_camera_init, object, observed, pnp_visitor); +} + +} // namespace + +const estimation::CreateSingleImuModelConfig imu_cal_cfg{ + .visualize_imu_model = false, // + .visualize_gyro = false, // + .visualize_magnetometer = false // +}; + +const estimation::CameraCalibrationConfig camera_cal_cfg{ + .visualize_camera = false, // + .visualize_camera_distortion = false, // + .visualize_camera_frustum = false // +}; + +bool visualize() { + return (camera_cal_cfg.visualize_camera || camera_cal_cfg.visualize_camera_distortion || + camera_cal_cfg.visualize_camera_frustum || imu_cal_cfg.visualize_imu_model || imu_cal_cfg.visualize_gyro || + imu_cal_cfg.visualize_magnetometer); +} + +void setup() { + if (visualize()) { + const auto view = viewer::get_window3d("Calibration"); + view->set_continue_time_ms(1); + const auto background = view->add_primitive(); + const geometry::shapes::Plane ground{jcc::Vec3::UnitZ(), 0.0}; + background->add_plane({ground, 0.1}); + background->flip(); + } +} + +struct JetModel { + std::map imu_calibration_from_imu_id; +}; + +jcc::Optional find_nearest_fiducial( + const estimation::calibration::CalibrationMeasurements& cal_meas, const estimation::TimePoint& t) { + JASSERT_FALSE(cal_meas.fiducial_meas.empty(), "Cannot calibrate with empty fiducial measurements"); + for (const auto& fiducial_meas : cal_meas.fiducial_meas) { + if (t < fiducial_meas.timestamp) { + return {fiducial_meas.measurement}; + } + } + return {}; +} + +SE3 compute_world_from_camera(const estimation::SingleImuCalibration& imu_cal) { + const geometry::Unit3 g_camera_frame = imu_cal.camera_from_gyro * imu_cal.g_estimate.direction; + const SO3 world_from_camera = geometry::spatial::form_coordinate_frame_from_zhat(g_camera_frame); + return SE3(world_from_camera, jcc::Vec3::Zero()); +} + +ejf::Parameters compute_parameters(const estimation::calibration::CalibrationMeasurements& cal_meas, + const JetModel& jet_model) { + const auto& imu_cal_1 = jet_model.imu_calibration_from_imu_id.at(IMU_1); + const auto& imu_cal_2 = jet_model.imu_calibration_from_imu_id.at(IMU_2); + + const SO3 world_from_camera_1 = compute_world_from_camera(imu_cal_1).so3(); + const SO3 world_from_camera_2 = compute_world_from_camera(imu_cal_2).so3(); + jcc::Warning() << "[Filter Setup] IMU->IMU Alignment Error: " + << (world_from_camera_1 * world_from_camera_2.inverse()).log().norm() << std::endl; + + const auto maybe_nearest_fiducial = find_nearest_fiducial(cal_meas, imu_cal_1.g_estimate.time); + assert(maybe_nearest_fiducial); + const auto fiducial_at_g_estimate = *maybe_nearest_fiducial; + const SO3 fiducial_from_camera = fiducial_at_g_estimate.T_fiducial_from_camera.so3(); + const SO3 world_from_fiducial = world_from_camera_1 * fiducial_from_camera.inverse(); + + auto p = ejf::JetFilter::reasonable_parameters(); + { + p.T_world_from_fiducial = SE3(world_from_fiducial, jcc::Vec3::Zero()); + p.T_imu1_from_vehicle = SE3(imu_cal_1.camera_from_gyro.inverse(), jcc::Vec3::Zero()); + p.T_imu2_from_vehicle = SE3(imu_cal_2.camera_from_gyro.inverse(), jcc::Vec3::Zero()); + } + + return p; +} + +void test_filter(const estimation::calibration::CalibrationMeasurements& cal_meas, const JetModel& jet_model) { + const auto view = viewer::get_window3d("Calibration"); + const auto geo = view->add_primitive(); + + const auto geo_stable = view->add_primitive(); + const auto jet_tree = view->add_primitive(); + + const jcc::Vec3 jet_origin(1.0, 0.0, 0.0); + + const planning::jet::JetModel jet_3dmodel; + constexpr bool DRAW_VEHICLE = true; + const SO3 jetmodel_from_body = jcc::exp_x(M_PI * 0.5).so3(); + if constexpr (DRAW_VEHICLE) { + jet_3dmodel.insert(*jet_tree); + } + + const auto& imu_1_meas = cal_meas.imu_cal.at(IMU_1); + const auto& imu_1_cal = jet_model.imu_calibration_from_imu_id.at(IMU_1); + const auto& imu_2_cal = jet_model.imu_calibration_from_imu_id.at(IMU_2); + + const auto& imu_1_model = imu_1_cal.imu_model; + const auto accel_interp = estimation::make_accel_interpolator(imu_1_meas.accel_meas, imu_1_model); + + const auto t0 = cal_meas.first(); + // const auto t0 = imu_1_meas.accel_meas.front().timestamp; + + auto xp0 = ejf::JetFilter::reasonable_initial_state(t0); + { // + xp0.x.R_world_from_body = compute_world_from_camera(jet_model.imu_calibration_from_imu_id.at(IMU_2)).so3(); + } + const auto p = compute_parameters(cal_meas, jet_model); + ejf::JetFilter jf(xp0, p); + + for (const auto& fiducial_meas : cal_meas.fiducial_meas) { + jf.measure_fiducial(fiducial_meas.measurement, fiducial_meas.timestamp); + } + + for (const auto& accel : cal_meas.imu_cal.at(IMU_1).accel_meas) { + const jcc::Vec3 corrected_accel = imu_1_model.correct_measured_accel(accel.measurement.observed_acceleration); + // jf.measure_imu({corrected_accel}, accel.timestamp); + } + + for (const auto& gyro : cal_meas.imu_cal.at(IMU_1).gyro_meas) { + jf.measure_gyro(gyro.measurement, gyro.timestamp); + } + + const auto& imu_2_model = imu_2_cal.imu_model; + for (const auto& accel : cal_meas.imu_cal.at(IMU_2).accel_meas) { + const jcc::Vec3 corrected_accel = imu_2_model.correct_measured_accel(accel.measurement.observed_acceleration); + // jf.measure_imu({corrected_accel}, accel.timestamp, true); + } + + for (const auto& gyro : cal_meas.imu_cal.at(IMU_2).gyro_meas) { + // jf.measure_gyro(gyro.measurement, gyro.timestamp, true); + } + + int k = 0; + while (jf.next_measurement()) { + ++k; + if (k % 2 != 0) { + continue; + } + const auto state = jf.state().x; + // geo->add_axes({get_world_from_body(state)}); + + const SE3 T_world_from_body = get_world_from_body(state); + constexpr bool PRINT_STATES = false; + if (PRINT_STATES) { + std::cout << "States: " << std::endl; + std::cout << "\taccel_bias: " << state.accel_bias.transpose() << std::endl; + std::cout << "\tgyro_bias: " << state.gyro_bias.transpose() << std::endl; + std::cout << "\teps_ddot: " << state.eps_ddot.transpose() << std::endl; + std::cout << "\teps_dot: " << state.eps_dot.transpose() << std::endl; + std::cout << "\tr: " << T_world_from_body.so3().log().transpose() << std::endl; + std::cout << "\tx: " << T_world_from_body.translation().transpose() << std::endl; + } + + // Fix jitter: Covariances plz bby + geo->add_axes({SE3(T_world_from_body.so3(), jet_origin)}); + + const jcc::Vec3 accel_imu_frame = *accel_interp(jf.state().time_of_validity); + + const SE3 imu_from_vehicle = jf.parameters().T_imu1_from_vehicle; + const SO3 world_from_imu = state.R_world_from_body * imu_from_vehicle.so3().inverse(); + + geo->add_line( + {jet_origin, jet_origin + (world_from_imu * accel_imu_frame) / 9.81, jcc::Vec4(0.2, 0.2, 1.0, 1.0), 5.0}); + + geo->add_line({jcc::Vec3::Zero(), accel_imu_frame, jcc::Vec4(0.2, 0.2, 1.0, 1.0), 5.0}); + // Velocity + geo->add_line({jcc::Vec3::Zero(), state.eps_dot.head<3>(), jcc::Vec4(1.0, 0.0, 0.0, 1.0), 5.0}); + // Angular Velocity + geo->add_line({jcc::Vec3::Zero(), -state.eps_dot.tail<3>(), jcc::Vec4(0.0, 1.0, 0.0, 1.0), 5.0}); + + jet_tree->set_world_from_root(SE3(T_world_from_body.so3() * jetmodel_from_body.inverse(), jet_origin)); + + geo->flip(); + view->spin_until_step(); + } +} + +void go() { + jcc::Success() << "Preparing to calibrate" << std::endl; + + // + // Grab the calibration data from the log + // + + // const std::string path = "/jet/logs/calibration-log-jul4-2/"; + + // const std::string path = "/jet/logs/calibration-log-jul6-6"; + const std::string path = "/jet/logs/calibration-log-jul6-7"; + + const TimeRange range{}; + const ExtractionConfiguration default_cfg{}; + const auto cal_measurements = extract_data_from_log(path, range, default_cfg); + + estimation::warn_sensor_rates(cal_measurements); + + const double total_time = estimation::to_seconds(cal_measurements.last() - cal_measurements.first()); + jcc::Success() << "Done reading log." << std::endl; + + if (total_time < 30.0) { + jcc::Warning() << "Total time (" << total_time << " seconds) less than 30.0 sec" << std::endl; + } else { + jcc::Success() << "Total time: " << total_time << " seconds" << std::endl; + } + + // + // Set up the viewer + // + setup(); + + jcc::Success() << "Calibrating..." << std::endl; + + // + // Calibrate the IMU intrinsics / Estrinsics + // + + JetModel jet_model; + for (const auto& imu_measurements : cal_measurements.imu_cal) { + const auto one_imu = estimation::create_single_imu_model(cal_measurements, imu_measurements.second, imu_cal_cfg); + jet_model.imu_calibration_from_imu_id[imu_measurements.first] = one_imu; + } + + // + // Validate the fiducial reprojections + // + + jcc::Success() << "[Camera] Validating fiducial measurements" << std::endl; + ImageStream image_stream(path, range); + jcc::Success() << "[Camera] Parsing images..." << std::endl; + + const CameraManager cam_mgr; + const auto view = viewer::get_window3d("Calibration"); + const auto geo = view->add_primitive(); + const auto ui2d = view->add_primitive(); + + bool set = false; + geometry::Unit3 permanent_g_fiducial_frame; + + std::vector> collected_fiducial_meas; + + int i = 0; + while (true) { + const auto image = image_stream.next(); + + ++i; + if (!image) { + break; + } + + const auto calibration = cam_mgr.get_calibration(image->serial_number); + const auto proj = proj_coeffs_from_opencv(calibration); + const auto model = estimation::NonlinearCameraModel(proj); + + const auto ids_corners = get_ids_and_corners(camera_frame); + const auto obj_pt_associations = obj_points_img_points_from_image(ids_corners); + const auto fiducial_from_camera = estimate_board_bottom_left_from_camera(ids_corners, calibration); + + const auto t = image->time; + if (fiducial_from_camera) { + ejf::FiducialMeasurement fiducial_measurement; + fiducial_measurement.T_fiducial_from_camera = *fiducial_from_camera; + collected_fiducial_meas.push_back({fiducial_measurement, t}); + } + + // TODO: NEXT, FILL THIS IN + if (camera_cal_cfg.visualize_camera) { + const auto associations = object_image_assoc_from_board_point(obj_pt_associations); + estimation::visualize_single_camera_frame( + model, fiducial_from_camera, associations, *image, ui2d, geo, camera_cal_cfg); + } + } + + jcc::Success() << "[Filter] Testing filter..." << std::endl; + { + estimation::calibration::CalibrationMeasurements new_cal_measurements = cal_measurements; + new_cal_measurements.fiducial_meas = collected_fiducial_meas; + test_filter(new_cal_measurements, jet_model); + } + jcc::Success() << "Done." << std::endl; +} + +} // namespace filtering +} // namespace jet + +int main() { + jet::filtering::go(); +} \ No newline at end of file diff --git a/filtering/extract_data_from_log.cc b/filtering/extract_data_from_log.cc new file mode 100644 index 0000000..c6f3ed0 --- /dev/null +++ b/filtering/extract_data_from_log.cc @@ -0,0 +1,157 @@ +#include "filtering/extract_data_from_log.hh" + +#include "camera/camera_image_message.hh" +#include "embedded/imu_driver/imu_message.hh" +#include "filtering/to_time_point.hh" +#include "infrastructure/logging/log_reader.hh" +#include "vision/fiducial_detection_message.hh" + +//%deps(log) +#include "third_party/experiments/logging/log.hh" +#include "third_party/experiments/out.hh" + +namespace jet { +namespace filtering { + +namespace { +void add_imu(const ImuMessage& msg, + const TimeRange& range, + Out meas) { + const auto time_of_validity = to_time_point(msg.timestamp); + + if ((time_of_validity < range.start) || (time_of_validity > range.end)) { + return; + } + + const int imu_id = msg.imu_id_leading_byte; + // Today, it is redundancy day today + meas->imu_cal[imu_id].imu_id = imu_id; + { + const jcc::Vec3 accel_mpss(msg.accel_mpss_x, msg.accel_mpss_y, msg.accel_mpss_z); + ejf::AccelMeasurement accel_meas; + accel_meas.observed_acceleration = accel_mpss; + meas->imu_cal[imu_id].accel_meas.push_back({accel_meas, time_of_validity}); + } + { + const jcc::Vec3 gyro_radps(msg.gyro_radps_x, msg.gyro_radps_y, msg.gyro_radps_z); + ejf::GyroMeasurement gyro_meas; + gyro_meas.observed_w = gyro_radps; + meas->imu_cal[imu_id].gyro_meas.push_back({gyro_meas, time_of_validity}); + } + + { + const jcc::Vec3 mag_utesla(msg.mag_utesla_x, msg.mag_utesla_y, msg.mag_utesla_z); + estimation::MagnetometerMeasurement mag_meas; + mag_meas.observed_bfield = mag_utesla; + meas->imu_cal[imu_id].mag_meas.push_back({mag_meas, time_of_validity}); + } +} + +void add_fiducial(const FiducialDetectionMessage& msg, + const TimeRange& range, + Out meas) { + const auto time_of_validity = to_time_point(msg.timestamp); + + if ((time_of_validity < range.start) || (time_of_validity > range.end)) { + return; + } + + const ejf::FiducialMeasurement fiducial_meas{ + .fiducial_id = 1, // + .T_fiducial_from_camera = msg.fiducial_from_camera() // + }; + + meas->fiducial_meas.push_back({fiducial_meas, time_of_validity}); +} + +} // namespace + +estimation::calibration::CalibrationMeasurements extract_data_from_log(const std::string& log_path, + const TimeRange& time_range, + const ExtractionConfiguration& cfg) { + estimation::calibration::CalibrationMeasurements meas; + + const std::vector channel_names = {"imu", "fiducial_detection_channel", "camera_image_channel", "imu_1", + "imu_2"}; + + jet::LogReader reader(log_path, channel_names); + + const auto channels_list = reader.get_available_channels(); + const bool one_imu_present = std::find(channels_list.begin(), channels_list.end(), "imu") != channels_list.end(); + + const std::vector single_imu({{"imu"}}); + const std::vector both_imu({{"imu_1"}, {"imu_2"}}); + + const std::vector imu_channels = one_imu_present ? single_imu : both_imu; + + // const bool two_imus = !one_imu_present; + // Read until we have read all IMU messages + bool got_anything = true; + while (got_anything) { + got_anything = false; + if (cfg.use_imu) { + for (const auto& imu_channel : imu_channels) { + ImuMessage imu_msg; + if (reader.read_next_message(imu_channel, imu_msg)) { + add_imu(imu_msg, time_range, out(meas)); + got_anything = true; + } else { + break; + } + } + } + + if (cfg.use_fiducial_detections) { + FiducialDetectionMessage fiducial_msg; + if (reader.read_next_message("fiducial_detection_channel", fiducial_msg)) { + add_fiducial(fiducial_msg, time_range, out(meas)); + got_anything = true; + } + } + } + + for (auto& field : meas.imu_cal) { + field.second.set_first(meas.first()); + field.second.set_last(meas.last()); + } + + return meas; +} + +class ImageStream::Nexter { + public: + Nexter(const std::string& log_path) { + const std::vector ch_nms = {"camera_image_channel"}; + reader_ = std::make_shared(log_path, ch_nms); + } + + bool get_next_image(Out msg) { + return reader_->read_next_message("camera_image_channel", *msg); + } + + private: + std::shared_ptr reader_; +}; + +ImageStream::ImageStream(const std::string& log_path, const TimeRange& time_range) : range_(time_range) { + nexter_ = std::make_shared(log_path); +} + +jcc::Optional ImageStream::next() { + CameraImageMessage img_msg; + if (!nexter_->get_next_image(out(img_msg))) { + return {}; + } + + const auto time_of_validity = to_time_point(img_msg.timestamp); + + if ((time_of_validity < range_.start) || (time_of_validity > range_.end)) { + return {}; + } + + const cv::Mat image = get_image_mat(std::move(img_msg)); + return {{time_of_validity, image, img_msg.camera_serial_number}}; +} + +} // namespace filtering +} // namespace jet diff --git a/filtering/extract_data_from_log.hh b/filtering/extract_data_from_log.hh new file mode 100644 index 0000000..af713ad --- /dev/null +++ b/filtering/extract_data_from_log.hh @@ -0,0 +1,39 @@ +#pragma once + +#include "third_party/experiments/estimation/calibration/calibration_dataset.hh" +#include "third_party/experiments/estimation/time_point.hh" +#include "third_party/experiments/util/optional.hh" + +#include + +namespace ejf = estimation::jet_filter; +namespace jet { +namespace filtering { + +struct TimeRange { + estimation::TimePoint start = estimation::TimePoint::min(); + estimation::TimePoint end = estimation::TimePoint::max(); +}; + +struct ExtractionConfiguration { + bool use_imu = true; + bool use_fiducial_detections = true; +}; + +estimation::calibration::CalibrationMeasurements extract_data_from_log(const std::string& log_path, + const TimeRange& time_range, + const ExtractionConfiguration& cfg = {}); + +class ImageStream { + public: + ImageStream(const std::string& log_path, const TimeRange& time_range); + jcc::Optional next(); + + private: + class Nexter; + std::shared_ptr nexter_; + TimeRange range_; +}; + +} // namespace filtering +} // namespace jet \ No newline at end of file diff --git a/filtering/filter_balsaq.cc b/filtering/filter_balsaq.cc index 765ea5e..d75a4ff 100644 --- a/filtering/filter_balsaq.cc +++ b/filtering/filter_balsaq.cc @@ -47,7 +47,9 @@ void FilterBq::init(const Config& config) { void FilterBq::loop() { FiducialDetectionMessage detection_msg; - std::cout << "Reading fiducial" << std::endl; + // + // Update with fiducial measurements + // if (fiducial_sub_->read(detection_msg, 1)) { estimation::jet_filter::FiducialMeasurement fiducial_meas; const SE3 fiducial_from_camera = detection_msg.fiducial_from_camera(); @@ -59,58 +61,64 @@ void FilterBq::loop() { if (!jf_.initialized()) { std::cout << "Initializing" << std::endl; auto xp0 = estimation::jet_filter::JetFilter::reasonable_initial_state(); - xp0.x.T_body_from_world = fiducial_meas.T_fiducial_from_camera.inverse(); + xp0.x.R_world_from_body = fiducial_meas.T_fiducial_from_camera.so3(); + xp0.x.x_world = fiducial_meas.T_fiducial_from_camera.translation(); xp0.time_of_validity = fiducial_time_of_validity; jf_.reset(xp0); gonogo().go(); } - std::cout << "Free-running" << std::endl; jf_.measure_fiducial(fiducial_meas, fiducial_time_of_validity); - jf_.free_run(); const auto state = jf_.state().x; } + // + // Update with IMU measurements + // ImuMessage imu_msg; bool got_imu_msg = false; - std::cout << "Reading old imu messages" << std::endl; while (imu_sub_->read(imu_msg, 1)) { + got_imu_msg = true; } + // if (jf_.initialized() && got_imu_msg) { + // const auto time_of_validity = to_time_point(imu_msg.timestamp); + // const jcc::Vec3 gyro_radps(imu_msg.gyro_radps_x, imu_msg.gyro_radps_y, imu_msg.gyro_radps_z); + // estimation::jet_filter::GyroMeasurement gyro_meas; + // gyro_meas.observed_w = gyro_radps; + // jf_.measure_gyro(gyro_meas, time_of_validity); + // } + + if (jf_.initialized()) { + jf_.free_run(); + } + + // + // Report a state + // if (jf_.initialized() && got_imu_msg) { const auto current_time = to_time_point(imu_msg.timestamp); - std::cout << "Generating view" << std::endl; const auto state = jf_.view(current_time); - std::cout << "eps_dot: " << state.eps_dot.transpose() << std::endl; - Pose pose; { const SE3 vehicle_real_from_vehicle_filtered; - pose.world_from_jet = (vehicle_real_from_vehicle_filtered * state.T_body_from_world).inverse(); + const SE3 T_world_from_body = estimation::jet_filter::get_world_from_body(state); + pose.world_from_jet = T_world_from_body * vehicle_real_from_vehicle_filtered.inverse(); const jcc::Vec3 log_translation_world_from_vehicle = pose.world_from_jet.translation(); const jcc::Vec3 log_rotation_world_from_vehicle = pose.world_from_jet.so3().log(); - std::cout << "log_translation_world_from_vehicle: [" << log_translation_world_from_vehicle[0] << ", " - << log_translation_world_from_vehicle[1] << ", " << log_translation_world_from_vehicle[2] << "] " - << std::endl; - - std::cout << "log_rotation_world_from_vehicle: [" << log_rotation_world_from_vehicle[0] << ", " - << log_rotation_world_from_vehicle[1] << ", " << log_rotation_world_from_vehicle[2] << "] " - << std::endl; - - pose.v_world_frame = (vehicle_real_from_vehicle_filtered.so3() * state.T_body_from_world.so3()).inverse() * + pose.v_world_frame = (vehicle_real_from_vehicle_filtered.so3() * state.R_world_from_body.inverse()).inverse() * state.eps_dot.head<3>(); - pose.w_world_frame = (vehicle_real_from_vehicle_filtered.so3() * state.T_body_from_world.so3()).inverse() * + pose.w_world_frame = (vehicle_real_from_vehicle_filtered.so3() * state.R_world_from_body.inverse()).inverse() * state.eps_dot.tail<3>(); pose.timestamp = imu_msg.timestamp; } PoseMessage pose_msg = PoseMessage::from_pose(pose); - std::cout << "Transmitting" << std::endl; pose_pub_->publish(pose_msg); } else { std::cout << "No means to queue time" << std::endl; diff --git a/filtering/filter_demo.cc b/filtering/filter_demo.cc deleted file mode 100644 index 5f31ee4..0000000 --- a/filtering/filter_demo.cc +++ /dev/null @@ -1,502 +0,0 @@ -#include - -//%deps(opencv) -#include - -#include "camera/camera_image_message.hh" -#include "embedded/imu_driver/imu_message.hh" -#include "infrastructure/logging/log_reader.hh" - -//%deps(fiducial_pose) -//%deps(rotation_to) -#include "third_party/experiments/estimation/vision/fiducial_pose.hh" -#include "third_party/experiments/geometry/rotation_to.hh" - -//%deps(jet_filter) -//%deps(simple_geometry) -//%deps(jet_optimizer) -//%deps(window_3d) -#include "third_party/experiments/estimation/jet/jet_filter.hh" -#include "third_party/experiments/estimation/jet/jet_optimizer.hh" -#include "third_party/experiments/viewer/primitives/simple_geometry.hh" -#include "third_party/experiments/viewer/window_3d.hh" - -//%deps(time_interpolator) -#include "third_party/experiments/geometry/spatial/time_interpolator.hh" - -//%deps(fit_ellipse) -#include "third_party/experiments/geometry/shapes/fit_ellipse.hh" - -// -#include "camera/camera_manager.hh" -#include "vision/fiducial_detection_and_pose.hh" -#include "vision/fiducial_detection_message.hh" - -namespace jet { -namespace filtering { -namespace { -// TODO FACTOR -void draw_states(viewer::SimpleGeometry& geo, const std::vector& states, bool truth) { - const int n_states = static_cast(states.size()); - for (int k = 0; k < n_states; ++k) { - auto& state = states.at(k); - const SE3 T_world_from_body = state.T_body_from_world.inverse(); - if (truth) { - geo.add_axes({T_world_from_body, 0.1}); - } else { - geo.add_axes({T_world_from_body, 0.01, 1.0, true}); - if (k < n_states - 1) { - const auto& next_state = states.at(k + 1); - const SE3 T_world_from_body_next = next_state.T_body_from_world.inverse(); - geo.add_line({T_world_from_body.translation(), T_world_from_body_next.translation()}); - } - } - } -} - -void setup() { - const auto view = viewer::get_window3d("Filter Debug"); - // view->set_azimuth(0.0); - // view->set_elevation(0.0); - // view->set_zoom(1.0); - view->set_target_from_world(SE3(SO3::exp(Eigen::Vector3d(-3.1415 * 0.5, 0.0, 0.0)), jcc::Vec3(-1.0, 0.0, -1.0))); - view->set_continue_time_ms(10); - const auto background = view->add_primitive(); - const geometry::shapes::Plane ground{jcc::Vec3::UnitZ(), 0.0}; - background->add_plane({ground, 0.1}); - background->flip(); -} - -estimation::TimePoint to_time_point(const Timestamp& ts) { - const auto epoch_offset = std::chrono::nanoseconds(uint64_t(ts)); - const estimation::TimePoint time_point = estimation::TimePoint{} + epoch_offset; - return time_point; -} - -class Calibrator { - public: - Calibrator() { - const auto view = viewer::get_window3d("Filter Debug"); - geo_ = view->add_primitive(); - } - - void maybe_add_imu(const ImuMessage& msg) { - const auto time_of_validity = to_time_point(msg.timestamp); - if (estimation::to_seconds(time_of_validity - earliest_camera_time_) > 25.0) { - return; - } - // if (timestamp > earliest_camera_time_) { - if (true) { - // std::cout << "Accel:" << uint64_t(msg.timestamp) << std::endl; - const jcc::Vec3 accel_mpss(msg.accel_mpss_x, msg.accel_mpss_y, msg.accel_mpss_z); - - estimation::jet_filter::AccelMeasurement accel_meas; - accel_meas.observed_acceleration = accel_mpss; - accel_meas_.push_back({accel_meas, time_of_validity}); - - const jcc::Vec3 gyro_radps(msg.gyro_radps_x, msg.gyro_radps_y, msg.gyro_radps_z); - estimation::jet_filter::GyroMeasurement gyro_meas; - gyro_meas.observed_w = gyro_radps; - gyro_meas_.push_back({gyro_meas, time_of_validity + estimation::to_duration(0.000001)}); - - const jcc::Vec3 mag_utesla(msg.mag_utesla_x, msg.mag_utesla_y, msg.mag_utesla_z); - mag_utesla_.push_back({mag_utesla, time_of_validity}); - geo_->add_point({(mag_utesla)}); - geo_->flush(); - } - // Otherwise, ignore it - } - - void add_fiducial(const Timestamp& ts, const SE3& world_from_camera) { - const auto time_of_validity = to_time_point(ts); - if (!got_camera_) { - auto xp0 = estimation::jet_filter::JetFilter::reasonable_initial_state(); - - xp0.x.T_body_from_world = world_from_camera.inverse(); - xp0.time_of_validity = time_of_validity; - jf_.reset(xp0); - - earliest_camera_time_ = time_of_validity; - } - // std::cout << "cam: " << uint64_t(ts) << std::endl; - if (estimation::to_seconds(time_of_validity - earliest_camera_time_) > 25.0) { - return; - } - - got_camera_ = true; - - estimation::jet_filter::FiducialMeasurement fiducial_meas; - fiducial_meas.T_fiducial_from_camera = world_from_camera; - - // jf_.measure_fiducial(fiducial_meas, time_of_validity); - // jet_opt_.measure_fiducial(fiducial_meas, time_of_validity); - fiducial_meas_.push_back({fiducial_meas, time_of_validity}); - - geo_->add_axes({world_from_camera, 0.025, 3.0}); - - const auto view = viewer::get_window3d("Filter Debug"); - - geo_->flush(); - } - - geometry::spatial::TimeInterpolator fit_mag() const { - std::vector measurements; - const auto view = viewer::get_window3d("Filter Debug"); - for (const auto& pt : mag_utesla_) { - measurements.push_back(pt.first); - } - - const auto ell_geo = view->add_primitive(); - const auto visitor = [&ell_geo, &view](const geometry::shapes::EllipseFit& fit) { - ell_geo->add_ellipsoid({fit.ellipse, jcc::Vec4(0.4, 0.6, 0.4, 0.7), 2.0}); - ell_geo->flip(); - }; - const auto result = geometry::shapes::fit_ellipse(measurements, visitor); - - ell_geo->add_ellipsoid({result.ellipse, jcc::Vec4(0.2, 1.0, 0.2, 1.0), 4.0}); - ell_geo->flip(); - view->spin_until_step(); - - std::vector control_points; - for (const auto& pt : mag_utesla_) { - const auto pt_time = pt.second; - - const jcc::Vec3 pt_mag_utesla = pt.first; - - const jcc::Vec3 pt_mag_corrected = - (result.ellipse.cholesky_factor.transpose().inverse() * (pt_mag_utesla - result.ellipse.p0)); - - ell_geo->add_point({pt_mag_corrected, jcc::Vec4(1.0, 0.2, 0.3, 1.0)}); - - control_points.push_back({pt_time, pt_mag_corrected}); - } - - ell_geo->flip(); - - const geometry::spatial::TimeInterpolator interpolator(control_points); - return interpolator; - } - - geometry::spatial::TimeInterpolator make_accel_interpolator() const { - std::vector points; - for (const auto& measurement : accel_meas_) { - points.push_back({measurement.second, measurement.first.observed_acceleration}); - } - const geometry::spatial::TimeInterpolator interp(points); - return interp; - } - - geometry::spatial::TimeInterpolator make_gyro_interpolator() const { - std::vector points; - for (const auto& measurement : gyro_meas_) { - points.push_back({measurement.second, measurement.first.observed_w}); - } - const geometry::spatial::TimeInterpolator interp(points); - return interp; - } - - void prepare() { - const auto view = viewer::get_window3d("Filter Debug"); - - const auto mag_interpolator = fit_mag(); - const auto accel_interpolator = make_accel_interpolator(); - - const SE3 imu_from_vehicle = jf_.parameters().T_imu_from_vehicle; - - for (const auto& fiducial_meas : fiducial_meas_) { - const SE3 world_from_vehicle = fiducial_meas.first.T_fiducial_from_camera; - const SE3 imu_from_world = imu_from_vehicle * world_from_vehicle.inverse(); - - // geo_->add_axes({imu_from_world.inverse(), 0.01}); - - const auto fiducial_measurement_t = fiducial_meas.second; - - const auto maybe_interp_at_t = accel_interpolator(fiducial_measurement_t); - if (!maybe_interp_at_t) { - std::cout << "Failed" << std::endl; - continue; - } - const jcc::Vec3 accel_meas_imu_at_t = *maybe_interp_at_t; - const jcc::Vec3 accel_meas_vehicle_frame = imu_from_vehicle.so3().inverse() * accel_meas_imu_at_t; - - const jcc::Vec3 mag_meas_vehicle_frame = imu_from_vehicle.so3() * (*mag_interpolator(fiducial_measurement_t)); - - constexpr double M_PER_MPSS = 0.01; - const jcc::Vec4 imu_obs_color(0.1, 0.7, 0.7, 0.4); - geo_->add_line({world_from_vehicle.translation(), world_from_vehicle * (accel_meas_vehicle_frame * M_PER_MPSS), - imu_obs_color}); - - /* - const jcc::Vec3 g_vehicle_frame = (world_from_vehicle.so3().inverse() * (jcc::Vec3::UnitZ() * 9.81)); - const jcc::Vec3 accel_g_subtracted_vehicle = accel_meas_vehicle_frame - g_vehicle_frame; - const jcc::Vec3 imu_direction = world_from_vehicle.so3() * (accel_g_subtracted_vehicle).normalized(); - const double accel_norm_no_g = accel_g_subtracted_vehicle.norm() * M_PER_MPSS; - geo_->add_ray({world_from_vehicle.translation(), imu_direction, accel_norm_no_g, imu_obs_color, 3.0}); - */ - - const jcc::Vec4 mag_obs_color(1.0, 0.3, 0.3, 0.4); - geo_->add_ray({world_from_vehicle.translation(), world_from_vehicle.so3() * mag_meas_vehicle_frame.normalized(), - 1.0, mag_obs_color, 3.0}); - - const double cos_angle_grav_bfield = - accel_meas_vehicle_frame.normalized().dot(mag_meas_vehicle_frame.normalized()); - - // geo_->add_ray( - // {jcc::Vec3::Zero(), world_from_vehicle.so3() * mag_meas_vehicle_frame.normalized(), 1.0, mag_obs_color, 3.0}); - } - view->spin_until_step(); - geo_->flush(); - } - - std::vector test_filter() { - const auto view = viewer::get_window3d("Filter Debug"); - const auto accel_interpolator = make_accel_interpolator(); - const auto gyro_interpolator = make_gyro_interpolator(); - - const SE3 imu_from_vehicle = jf_.parameters().T_imu_from_vehicle; - - std::vector est_states; - geo_->clear(); - - const auto first_t = fiducial_meas_.front().second; - for (const auto& fiducial_meas : fiducial_meas_) { - const estimation::TimePoint t = fiducial_meas.second; - if (t < first_t + estimation::to_duration(0.5)) { - continue; - } - jf_.measure_fiducial(fiducial_meas.first, t); - jet_opt_.measure_fiducial(fiducial_meas.first, t); - - std::cout << "Camera Location: " << fiducial_meas.first.T_fiducial_from_camera.translation().transpose() - << std::endl; - - geo_->add_axes({fiducial_meas.first.T_fiducial_from_camera, 0.001, 3.0}); - } - - for (const auto& accel_meas : accel_meas_) { - const estimation::TimePoint t = accel_meas.second; - if (t <= (first_t + estimation::to_duration(3.0))) { - std::cout << "Skipping" << std::endl; - continue; - } - // jf_.measure_imu(accel_meas.first, t); - // jet_opt_.measure_imu(accel_meas.first, t); - } - - estimation::TimePoint prev_time; - - while (true) { - std::cout << "\n" << std::endl; - const auto maybe_state = jf_.next_measurement(); - if (!maybe_state) { - break; - } - - const auto state = jf_.state().x; - est_states.push_back(state); - - const auto cov = jf_.state().P; - const Eigen::LLT> P_llt( - cov.block<3, 3>(estimation::jet_filter::StateDelta::T_body_from_world_error_log_ind, - estimation::jet_filter::StateDelta::T_body_from_world_error_log_ind)); - - const auto t = jf_.state().time_of_validity; - - std::cout << "dt: " << estimation::to_seconds(t - prev_time) << std::endl; - prev_time = t; - - const SE3 T_world_from_body = state.T_body_from_world.inverse(); - - constexpr double M_PER_MPSS = 0.01; - - const jcc::Vec3 meas_accel_imu_t = *accel_interpolator(t); - const jcc::Vec3 expected_accel_imu = observe_accel(state, jf_.parameters()).observed_acceleration; - - const jcc::Vec3 meas_gyro_imu_t = *gyro_interpolator(t); - const jcc::Vec3 expected_gyro_imu = observe_gyro(state, jf_.parameters()).observed_w; - /* - geo_->add_line({T_world_from_body.translation(), T_world_from_body * (meas_accel_imu_t * M_PER_MPSS), - jcc::Vec4(1.0, 0.0, 0.0, 0.8)}); - - geo_->add_line({T_world_from_body.translation(), T_world_from_body * (expected_accel_imu * M_PER_MPSS), - jcc::Vec4(0.0, 1.0, 0.0, 0.8)}); - */ - const jcc::Vec3 g_world = jcc::Vec3::UnitZ() * 9.81; - const jcc::Vec3 g_vehicle_frame = (T_world_from_body.so3().inverse() * g_world); - - // const jcc::Vec3 accel_g_subtracted_vehicle = meas_accel_imu_t - g_vehicle_frame; - const jcc::Vec3 g_imu = imu_from_vehicle.so3() * T_world_from_body.so3().inverse() * g_world; - - std::cout << "\n" << std::endl; - // std::cout << "g_body:" << g_vehicle_frame.transpose() << std::endl; - std::cout << "g_imu: " << g_imu.transpose() << std::endl; - // std::cout << "accel : " << accel_g_subtracted_vehicle.transpose() << std::endl; - std::cout << "meas accel: " << meas_accel_imu_t.transpose() << std::endl; - std::cout << "exp accel: " << expected_accel_imu.transpose() << std::endl; - - std::cout << "meas sp f: " << (meas_accel_imu_t - g_imu).transpose() << std::endl; - std::cout << "exp sp f: " << (expected_accel_imu - g_imu).transpose() << std::endl; - - std::cout << "meas gyro: " << meas_gyro_imu_t.transpose() << std::endl; - std::cout << "exp gyro: " << expected_gyro_imu.transpose() << std::endl; - - std::cout << "accel_bias: " << state.accel_bias.transpose() << std::endl; - std::cout << "eps_ddot: " << state.eps_ddot.transpose() << std::endl; - std::cout << "eps_dot: " << state.eps_dot.transpose() << std::endl; - - geo_->add_axes({T_world_from_body, 0.01, 1.0, true}); - geo_->add_ellipsoid({geometry::shapes::Ellipse{P_llt.matrixU(), T_world_from_body.translation()}}); - - geo_->flush(); - view->spin_until_step(); - } - return est_states; - } - - void run() { - prepare(); - const std::vector est_states = test_filter(); - - const auto view = viewer::get_window3d("Filter Debug"); - - std::cout << "Calibrating" << std::endl; - const auto visitor = make_visitor(); - const auto solution = jet_opt_.solve(est_states, jf_.parameters(), visitor); - } - - private: - estimation::jet_filter::JetPoseOptimizer::Visitor make_visitor() { - const auto view = viewer::get_window3d("Filter Debug"); - const auto visitor_geo = view->add_primitive(); - const auto visitor = [view, visitor_geo](const estimation::jet_filter::JetPoseOptimizer::Solution& soln) { - visitor_geo->clear(); - draw_states(*visitor_geo, soln.x, false); - visitor_geo->flip(); - // std::cout << "\tOptimized g: " << soln.p.g_world.transpose() << std::endl; - std::cout << "\tOptimized T_imu_from_vehicle: " << soln.p.T_imu_from_vehicle.translation().transpose() << "; " - << soln.p.T_imu_from_vehicle.so3().log().transpose() << std::endl; - view->spin_until_step(); - }; - return visitor; - } - - estimation::TimePoint earliest_camera_time_ = estimation::TimePoint::max(); - bool got_camera_ = false; - - std::vector> accel_meas_; - std::vector> fiducial_meas_; - std::vector> gyro_meas_; - - std::vector> mag_utesla_; - - estimation::jet_filter::JetFilter jf_; - estimation::jet_filter::JetOptimizer jet_opt_; - - // temp - std::shared_ptr geo_; -}; - -} // namespace - -void go() { - setup(); - CameraManager camera_manager_ = CameraManager(); - const auto view = viewer::get_window3d("Filter Debug"); - const auto geo = view->add_primitive(); - const std::vector channel_names = {"imu", "fiducial_detection_channel", "camera_image_channel"}; - - // const std::string path = "/jet/logs/calibration-log-jan26-1"; - const std::string path = "/jet/logs/calibration-log-jan31-1"; - // const std::string path = "/jet/logs/calibration-log-feb9-1"; - // const std::string path = "/jet/logs/calibration-log-feb9-2"; - - Calibrator calibrator; - jet::LogReader reader(path, channel_names); - - bool accepted_any = false; - SE3 last_world_from_camera; - - constexpr bool USE_CAMERA_IMAGES = false; - constexpr bool USE_FIDUCIAL_DETECTIONS = true; - - int imu_ct = 0; - for (int k = 0; k < 3000; ++k) { - { - ImuMessage imu_msg; - if (reader.read_next_message("imu", imu_msg)) { - imu_ct++; - // if (imu_ct % 1 == 0) { - if (imu_ct) { - calibrator.maybe_add_imu(imu_msg); - } - } else { - std::cout << "Breaking at : " << k << std::endl; - break; - } - } - - /* - if (USE_CAMERA_IMAGES) { - CameraImageMessage cam_msg; - if (reader.read_next_message("camera_image_channel", cam_msg)) { - Calibration camera_calibration = camera_manager_.get_camera(cam_msg.camera_serial_number).calibration; - const auto image = get_image_mat(cam_msg); - - const auto result = - estimate_board_center_from_camera_from_image(get_ids_and_corners(image), camera_calibration); - if (result) { - const SE3 world_from_camera = *result; - - if (accepted_any) { - const SE3 camera_from_last_camera = world_from_camera.inverse() * last_world_from_camera; - constexpr double MAX_OUTLIER_DIST_M = 0.7; - if (camera_from_last_camera.translation().norm() > MAX_OUTLIER_DIST_M) { - continue; - } - } - - accepted_any = true; - last_world_from_camera = world_from_camera; - - calibrator.add_fiducial(cam_msg.timestamp, world_from_camera); - } - } - } - */ - - if (USE_FIDUCIAL_DETECTIONS) { - FiducialDetectionMessage fiducial_msg; - if (reader.read_next_message("fiducial_detection_channel", fiducial_msg)) { - const SE3 world_from_camera = fiducial_msg.fiducial_from_camera(); - - // if (accepted_any) { - // const SE3 camera_from_last_camera = world_from_camera.inverse() * last_world_from_camera; - // constexpr double MAX_OUTLIER_DIST_M = 0.7; - // if (camera_from_last_camera.translation().norm() > MAX_OUTLIER_DIST_M) { - // continue; - // } - // } - - calibrator.add_fiducial(fiducial_msg.timestamp, world_from_camera); - accepted_any = true; - last_world_from_camera = world_from_camera; - } - } - // cv::imshow("Image", image); - // cv::waitKey(0); - } - - geo->flush(); - view->spin_until_step(); - std::cout << "Done, preparing to calibrate" << std::endl; - - calibrator.run(); - view->spin_until_step(); -} - -} // namespace filtering -} // namespace jet - -int main() { - jet::filtering::go(); -} diff --git a/filtering/filter_viz_balsaq.cc b/filtering/filter_viz_balsaq.cc index e2cbbc5..b4b3677 100644 --- a/filtering/filter_viz_balsaq.cc +++ b/filtering/filter_viz_balsaq.cc @@ -15,7 +15,7 @@ // %deps(simple_geometry) // %deps(window_3d) -//%deps(fit_ellipse) +// %deps(fit_ellipse) #include "third_party/experiments/estimation/time_point.hh" #include "third_party/experiments/geometry/shapes/fit_ellipse.hh" #include "third_party/experiments/viewer/primitives/simple_geometry.hh" @@ -44,12 +44,10 @@ void FilterVizBq::init(const Config& config) { sensor_geo_ = view->add_primitive(); pose_geo_ = view->add_primitive(); - persistent_ = view->add_primitive(); servo_geo_ = view->add_primitive(); std::cout << "Subscribing IMU" << std::endl; imu_sub_ = make_subscriber("imu"); - std::cout << "Subscribing Fiducial" << std::endl; fiducial_sub_ = make_subscriber("fiducial_detection_channel"); @@ -71,7 +69,7 @@ void FilterVizBq::draw_sensors() { ImuMessage imu_msg; FiducialDetectionMessage detection_msg; - if (fiducial_sub_->read(detection_msg, 1)) { + if (true && fiducial_sub_->read(detection_msg, 1)) { const SE3 fiducial_from_camera = detection_msg.fiducial_from_camera(); const SE3 fiducial_from_body = fiducial_from_camera * camera_from_body_; @@ -89,19 +87,54 @@ void FilterVizBq::draw_sensors() { fiducial_history_.push_back(fiducial_from_camera); } + MatNd<3, 3> L; + { // clang-format off + L.row(0) << 9.67735, 0, 0; + L.row(1) << 0.136597, 9.59653, 0; + L.row(2) << -0.216635, 0.00400047, 9.64812; + // clang-format on + } + const jcc::Vec3 offset(0.0562102, 0.42847, -0.119841); + const geometry::shapes::Ellipse ellipse{L, offset}; + while (imu_sub_->read(imu_msg, 1)) { const jcc::Vec3 accel_mpss(imu_msg.accel_mpss_x, imu_msg.accel_mpss_y, imu_msg.accel_mpss_z); const jcc::Vec3 mag_utesla(imu_msg.mag_utesla_x, imu_msg.mag_utesla_y, imu_msg.mag_utesla_z); const jcc::Vec3 gyro_radps(imu_msg.gyro_radps_x, imu_msg.gyro_radps_y, imu_msg.gyro_radps_z); - accel_history_.push_back({accel_mpss, gyro_radps, mag_utesla}); + accel_history_.push_back( + {geometry::shapes::deform_ellipse_to_unit_sphere(accel_mpss, ellipse) * 9.81, gyro_radps, mag_utesla}); mag_utesla_.push_back({mag_utesla}); + + std::cout << accel_mpss.transpose() << ", " << accel_mpss.norm() << std::endl; } - while (accel_history_.size() > 15u) { + while (accel_history_.size() > 25000u) { accel_history_.pop_front(); } + std::vector accels; + accels.reserve(accel_history_.size()); + for (const auto& meas : accel_history_) { + accels.push_back(meas.accel_mpss); + sensor_geo_->add_point({meas.accel_mpss, jcc::Vec4(0.1, 0.7, 0.3, 0.9)}); + } + + if (accel_history_.size() > 2500u) { + const auto visitor = [this, &view](const geometry::shapes::EllipseFit& fit) { + sensor_geo_->add_ellipsoid({fit.ellipse, jcc::Vec4(0.4, 0.6, 0.4, 0.7), 2.0}); + sensor_geo_->flush(); + }; + // std::cout << "Optimizing" << std::endl; + const auto result = geometry::shapes::fit_ellipse(accels); + sensor_geo_->add_ellipsoid({result.ellipse, jcc::Vec4(0.2, 0.9, 0.2, 1.0), 5.0}); + + std::cerr << "chol" << std::endl; + std::cerr << result.ellipse.cholesky_factor << std::endl; + std::cerr << "p0" << std::endl; + std::cerr << result.ellipse.p0.transpose() << std::endl; + } + while (fiducial_history_.size() > 10u) { fiducial_history_.pop_front(); } @@ -110,19 +143,17 @@ void FilterVizBq::draw_sensors() { mag_utesla_.pop_front(); } - // Tag in world frame sensor_geo_->add_sphere({tag_from_world_.inverse().translation(), 0.3}); sensor_geo_->add_axes({tag_from_world_.inverse()}); - // Camera in body frame - // sensor_geo_->add_sphere({camera_from_body_.inverse().translation(), 0.3, jcc::Vec4(0.0, 1.0, 1.0, 1.0)}); - // sensor_geo_->add_axes({camera_from_body_.inverse()}); + sensor_geo_->add_sphere({camera_from_body_.inverse().translation(), 0.3}); + sensor_geo_->add_axes({camera_from_body_.inverse()}); if (!fiducial_history_.empty()) { const SE3 fiducial_from_camera = fiducial_history_.back(); - constexpr bool DRAW_FIDUCIAL_POSE = true; - constexpr bool DRAW_VEHICLE_POSE = false; + constexpr bool DRAW_FIDUCIAL_POSE = false; + constexpr bool DRAW_VEHICLE_POSE = true; constexpr bool DRAW_FIDUCIAL_IN_BODY_FRAME = false; if (DRAW_FIDUCIAL_POSE) { // Draw the fiducial axes in the camera frame diff --git a/filtering/filter_viz_balsaq.hh b/filtering/filter_viz_balsaq.hh index 9c2c4c6..4abf2b4 100644 --- a/filtering/filter_viz_balsaq.hh +++ b/filtering/filter_viz_balsaq.hh @@ -42,13 +42,12 @@ class FilterVizBq : public BalsaQ { std::shared_ptr pose_geo_; std::shared_ptr servo_geo_; - std::shared_ptr persistent_; - struct AccelStuff { jcc::Vec3 accel_mpss; jcc::Vec3 gyro_radps; jcc::Vec3 mag_utesla; }; + std::deque accel_history_; std::deque fiducial_history_; std::deque mag_utesla_; diff --git a/filtering/to_time_point.cc b/filtering/to_time_point.cc new file mode 100644 index 0000000..ce13982 --- /dev/null +++ b/filtering/to_time_point.cc @@ -0,0 +1,10 @@ +#include "filtering/to_time_point.hh" + +namespace jet { + +estimation::TimePoint to_time_point(const Timestamp& ts) { + const auto epoch_offset = std::chrono::nanoseconds(uint64_t(ts)); + const estimation::TimePoint time_point = estimation::TimePoint{} + epoch_offset; + return time_point; +} +} // namespace jet diff --git a/filtering/to_time_point.hh b/filtering/to_time_point.hh new file mode 100644 index 0000000..c0e1b54 --- /dev/null +++ b/filtering/to_time_point.hh @@ -0,0 +1,6 @@ +#include "infrastructure/time/timestamp.hh" +#include "third_party/experiments/estimation/time_point.hh" + +namespace jet { +estimation::TimePoint to_time_point(const Timestamp& ts); +} // namespace jet diff --git a/infrastructure/gonogo/gonogo_bq.cc b/infrastructure/gonogo/gonogo_bq.cc index 9f1babc..6762a91 100644 --- a/infrastructure/gonogo/gonogo_bq.cc +++ b/infrastructure/gonogo/gonogo_bq.cc @@ -1,4 +1,6 @@ //%bin(gonogo_bq_main) +//%deps(balsa_queue) +//%deps(message) #include "infrastructure/gonogo/gonogo_bq.hh" #include "infrastructure/balsa_queue/bq_main_macro.hh" diff --git a/third_party/experiments b/third_party/experiments index c056c6c..d19fe27 160000 --- a/third_party/experiments +++ b/third_party/experiments @@ -1 +1 @@ -Subproject commit c056c6c7503d087266e871fb68104d58c2fbf6b6 +Subproject commit d19fe279602e46ee922e2c259a4e0a91420c2342 diff --git a/vision/fiducial_detection_and_pose.cc b/vision/fiducial_detection_and_pose.cc index 576aba5..3c8cfef 100644 --- a/vision/fiducial_detection_and_pose.cc +++ b/vision/fiducial_detection_and_pose.cc @@ -26,7 +26,8 @@ BoardIdsAndCorners get_ids_and_corners(const cv::Mat& input_image) { // multiple unique boards const auto t0 = time::get_current_time(); cv::aruco::detectMarkers(input_image, get_aruco_dictionary(), corners, ids, params); - std::cout << "to do cv::aruco::detectMarkers " << (float)(time::get_current_time() - t0) / 1000000 << "ms" << std::endl; + std::cout << "to do cv::aruco::detectMarkers " << (float)(time::get_current_time() - t0) / 1000000 << "ms" + << std::endl; BoardIdsAndCorners result = {ids, corners}; return result; } @@ -36,21 +37,18 @@ std::vector obj_points_img_points_from_image(co cv::aruco::getBoardObjectAndImagePoints(get_aruco_board(), ids_corners.corners, ids_corners.ids, boardPoints, imgPoints); std::vector result; - for (int i = 0; i < boardPoints.rows; i++) { + + for (int i = 0; i < board_points.rows; i++) { BoardPointImagePointAssociation association = {}; - boardPoints.at(i, 0); - std::cout << "in detector " << imgPoints.at(i, 0) << " " << imgPoints.at(i, 1) << std::endl; - association.point_board_space = jcc::Vec2(boardPoints.at(i, 0), boardPoints.at(i, 1)); - association.point_image_space = jcc::Vec2(imgPoints.at(i, 0), imgPoints.at(i, 1)); - std::cout << "in detector2" << association.point_image_space.vals[1] << " " << association.point_image_space.vals[0] - << std::endl; + association.point_board_space = jcc::Vec2(board_points.at(i, 0), board_points.at(i, 1)); + association.point_image_space = jcc::Vec2(img_points.at(i, 0), img_points.at(i, 1)); result.push_back(association); } return result; } -std::optional estimate_board_center_from_camera_from_image(const BoardIdsAndCorners& ids_corners, - const Calibration& calibration) { +std::optional estimate_board_bottom_left_from_camera(const BoardIdsAndCorners& ids_corners, + const Calibration& calibration) { const cv::Mat camera_matrix = calibration.camera_matrix; const cv::Mat distortion_coefficients = calibration.distortion_coefficients; diff --git a/vision/fiducial_detection_and_pose.hh b/vision/fiducial_detection_and_pose.hh index 781f5a1..a240b05 100644 --- a/vision/fiducial_detection_and_pose.hh +++ b/vision/fiducial_detection_and_pose.hh @@ -6,9 +6,9 @@ #include #include +#include "camera/camera_manager.hh" #include "third_party/experiments/eigen.hh" #include "third_party/experiments/sophus.hh" -#include "camera/camera_manager.hh" #include "infrastructure/comms/serialization/serialization_macros.hh" @@ -52,7 +52,7 @@ struct MatWrapper { } operator Mat() const { - const Eigen::Map mtx_mmap(vals); + const Eigen::Map mtx_mmap(vals.data()); Mat true_mtx(mtx_mmap); return true_mtx; } @@ -72,11 +72,12 @@ struct BoardPointImagePointAssociation { SERIALIZABLE_STRUCTURE(BoardPointImagePointAssociation, point_board_space, point_image_space); }; -BoardIdsAndCorners get_ids_and_corners(const cv::Mat &input_image); +BoardIdsAndCorners get_ids_and_corners(const cv::Mat& input_image); std::vector get_world_from_marker_centers(const cv::Mat& camera_image, const SE3& world_from_camera); -std::optional estimate_board_center_from_camera_from_image(const BoardIdsAndCorners& ids_corners, const Calibration& calibration); +std::optional estimate_board_bottom_left_from_camera(const BoardIdsAndCorners& ids_corners, + const Calibration& calibration); std::vector obj_points_img_points_from_image(const BoardIdsAndCorners& ids_corners); diff --git a/vision/fiducial_detection_message.cc b/vision/fiducial_detection_message.cc index 6766d32..0795105 100644 --- a/vision/fiducial_detection_message.cc +++ b/vision/fiducial_detection_message.cc @@ -8,11 +8,10 @@ SE3 FiducialDetectionMessage::fiducial_from_camera() const { } std::optional create_fiducial_detection_message(const cv::Mat& camera_frame, - const Calibration& camera_calibration, - const Timestamp& timestamp) { + const Calibration& camera_calibration, + const Timestamp& timestamp) { const auto ids_corners = get_ids_and_corners(camera_frame); - const std::optional board_from_camera = - estimate_board_center_from_camera_from_image(ids_corners, camera_calibration); + const std::optional board_from_camera = estimate_board_bottom_left_from_camera(ids_corners, camera_calibration); if (board_from_camera) { FiducialDetectionMessage detection_message; const jcc::Vec6 log_fiducial_from_camera = board_from_camera->log(); @@ -20,11 +19,11 @@ std::optional create_fiducial_detection_message(const detection_message.fiducial_from_camera_log[i] = log_fiducial_from_camera[i]; } detection_message.timestamp = timestamp; - const std::vector board_point_assocs = obj_points_img_points_from_image(ids_corners); + const std::vector board_point_assocs = + obj_points_img_points_from_image(ids_corners); detection_message.board_points_image_points = board_point_assocs; return detection_message; - } return {}; } From cb0c985bb24b5322d12afa569b0565bdeea7ebc2 Mon Sep 17 00:00:00 2001 From: Jacob Panikulam Date: Sun, 7 Jul 2019 18:42:39 -0400 Subject: [PATCH 09/13] Fix first round of merge conflicts --- CMakeLists.txt | 3 +- config/fiducial_map/read_fiducial_map.cc | 19 ++- config/fiducial_map/read_fiducial_map.hh | 8 +- config/hoverjet/controller.yaml | 9 ++ config/hoverjet/filter.yaml | 106 +++++++++++++ config/supervisor/supervisor.conf | 4 +- control/controller_balsaq.cc | 30 ++-- control/controller_balsaq.hh | 7 + control/jet_vane_model.hh | 3 +- filtering/calibrate_camera_from_log.cc | 11 +- filtering/calibrate_jet.cc | 193 ++++++++++++++--------- filtering/convert_messages.cc | 37 +++++ filtering/convert_messages.hh | 14 ++ filtering/extract_data_from_log.cc | 44 ++---- filtering/extract_data_from_log.hh | 6 +- filtering/filter_balsaq.cc | 135 +++++++--------- filtering/filter_balsaq.hh | 18 +-- filtering/filter_viz_balsaq.cc | 103 ++++-------- filtering/filter_viz_balsaq.hh | 6 +- filtering/imu_model_from_yaml.cc | 19 +++ filtering/imu_model_from_yaml.hh | 17 ++ filtering/measurement_sim_bq.cc | 106 +++++++++++++ filtering/measurement_sim_bq.hh | 43 +++++ filtering/pose_message.hh | 7 + filtering/transform_network_from_yaml.cc | 82 ++++++++++ filtering/transform_network_from_yaml.hh | 14 ++ filtering/yaml_matrix.hh | 39 +++++ filtering/yaml_matrix_test.cc | 35 ++++ infrastructure/comms/mqtt_subscriber.cc | 8 +- infrastructure/gonogo/gonogo.cc | 8 +- third_party/experiments | 2 +- vision/fiducial_detection_and_pose.cc | 15 +- vision/fiducial_detection_and_pose.hh | 2 +- vision/fiducial_detection_message.hh | 6 +- visualization/thrust_stand_visualizer.cc | 13 +- visualization/thrust_stand_visualizer.hh | 1 - 36 files changed, 835 insertions(+), 338 deletions(-) create mode 100644 config/hoverjet/controller.yaml create mode 100644 config/hoverjet/filter.yaml create mode 100644 filtering/convert_messages.cc create mode 100644 filtering/convert_messages.hh create mode 100644 filtering/imu_model_from_yaml.cc create mode 100644 filtering/imu_model_from_yaml.hh create mode 100644 filtering/measurement_sim_bq.cc create mode 100644 filtering/measurement_sim_bq.hh create mode 100644 filtering/transform_network_from_yaml.cc create mode 100644 filtering/transform_network_from_yaml.hh create mode 100644 filtering/yaml_matrix.hh create mode 100644 filtering/yaml_matrix_test.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 269f558..9870fc3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ FIND_PACKAGE(crossguid REQUIRED) # CMakeLists find_package(OpenCV REQUIRED) +include_directories(${OpenCV_INCLUDE_DIRS}) SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/run) @@ -29,7 +30,7 @@ if(ret EQUAL "1") endif() set(CMAKE_CXX_COMPILER "g++-7") -set(CMAKE_CXX_FLAGS "--std=c++17 -g -O1 -fno-omit-frame-pointer -Wall -Wno-deprecated-declarations ${CMAKE_CXX_FLAGS}") +set(CMAKE_CXX_FLAGS "--std=c++17 -g -O1 -fno-omit-frame-pointer -Wall -Wno-deprecated-declarations -Wno-psabi ${CMAKE_CXX_FLAGS}") # Wait! Don't edit this file! Use pymake of updating this cmake. diff --git a/config/fiducial_map/read_fiducial_map.cc b/config/fiducial_map/read_fiducial_map.cc index 55a17c2..3c8da70 100644 --- a/config/fiducial_map/read_fiducial_map.cc +++ b/config/fiducial_map/read_fiducial_map.cc @@ -3,14 +3,13 @@ namespace jet { - -fiducial_pose get_fiducial_pose() { +FiducialDescription get_fiducial_pose() { YAML::Node config = YAML::LoadFile("/jet/config/fiducial_map/fiducial_map.yaml"); auto const node = config["fiducial_5x5"]; - fiducial_pose result; + FiducialDescription result; const auto log_translation_tag_from_world = node["log_translation_tag_from_world"].as>(); - const auto log_rotation_tag_from_world = node["log_rotation_tag_from_world"].as>(); + const auto log_rotation_tag_from_world = node["log_rotation_tag_from_world"].as>(); result.tag_from_world = SE3(SO3::exp(jcc::Vec3( @@ -24,18 +23,18 @@ fiducial_pose get_fiducial_pose() { return result; } - -camera_extrinsics_struct get_camera_extrinsics() { +CameraExtrinsics get_camera_extrinsics() { YAML::Node node = YAML::LoadFile("/jet/config/camera_extrinsics.yaml"); - camera_extrinsics_struct result; + CameraExtrinsics result; const auto log_translation_camera_from_frame = node["log_translation_camera_from_frame"].as>(); - const auto log_rotation_camera_from_frame = node["log_rotation_camera_from_frame"].as>(); + const auto log_rotation_camera_from_frame = node["log_rotation_camera_from_frame"].as>(); result.camera_from_frame = SE3(SO3::exp(jcc::Vec3( log_rotation_camera_from_frame[0], log_rotation_camera_from_frame[1], log_rotation_camera_from_frame[2])), - jcc::Vec3( - log_translation_camera_from_frame[0], log_translation_camera_from_frame[1], log_translation_camera_from_frame[2])); + jcc::Vec3(log_translation_camera_from_frame[0], + log_translation_camera_from_frame[1], + log_translation_camera_from_frame[2])); return result; } diff --git a/config/fiducial_map/read_fiducial_map.hh b/config/fiducial_map/read_fiducial_map.hh index b23b054..ec15eeb 100644 --- a/config/fiducial_map/read_fiducial_map.hh +++ b/config/fiducial_map/read_fiducial_map.hh @@ -8,7 +8,7 @@ namespace jet { -struct fiducial_pose +struct FiducialDescription { SE3 tag_from_world; int tag_size_squares; @@ -16,13 +16,13 @@ struct fiducial_pose double arcode_gap_mm; }; -struct camera_extrinsics_struct +struct CameraExtrinsics { SE3 camera_from_frame; }; -fiducial_pose get_fiducial_pose(); -camera_extrinsics_struct get_camera_extrinsics(); +FiducialDescription get_fiducial_pose(); +CameraExtrinsics get_camera_extrinsics(); } // namespace jet diff --git a/config/hoverjet/controller.yaml b/config/hoverjet/controller.yaml new file mode 100644 index 0000000..0deba5a --- /dev/null +++ b/config/hoverjet/controller.yaml @@ -0,0 +1,9 @@ +k_proportional: + - 0.2 + - 0.2 + - 0.4 + +k_derivative: + - 0.2 + - 0.2 + - 0.4 \ No newline at end of file diff --git a/config/hoverjet/filter.yaml b/config/hoverjet/filter.yaml new file mode 100644 index 0000000..c1649af --- /dev/null +++ b/config/hoverjet/filter.yaml @@ -0,0 +1,106 @@ +use_accelerometer: false +use_gyro: true +use_imus: + - 78 +use_fiducial: true +max_fiducial_latency_s: 0.4 + +transforms: + - source: imu_36 + destination: camera + source_from_destination: + log_R_source_from_destination: + - -1.616591025757221 + - 0.05134578843446101 + - 0.05091124055477719 + translation_source_from_destination: + - -0.001292535846636506 + - -0.0009267741933531443 + - -0.0199366607203838 + - source_from_destination: + log_R_source_from_destination: + - -2.207252187700393 + - 2.221627059568664 + - 0.0729343244927283 + translation_source_from_destination: + - -0.1457315425013999 + - 0.07004447260546187 + - -0.2286505835933628 + destination: camera + source: imu_78 + - destination: camera + source_from_destination: + log_R_source_from_destination: + - 1.209199576156145 + - 1.209199576156145 + - 1.209199576156145 + translation_source_from_destination: + - 0.11 + - 0 + - 0.32 + source: vehicle +imus: + 36: + source_log: /jet/logs/calibration-log-jul6-7 + accelerometer_gains_p0: + - 0 + - 0 + - 0 + magnetometer_gains_p0: + - -79.29922702237944 + - -4.476413645049656 + - -46.83316633469963 + id: 36 + magnetometer_gains_scaling: + - 32.1877202815169 + - 3.408923758371102 + - 10.62688468310214 + - 0 + - 33.17611158621461 + - -13.05820640963059 + - 0 + - 0 + - 34.80217509591158 + name: imu_36 + accelerometer_gains_scaling: + - 9.480392053156724 + - -0.367804276102152 + - 0.1097718806953435 + - 0 + - 9.392748712511086 + - -0.223199776102899 + - 0 + - 0 + - 9.674406404535416 + 78: + magnetometer_gains_scaling: + - 27.28940423457934 + - -8.791590927283378 + - 2.517804983200733 + - 0 + - 32.00283422687422 + - 2.359513709031368 + - 0 + - 0 + - 27.18480244848202 + name: imu_78 + accelerometer_gains_p0: + - 0 + - 0 + - 0 + id: 78 + accelerometer_gains_scaling: + - 9.699410372939981 + - -1.712699829531295 + - -0.3186110085605555 + - 0 + - 9.844189129580382 + - -0.1136357460553914 + - 0 + - 0 + - 8.810237581896597 + magnetometer_gains_p0: + - 45.59041987760968 + - 53.1135550856215 + - 21.82576956097864 + source_log: /jet/logs/calibration-log-jul6-7 diff --git a/config/supervisor/supervisor.conf b/config/supervisor/supervisor.conf index 11e462c..de44bad 100644 --- a/config/supervisor/supervisor.conf +++ b/config/supervisor/supervisor.conf @@ -76,14 +76,14 @@ stopsignal=INT priority=2 [program:pose_filter] -command=/jet/bin/run/filter_balsaq_main +command=/jet/bin/run/filter_balsaq_main /jet/config/hoverjet/filter.yaml stderr_logfile = /var/log/supervisord/filter-stderr.log stdout_logfile = /var/log/supervisord/filter-stdout.log stopsignal=INT priority=1 [program:controller] -command=/jet/bin/run/controller_balsaq_main +command=/jet/bin/run/controller_balsaq_main /jet/config/hoverjet/controller.yaml stderr_logfile = /var/log/supervisord/controller-stderr.log stdout_logfile = /var/log/supervisord/controller-stdout.log stopsignal=INT diff --git a/control/controller_balsaq.cc b/control/controller_balsaq.cc index 1d82787..ecd1a70 100644 --- a/control/controller_balsaq.cc +++ b/control/controller_balsaq.cc @@ -7,23 +7,19 @@ #include "control/control_utilities.hh" #include "control/jet_vane_mapper.hh" #include "control/servo_interface.hh" + #include "filtering/pose_message.hh" +#include "filtering/yaml_matrix.hh" + #include "infrastructure/balsa_queue/bq_main_macro.hh" #include "infrastructure/engine/turbine_state_message.hh" -#include "third_party/experiments/estimation/time_point.hh" - namespace jet { namespace control { namespace { -estimation::TimePoint to_time_point(const Timestamp& ts) { - const auto epoch_offset = std::chrono::nanoseconds(uint64_t(ts)); - const estimation::TimePoint time_point = estimation::TimePoint{} + epoch_offset; - return time_point; -} - +/* jcc::Vec3 sigmoid(const jcc::Vec3& v) { const double v_nrm = v.norm(); @@ -31,11 +27,18 @@ jcc::Vec3 sigmoid(const jcc::Vec3& v) { return interp_value * v.normalized(); } +*/ -QuadraframeStatus generate_control(const SO3& world_from_target, const Pose& pose, const JetStatus& jet_status) { +QuadraframeStatus generate_control(const SO3& world_from_target, + const Pose& pose, + const JetStatus& jet_status, + const GainConfig& cfg) { JetVaneMapper mapper_; - const MatNd<3, 3> K = jcc::Vec3(0.3, 0.3, 0.4).asDiagonal(); + const jcc::Vec3 w_jet_frame = pose.world_from_jet.inverse() * pose.w_world_frame; + + const MatNd<3, 3> Kp = cfg.k_proportional.asDiagonal(); + const MatNd<3, 3> Kd = cfg.k_derivative.asDiagonal(); // // Compute the current expected jet force (All servos zero'd) @@ -53,7 +56,7 @@ QuadraframeStatus generate_control(const SO3& world_from_target, const Pose& pos const jcc::Vec3 desired_force_jet_frame = wrench_for_zero.force_N; const SO3 target_from_jet = world_from_target.inverse() * pose.world_from_jet.so3(); - const jcc::Vec3 desired_torque_jet_frame = -K * target_from_jet.log(); + const jcc::Vec3 desired_torque_jet_frame = -(Kp * target_from_jet.log()) + -(Kd * w_jet_frame); Wrench target_wrench; target_wrench.torque_Nm = desired_torque_jet_frame; @@ -99,6 +102,9 @@ void ControllerBq::init(const Config& config) { std::cout << "Subscribing Turbine State" << std::endl; turbine_state_sub_ = make_subscriber("turbine_state"); + read_matrix(config["k_proportional"], gain_cfg_.k_proportional); + read_matrix(config["k_derivative"], gain_cfg_.k_derivative); + std::cout << "Controller starting" << std::endl; } @@ -125,7 +131,7 @@ void ControllerBq::loop() { if (got_pose_msg && got_turbine_status_) { const Pose pose = pose_msg.to_pose(); - const auto target_qframe_status = generate_control(target_from_world, pose, jet_status_); + const auto target_qframe_status = generate_control(target_from_world, pose, jet_status_, gain_cfg_); SetServoMessage servo_message = create_servo_command(target_qframe_status); servo_pub_->publish(servo_message); } diff --git a/control/controller_balsaq.hh b/control/controller_balsaq.hh index f711809..b5fac17 100644 --- a/control/controller_balsaq.hh +++ b/control/controller_balsaq.hh @@ -8,6 +8,11 @@ namespace jet { namespace control { +struct GainConfig { + jcc::Vec3 k_proportional; + jcc::Vec3 k_derivative; +}; + class ControllerBq : public BalsaQ { public: ControllerBq(); @@ -19,6 +24,8 @@ class ControllerBq : public BalsaQ { const static uint loop_delay_microseconds = 10000; private: + GainConfig gain_cfg_; + SubscriberPtr roll_sub_; SubscriberPtr pitch_sub_; SubscriberPtr yaw_sub_; diff --git a/control/jet_vane_model.hh b/control/jet_vane_model.hh index eb41642..5f75e86 100644 --- a/control/jet_vane_model.hh +++ b/control/jet_vane_model.hh @@ -24,7 +24,8 @@ struct VaneConfiguration { SE3 T_vane_unit_from_vane_default = detail::compute_T_vane_unit_from_vane_default(); double offset_vane_from_servo_rad = 0.0; - double gear_ratio_servo_from_vane = 5.33; + // double gear_ratio_servo_from_vane = 5.33; + double gear_ratio_servo_from_vane = 1.0; double max_servo_angle_rad = 1.116; double min_servo_angle_rad = -1.116; diff --git a/filtering/calibrate_camera_from_log.cc b/filtering/calibrate_camera_from_log.cc index de19e0b..21d66dc 100644 --- a/filtering/calibrate_camera_from_log.cc +++ b/filtering/calibrate_camera_from_log.cc @@ -81,7 +81,9 @@ void calibrate_camera(const std::string& log_path) { break; } - const auto obj_pt_associations = obj_points_img_points_from_image(image->image); + + const auto ids_corners = get_ids_and_corners(image->image); + const auto obj_pt_associations = obj_points_img_points_from_image(ids_corners); serial_number = image->serial_number; cols = image->image.cols; @@ -130,13 +132,14 @@ void calibrate_camera(const std::string& log_path) { jcc::Warning() << "Calibrating camera, this might take a while..." << std::endl; int flag = 0; const auto t0 = jcc::now(); - cv::calibrateCamera(object_points, image_points, cv::Size(480, 270), K, D, rvecs, tvecs, flag); + cv::Size resolution(480, 270); + cv::calibrateCamera(object_points, image_points, resolution, K, D, rvecs, tvecs, flag); const auto t1 = jcc::now(); jcc::Success() << "Done. Took: " << estimation::to_seconds(t1 - t0) << std::endl; jcc::Success() << "Generating yaml..." << std::endl; - YAML::Node node; // starts out as null - node["serial_number"] = serial_number; // it now is a map node + YAML::Node node; + node["serial_number"] = serial_number; { std::stringstream ss; diff --git a/filtering/calibrate_jet.cc b/filtering/calibrate_jet.cc index 3538868..3638d7c 100644 --- a/filtering/calibrate_jet.cc +++ b/filtering/calibrate_jet.cc @@ -3,24 +3,25 @@ //%deps(nonlinear_camera_model) //%deps(warn_sensor_rates) //%deps(make_interpolator) -#include "third_party/experiments/estimation/sensors/make_interpolator.hh" - #include "third_party/experiments/estimation/calibration/nonlinear_camera_model.hh" #include "third_party/experiments/estimation/calibration/warn_sensor_rates.hh" #include "third_party/experiments/estimation/jet/jet_filter.hh" #include "third_party/experiments/estimation/jet/jet_optimizer.hh" +#include "third_party/experiments/estimation/sensors/make_interpolator.hh" -//%deps(calibrate_single_imu) +//%deps(create_static_jet_model) //%deps(robust_pnp) //%deps(visualize_calibration) //%deps(visualize_camera_calibration) -#include "third_party/experiments/estimation/calibration/calibrate_single_imu.hh" +#include "third_party/experiments/estimation/jet/create_static_jet_model.hh" #include "third_party/experiments/estimation/vision/robust_pnp.hh" #include "third_party/experiments/estimation/visualization/visualize_calibration.hh" #include "third_party/experiments/estimation/visualization/visualize_camera_calibration.hh" //%deps(form_coordinate_frame) +//%deps(put_transform_network) #include "third_party/experiments/geometry/spatial/form_coordinate_frame.hh" +#include "third_party/experiments/geometry/visualization/put_transform_network.hh" //%deps(simple_geometry) //%deps(ui2d) @@ -39,6 +40,9 @@ #include "camera/camera_manager.hh" #include "vision/fiducial_detection_and_pose.hh" +#include "filtering/transform_network_from_yaml.hh" +#include "filtering/yaml_matrix.hh" + // - Validate reprojection of fiducial detection using the packaged calibration // - Validate opencv calibration // x - Estimate IMU intrinsics @@ -83,20 +87,6 @@ std::vector object_image_assoc_from_board_p return obj_image; } -void align_crappy_jake(const std::shared_ptr& geo, - const std::shared_ptr& ui2d, - const estimation::NonlinearCameraModel& model, - const SE3& fiducial_from_camera_init, - const std::vector& obj_pt_associations) { - std::vector observed; - std::vector object; - - const auto associations = object_image_assoc_from_board_point(obj_pt_associations); - - const auto pnp_visitor = estimation::make_pnp_visitor(model, object, observed); - const auto pnp_result = estimation::robust_pnp(model, fiducial_from_camera_init, object, observed, pnp_visitor); -} - } // namespace const estimation::CreateSingleImuModelConfig imu_cal_cfg{ @@ -111,10 +101,12 @@ const estimation::CameraCalibrationConfig camera_cal_cfg{ .visualize_camera_frustum = false // }; +bool visualize_filter = true; + bool visualize() { return (camera_cal_cfg.visualize_camera || camera_cal_cfg.visualize_camera_distortion || camera_cal_cfg.visualize_camera_frustum || imu_cal_cfg.visualize_imu_model || imu_cal_cfg.visualize_gyro || - imu_cal_cfg.visualize_magnetometer); + imu_cal_cfg.visualize_magnetometer || visualize_filter); } void setup() { @@ -128,16 +120,41 @@ void setup() { } } -struct JetModel { - std::map imu_calibration_from_imu_id; -}; +void emit_yaml_for_imu_calibrations(const estimation::jet::JetModel& jet_model, const std::string& source_log_name) { + YAML::Node node; + YAML::Node imus_node = node["imus"]; -jcc::Optional find_nearest_fiducial( - const estimation::calibration::CalibrationMeasurements& cal_meas, const estimation::TimePoint& t) { - JASSERT_FALSE(cal_meas.fiducial_meas.empty(), "Cannot calibrate with empty fiducial measurements"); - for (const auto& fiducial_meas : cal_meas.fiducial_meas) { - if (t < fiducial_meas.timestamp) { - return {fiducial_meas.measurement}; + for (const auto& imu_pair : jet_model.imu_calibration_from_imu_id) { + const auto& single_cal = imu_pair.second; + + const std::string imu_id_str = estimation::jet::make_imu_id_string(single_cal.imu_id); + { + YAML::Node single_imu_node = imus_node[single_cal.imu_id]; + const auto intrinsics = single_cal.imu_model.intrinsics(); + + set_matrix(single_imu_node, "accelerometer_gains_scaling", intrinsics.imu_gains.cholesky_factor); + set_matrix(single_imu_node, "accelerometer_gains_p0", intrinsics.imu_gains.p0); + set_matrix(single_imu_node, "magnetometer_gains_scaling", intrinsics.magnetometer_gains.cholesky_factor); + set_matrix(single_imu_node, "magnetometer_gains_p0", intrinsics.magnetometer_gains.p0); + + single_imu_node["id"] = single_cal.imu_id; + single_imu_node["name"] = imu_id_str; + single_imu_node["source_log"] = source_log_name; + } + } + + transform_network_to_yaml(node, jet_model.transform_network); + + std::cout << node << std::endl; +} + +jcc::Optional find_nearest_fiducial_in_time( + const std::vector> fiducial_measurements, + const estimation::TimePoint& t) { + JASSERT_FALSE(fiducial_measurements.empty(), "Cannot calibrate with empty fiducial measurements"); + for (const auto& fiducial_measurements : fiducial_measurements) { + if (t < fiducial_measurements.timestamp) { + return {fiducial_measurements.measurement}; } } return {}; @@ -149,17 +166,18 @@ SE3 compute_world_from_camera(const estimation::SingleImuCalibration& imu_cal) { return SE3(world_from_camera, jcc::Vec3::Zero()); } -ejf::Parameters compute_parameters(const estimation::calibration::CalibrationMeasurements& cal_meas, - const JetModel& jet_model) { +ejf::Parameters compute_filter_fixed_parameters( + const std::vector> fiducial_measurements, + const estimation::jet::JetModel& jet_model) { const auto& imu_cal_1 = jet_model.imu_calibration_from_imu_id.at(IMU_1); const auto& imu_cal_2 = jet_model.imu_calibration_from_imu_id.at(IMU_2); const SO3 world_from_camera_1 = compute_world_from_camera(imu_cal_1).so3(); const SO3 world_from_camera_2 = compute_world_from_camera(imu_cal_2).so3(); jcc::Warning() << "[Filter Setup] IMU->IMU Alignment Error: " - << (world_from_camera_1 * world_from_camera_2.inverse()).log().norm() << std::endl; + << (world_from_camera_1 * world_from_camera_2.inverse()).log().norm() << " Radians" << std::endl; - const auto maybe_nearest_fiducial = find_nearest_fiducial(cal_meas, imu_cal_1.g_estimate.time); + const auto maybe_nearest_fiducial = find_nearest_fiducial_in_time(fiducial_measurements, imu_cal_1.g_estimate.time); assert(maybe_nearest_fiducial); const auto fiducial_at_g_estimate = *maybe_nearest_fiducial; const SO3 fiducial_from_camera = fiducial_at_g_estimate.T_fiducial_from_camera.so3(); @@ -168,14 +186,16 @@ ejf::Parameters compute_parameters(const estimation::calibration::CalibrationMea auto p = ejf::JetFilter::reasonable_parameters(); { p.T_world_from_fiducial = SE3(world_from_fiducial, jcc::Vec3::Zero()); - p.T_imu1_from_vehicle = SE3(imu_cal_1.camera_from_gyro.inverse(), jcc::Vec3::Zero()); - p.T_imu2_from_vehicle = SE3(imu_cal_2.camera_from_gyro.inverse(), jcc::Vec3::Zero()); + + p.T_imu1_from_vehicle = jet_model.transform_network.find_source_from_destination("imu_78", "vehicle"); + p.T_imu2_from_vehicle = jet_model.transform_network.find_source_from_destination("imu_36", "vehicle"); + p.T_camera_from_vehicle = jet_model.transform_network.find_source_from_destination("camera", "vehicle"); } return p; } -void test_filter(const estimation::calibration::CalibrationMeasurements& cal_meas, const JetModel& jet_model) { +void test_filter(const estimation::CalibrationMeasurements& cal_meas, const estimation::jet::JetModel& jet_model) { const auto view = viewer::get_window3d("Calibration"); const auto geo = view->add_primitive(); @@ -186,7 +206,6 @@ void test_filter(const estimation::calibration::CalibrationMeasurements& cal_mea const planning::jet::JetModel jet_3dmodel; constexpr bool DRAW_VEHICLE = true; - const SO3 jetmodel_from_body = jcc::exp_x(M_PI * 0.5).so3(); if constexpr (DRAW_VEHICLE) { jet_3dmodel.insert(*jet_tree); } @@ -201,34 +220,45 @@ void test_filter(const estimation::calibration::CalibrationMeasurements& cal_mea const auto t0 = cal_meas.first(); // const auto t0 = imu_1_meas.accel_meas.front().timestamp; + const auto p = compute_filter_fixed_parameters(cal_meas.fiducial_meas, jet_model); + auto xp0 = ejf::JetFilter::reasonable_initial_state(t0); - { // - xp0.x.R_world_from_body = compute_world_from_camera(jet_model.imu_calibration_from_imu_id.at(IMU_2)).so3(); + { + const SE3 world_from_vehicle = + compute_world_from_camera(jet_model.imu_calibration_from_imu_id.at(IMU_2)) * p.T_camera_from_vehicle; + xp0.x.R_world_from_body = world_from_vehicle.so3(); + xp0.x.x_world = world_from_vehicle.translation(); } - const auto p = compute_parameters(cal_meas, jet_model); + ejf::JetFilter jf(xp0, p); for (const auto& fiducial_meas : cal_meas.fiducial_meas) { jf.measure_fiducial(fiducial_meas.measurement, fiducial_meas.timestamp); } - for (const auto& accel : cal_meas.imu_cal.at(IMU_1).accel_meas) { - const jcc::Vec3 corrected_accel = imu_1_model.correct_measured_accel(accel.measurement.observed_acceleration); - // jf.measure_imu({corrected_accel}, accel.timestamp); + constexpr bool USE_ACCELEROMETER = false; + if (USE_ACCELEROMETER) { + for (const auto& accel : cal_meas.imu_cal.at(IMU_1).accel_meas) { + const jcc::Vec3 corrected_accel = imu_1_model.correct_measured_accel(accel.measurement.observed_acceleration); + jf.measure_imu({corrected_accel}, accel.timestamp); + } } for (const auto& gyro : cal_meas.imu_cal.at(IMU_1).gyro_meas) { jf.measure_gyro(gyro.measurement, gyro.timestamp); } - const auto& imu_2_model = imu_2_cal.imu_model; - for (const auto& accel : cal_meas.imu_cal.at(IMU_2).accel_meas) { - const jcc::Vec3 corrected_accel = imu_2_model.correct_measured_accel(accel.measurement.observed_acceleration); - // jf.measure_imu({corrected_accel}, accel.timestamp, true); - } + constexpr bool USE_IMU_2 = false; + if (USE_IMU_2) { + const auto& imu_2_model = imu_2_cal.imu_model; + for (const auto& accel : cal_meas.imu_cal.at(IMU_2).accel_meas) { + const jcc::Vec3 corrected_accel = imu_2_model.correct_measured_accel(accel.measurement.observed_acceleration); + jf.measure_imu({corrected_accel}, accel.timestamp, true); + } - for (const auto& gyro : cal_meas.imu_cal.at(IMU_2).gyro_meas) { - // jf.measure_gyro(gyro.measurement, gyro.timestamp, true); + for (const auto& gyro : cal_meas.imu_cal.at(IMU_2).gyro_meas) { + jf.measure_gyro(gyro.measurement, gyro.timestamp, true); + } } int k = 0; @@ -238,14 +268,13 @@ void test_filter(const estimation::calibration::CalibrationMeasurements& cal_mea continue; } const auto state = jf.state().x; - // geo->add_axes({get_world_from_body(state)}); const SE3 T_world_from_body = get_world_from_body(state); constexpr bool PRINT_STATES = false; if (PRINT_STATES) { std::cout << "States: " << std::endl; std::cout << "\taccel_bias: " << state.accel_bias.transpose() << std::endl; - std::cout << "\tgyro_bias: " << state.gyro_bias.transpose() << std::endl; + std::cout << "\tgyro_bias: " << state.gyro_bias.transpose() << std::endl; std::cout << "\teps_ddot: " << state.eps_ddot.transpose() << std::endl; std::cout << "\teps_dot: " << state.eps_dot.transpose() << std::endl; std::cout << "\tr: " << T_world_from_body.so3().log().transpose() << std::endl; @@ -269,7 +298,21 @@ void test_filter(const estimation::calibration::CalibrationMeasurements& cal_mea // Angular Velocity geo->add_line({jcc::Vec3::Zero(), -state.eps_dot.tail<3>(), jcc::Vec4(0.0, 1.0, 0.0, 1.0), 5.0}); - jet_tree->set_world_from_root(SE3(T_world_from_body.so3() * jetmodel_from_body.inverse(), jet_origin)); + auto tfn2 = jet_model.transform_network; + const auto maybe_nearest_fiducial = + find_nearest_fiducial_in_time(cal_meas.fiducial_meas, jf.state().time_of_validity); + + // const SE3 visualized_world_from_vehicle = SE3(T_world_from_body.so3(), jet_origin); + const SE3 visualized_world_from_vehicle = T_world_from_body; + + tfn2.update_edge("world", "vehicle", visualized_world_from_vehicle); + if (maybe_nearest_fiducial) { + tfn2.update_edge("fiducial", "camera", maybe_nearest_fiducial->T_fiducial_from_camera); + } + + geometry::put_transform_network(*geo, tfn2, "world"); + const SE3 vehicle_from_model = jcc::exp_z(-M_PI * 0.5); + jet_tree->set_world_from_root(visualized_world_from_vehicle * vehicle_from_model); geo->flip(); view->spin_until_step(); @@ -310,33 +353,21 @@ void go() { jcc::Success() << "Calibrating..." << std::endl; - // - // Calibrate the IMU intrinsics / Estrinsics - // - - JetModel jet_model; - for (const auto& imu_measurements : cal_measurements.imu_cal) { - const auto one_imu = estimation::create_single_imu_model(cal_measurements, imu_measurements.second, imu_cal_cfg); - jet_model.imu_calibration_from_imu_id[imu_measurements.first] = one_imu; - } - // // Validate the fiducial reprojections // jcc::Success() << "[Camera] Validating fiducial measurements" << std::endl; ImageStream image_stream(path, range); - jcc::Success() << "[Camera] Parsing images..." << std::endl; + jcc::Success() << "[Camera] Re-running Fiducial Detection on images..." << std::endl; const CameraManager cam_mgr; const auto view = viewer::get_window3d("Calibration"); const auto geo = view->add_primitive(); const auto ui2d = view->add_primitive(); - bool set = false; - geometry::Unit3 permanent_g_fiducial_frame; - - std::vector> collected_fiducial_meas; + // Compute new fiducial measurements + std::vector> collected_fiducial_meas; int i = 0; while (true) { @@ -346,13 +377,8 @@ void go() { if (!image) { break; } - const auto calibration = cam_mgr.get_calibration(image->serial_number); - const auto proj = proj_coeffs_from_opencv(calibration); - const auto model = estimation::NonlinearCameraModel(proj); - - const auto ids_corners = get_ids_and_corners(camera_frame); - const auto obj_pt_associations = obj_points_img_points_from_image(ids_corners); + const auto ids_corners = get_ids_and_corners(image->image); const auto fiducial_from_camera = estimate_board_bottom_left_from_camera(ids_corners, calibration); const auto t = image->time; @@ -364,6 +390,9 @@ void go() { // TODO: NEXT, FILL THIS IN if (camera_cal_cfg.visualize_camera) { + const auto obj_pt_associations = obj_points_img_points_from_image(ids_corners); + const auto proj = proj_coeffs_from_opencv(calibration); + const auto model = estimation::NonlinearCameraModel(proj); const auto associations = object_image_assoc_from_board_point(obj_pt_associations); estimation::visualize_single_camera_frame( model, fiducial_from_camera, associations, *image, ui2d, geo, camera_cal_cfg); @@ -371,11 +400,19 @@ void go() { } jcc::Success() << "[Filter] Testing filter..." << std::endl; - { - estimation::calibration::CalibrationMeasurements new_cal_measurements = cal_measurements; - new_cal_measurements.fiducial_meas = collected_fiducial_meas; - test_filter(new_cal_measurements, jet_model); - } + estimation::CalibrationMeasurements new_cal_measurements = cal_measurements; + { new_cal_measurements.fiducial_meas = collected_fiducial_meas; } + + const auto jet_model = estimation::jet::create_static_jet_model(new_cal_measurements, imu_cal_cfg); + + // + // Calibrate the IMU intrinsics / Estrinsics + // + + emit_yaml_for_imu_calibrations(jet_model, path); + + test_filter(new_cal_measurements, jet_model); + jcc::Success() << "Done." << std::endl; } diff --git a/filtering/convert_messages.cc b/filtering/convert_messages.cc new file mode 100644 index 0000000..763a5d4 --- /dev/null +++ b/filtering/convert_messages.cc @@ -0,0 +1,37 @@ +#include "filtering/convert_messages.hh" + +#include "filtering/to_time_point.hh" + +namespace jet { + +estimation::TimedMeasurement to_accel_meas(const ImuMessage& msg) { + const auto time_of_validity = to_time_point(msg.timestamp); + const jcc::Vec3 accel_mpss(msg.accel_mpss_x, msg.accel_mpss_y, msg.accel_mpss_z); + ejf::AccelMeasurement accel_meas; + accel_meas.observed_acceleration = accel_mpss; + return estimation::TimedMeasurement{accel_meas, time_of_validity}; +} +estimation::TimedMeasurement to_gyro_meas(const ImuMessage& msg) { + const auto time_of_validity = to_time_point(msg.timestamp); + const jcc::Vec3 gyro_radps(msg.gyro_radps_x, msg.gyro_radps_y, msg.gyro_radps_z); + ejf::GyroMeasurement gyro_meas; + gyro_meas.observed_w = gyro_radps; + return estimation::TimedMeasurement{gyro_meas, time_of_validity}; +} +estimation::TimedMeasurement to_mag_meas(const ImuMessage& msg) { + const auto time_of_validity = to_time_point(msg.timestamp); + const jcc::Vec3 mag_utesla(msg.mag_utesla_x, msg.mag_utesla_y, msg.mag_utesla_z); + estimation::MagnetometerMeasurement mag_meas; + mag_meas.observed_bfield = mag_utesla; + return estimation::TimedMeasurement{mag_meas, time_of_validity}; +} +estimation::TimedMeasurement to_fiducial_meas(const FiducialDetectionMessage& msg) { + const ejf::FiducialMeasurement fiducial_meas{ + .fiducial_id = 1, // + .T_fiducial_from_camera = msg.fiducial_from_camera() // + }; + const auto time_of_validity = to_time_point(msg.timestamp); + return {fiducial_meas, time_of_validity}; +} + +} // namespace jet \ No newline at end of file diff --git a/filtering/convert_messages.hh b/filtering/convert_messages.hh new file mode 100644 index 0000000..7742e71 --- /dev/null +++ b/filtering/convert_messages.hh @@ -0,0 +1,14 @@ +#pragma once + +#include "embedded/imu_driver/imu_message.hh" +#include "vision/fiducial_detection_message.hh" + +#include "third_party/experiments/estimation/calibration/calibration_dataset.hh" + +namespace jet { +namespace ejf = estimation::jet_filter; +estimation::TimedMeasurement to_accel_meas(const ImuMessage& msg); +estimation::TimedMeasurement to_gyro_meas(const ImuMessage& msg); +estimation::TimedMeasurement to_mag_meas(const ImuMessage& msg); +estimation::TimedMeasurement to_fiducial_meas(const FiducialDetectionMessage& msg); +} // namespace jet \ No newline at end of file diff --git a/filtering/extract_data_from_log.cc b/filtering/extract_data_from_log.cc index c6f3ed0..9ac8f21 100644 --- a/filtering/extract_data_from_log.cc +++ b/filtering/extract_data_from_log.cc @@ -5,6 +5,7 @@ #include "filtering/to_time_point.hh" #include "infrastructure/logging/log_reader.hh" #include "vision/fiducial_detection_message.hh" +#include "filtering/convert_messages.hh" //%deps(log) #include "third_party/experiments/logging/log.hh" @@ -14,9 +15,8 @@ namespace jet { namespace filtering { namespace { -void add_imu(const ImuMessage& msg, - const TimeRange& range, - Out meas) { + +void add_imu(const ImuMessage& msg, const TimeRange& range, Out meas) { const auto time_of_validity = to_time_point(msg.timestamp); if ((time_of_validity < range.start) || (time_of_validity > range.end)) { @@ -26,50 +26,30 @@ void add_imu(const ImuMessage& msg, const int imu_id = msg.imu_id_leading_byte; // Today, it is redundancy day today meas->imu_cal[imu_id].imu_id = imu_id; - { - const jcc::Vec3 accel_mpss(msg.accel_mpss_x, msg.accel_mpss_y, msg.accel_mpss_z); - ejf::AccelMeasurement accel_meas; - accel_meas.observed_acceleration = accel_mpss; - meas->imu_cal[imu_id].accel_meas.push_back({accel_meas, time_of_validity}); - } - { - const jcc::Vec3 gyro_radps(msg.gyro_radps_x, msg.gyro_radps_y, msg.gyro_radps_z); - ejf::GyroMeasurement gyro_meas; - gyro_meas.observed_w = gyro_radps; - meas->imu_cal[imu_id].gyro_meas.push_back({gyro_meas, time_of_validity}); - } - { - const jcc::Vec3 mag_utesla(msg.mag_utesla_x, msg.mag_utesla_y, msg.mag_utesla_z); - estimation::MagnetometerMeasurement mag_meas; - mag_meas.observed_bfield = mag_utesla; - meas->imu_cal[imu_id].mag_meas.push_back({mag_meas, time_of_validity}); - } + meas->imu_cal[imu_id].accel_meas.push_back(to_accel_meas(msg)); + meas->imu_cal[imu_id].gyro_meas.push_back(to_gyro_meas(msg)); + meas->imu_cal[imu_id].mag_meas.push_back(to_mag_meas(msg)); } void add_fiducial(const FiducialDetectionMessage& msg, const TimeRange& range, - Out meas) { + Out meas) { const auto time_of_validity = to_time_point(msg.timestamp); if ((time_of_validity < range.start) || (time_of_validity > range.end)) { return; } - const ejf::FiducialMeasurement fiducial_meas{ - .fiducial_id = 1, // - .T_fiducial_from_camera = msg.fiducial_from_camera() // - }; - - meas->fiducial_meas.push_back({fiducial_meas, time_of_validity}); + meas->fiducial_meas.push_back(to_fiducial_meas(msg)); } } // namespace -estimation::calibration::CalibrationMeasurements extract_data_from_log(const std::string& log_path, - const TimeRange& time_range, - const ExtractionConfiguration& cfg) { - estimation::calibration::CalibrationMeasurements meas; +estimation::CalibrationMeasurements extract_data_from_log(const std::string& log_path, + const TimeRange& time_range, + const ExtractionConfiguration& cfg) { + estimation::CalibrationMeasurements meas; const std::vector channel_names = {"imu", "fiducial_detection_channel", "camera_image_channel", "imu_1", "imu_2"}; diff --git a/filtering/extract_data_from_log.hh b/filtering/extract_data_from_log.hh index af713ad..273066d 100644 --- a/filtering/extract_data_from_log.hh +++ b/filtering/extract_data_from_log.hh @@ -20,9 +20,9 @@ struct ExtractionConfiguration { bool use_fiducial_detections = true; }; -estimation::calibration::CalibrationMeasurements extract_data_from_log(const std::string& log_path, - const TimeRange& time_range, - const ExtractionConfiguration& cfg = {}); +estimation::CalibrationMeasurements extract_data_from_log(const std::string& log_path, + const TimeRange& time_range, + const ExtractionConfiguration& cfg = {}); class ImageStream { public: diff --git a/filtering/filter_balsaq.cc b/filtering/filter_balsaq.cc index d75a4ff..f543a66 100644 --- a/filtering/filter_balsaq.cc +++ b/filtering/filter_balsaq.cc @@ -1,129 +1,108 @@ //%bin(filter_balsaq_main) #include "filtering/filter_balsaq.hh" -#include "embedded/imu_driver/imu_message.hh" #include "infrastructure/balsa_queue/bq_main_macro.hh" -#include "config/fiducial_map/read_fiducial_map.hh" +#include "embedded/imu_driver/imu_message.hh" #include "filtering/pose_message.hh" #include "vision/fiducial_detection_message.hh" +#include "filtering/convert_messages.hh" +#include "filtering/imu_model_from_yaml.hh" +#include "filtering/to_time_point.hh" +#include "filtering/transform_network_from_yaml.hh" + #include #include -// Filter requires this -#include "third_party/experiments/estimation/time_point.hh" - -//%deps(fit_ellipse) -#include "third_party/experiments/geometry/shapes/fit_ellipse.hh" - namespace jet { namespace filtering { +constexpr int IMU_1_ID = 78; +constexpr int IMU_2_ID = 36; + FilterBq::FilterBq() { set_comms_factory(std::make_unique()); } -estimation::TimePoint to_time_point(const Timestamp& ts) { - const auto epoch_offset = std::chrono::nanoseconds(uint64_t(ts)); - const estimation::TimePoint time_point = estimation::TimePoint{} + epoch_offset; - return time_point; -} - void FilterBq::init(const Config& config) { - std::cout << "Subscribing IMU" << std::endl; - imu_sub_ = make_subscriber("imu"); - std::cout << "Subscribing Fiducial" << std::endl; + gonogo().nogo("Starting up"); + std::cout << "Starting up" << std::endl; + + std::cout << "Subscribing Fiducial..." << std::endl; fiducial_sub_ = make_subscriber("fiducial_detection_channel"); - std::cout << "Advertising Pose" << std::endl; + std::cout << "Advertising Pose..." << std::endl; pose_pub_ = make_publisher("pose"); - camera_from_vehicle_ = get_camera_extrinsics().camera_from_frame; - tag_from_world_ = get_fiducial_pose().tag_from_world; + // + // Load intrinsics for each IMU + // - std::cout << "Filter starting" << std::endl; -} + ejf::FilterManagerConfiguration filter_cfg; -void FilterBq::loop() { - FiducialDetectionMessage detection_msg; + std::cout << "Setting up transform network" << std::endl; + filter_cfg.max_fiducial_latency_s = config["max_fiducial_latency_s"].as(); + filter_cfg.transform_network = transform_network_from_yaml(config["transforms"]); - // - // Update with fiducial measurements - // - if (fiducial_sub_->read(detection_msg, 1)) { - estimation::jet_filter::FiducialMeasurement fiducial_meas; - const SE3 fiducial_from_camera = detection_msg.fiducial_from_camera(); - const SE3 world_from_camera = tag_from_world_.inverse() * fiducial_from_camera; - fiducial_meas.T_fiducial_from_camera = world_from_camera * camera_from_vehicle_; - - const auto fiducial_time_of_validity = to_time_point(detection_msg.timestamp); - - if (!jf_.initialized()) { - std::cout << "Initializing" << std::endl; - auto xp0 = estimation::jet_filter::JetFilter::reasonable_initial_state(); - xp0.x.R_world_from_body = fiducial_meas.T_fiducial_from_camera.so3(); - xp0.x.x_world = fiducial_meas.T_fiducial_from_camera.translation(); - xp0.time_of_validity = fiducial_time_of_validity; - jf_.reset(xp0); - - gonogo().go(); - } + std::cout << "Loading IMU calibrations..." << std::endl; + { + const auto cfg_imu_1 = imu_model_from_yaml(config["imus"][IMU_1_ID]); + const auto cfg_imu_2 = imu_model_from_yaml(config["imus"][IMU_2_ID]); - jf_.measure_fiducial(fiducial_meas, fiducial_time_of_validity); - const auto state = jf_.state().x; + filter_cfg.imu_model_from_id[cfg_imu_1.imu_id] = cfg_imu_1.imu_model; + filter_cfg.imu_model_from_id[cfg_imu_2.imu_id] = cfg_imu_2.imu_model; + + std::cout << "Starting imu subscriptions..." << std::endl; + imu_sub_from_id_[cfg_imu_1.imu_id] = make_subscriber("imu_1"); + imu_sub_from_id_[cfg_imu_2.imu_id] = make_subscriber("imu_2"); } + filter_manager_.init(filter_cfg); +} + +void FilterBq::loop() { // // Update with IMU measurements // - ImuMessage imu_msg; - bool got_imu_msg = false; - while (imu_sub_->read(imu_msg, 1)) { - got_imu_msg = true; + ImuMessage imu_msg; + while (imu_sub_from_id_[IMU_1_ID]->read(imu_msg, 1)) { + filter_manager_.measure_gyro(to_gyro_meas(imu_msg)); + filter_manager_.measure_imu(to_accel_meas(imu_msg), IMU_1_ID); } - // if (jf_.initialized() && got_imu_msg) { - // const auto time_of_validity = to_time_point(imu_msg.timestamp); - // const jcc::Vec3 gyro_radps(imu_msg.gyro_radps_x, imu_msg.gyro_radps_y, imu_msg.gyro_radps_z); - // estimation::jet_filter::GyroMeasurement gyro_meas; - // gyro_meas.observed_w = gyro_radps; - // jf_.measure_gyro(gyro_meas, time_of_validity); - // } - - if (jf_.initialized()) { - jf_.free_run(); + FiducialDetectionMessage detection_msg; + while (fiducial_sub_->read(detection_msg, 1)) { + filter_manager_.measure_fiducial(to_fiducial_meas(detection_msg)); } + const auto current_time = to_time_point(get_current_time()); + filter_manager_.update(current_time); + // // Report a state // - if (jf_.initialized() && got_imu_msg) { - const auto current_time = to_time_point(imu_msg.timestamp); - const auto state = jf_.view(current_time); + if (filter_manager_.stage() == ejf::FilterStage::RUNNING) { + gonogo().go("Running"); + const auto state = filter_manager_.state(current_time); Pose pose; { - const SE3 vehicle_real_from_vehicle_filtered; - const SE3 T_world_from_body = estimation::jet_filter::get_world_from_body(state); - pose.world_from_jet = T_world_from_body * vehicle_real_from_vehicle_filtered.inverse(); - - const jcc::Vec3 log_translation_world_from_vehicle = pose.world_from_jet.translation(); - const jcc::Vec3 log_rotation_world_from_vehicle = pose.world_from_jet.so3().log(); - - pose.v_world_frame = (vehicle_real_from_vehicle_filtered.so3() * state.R_world_from_body.inverse()).inverse() * - state.eps_dot.head<3>(); - pose.w_world_frame = (vehicle_real_from_vehicle_filtered.so3() * state.R_world_from_body.inverse()).inverse() * - state.eps_dot.tail<3>(); - pose.timestamp = imu_msg.timestamp; + const SE3 T_world_from_body = estimation::jet_filter::get_world_from_body(state.x); + pose.world_from_jet = T_world_from_body; + + pose.v_world_frame = state.x.eps_dot.head<3>(); + pose.w_world_frame = state.x.eps_dot.tail<3>(); + pose.timestamp = Timestamp(state.time_of_validity); } + // Publish is non-const PoseMessage pose_msg = PoseMessage::from_pose(pose); pose_pub_->publish(pose_msg); } else { - std::cout << "No means to queue time" << std::endl; + gonogo().nogo(ejf::stage_to_string(filter_manager_.stage())); } -} +} // namespace filtering void FilterBq::shutdown() { std::cout << "Filter shutdown" << std::endl; diff --git a/filtering/filter_balsaq.hh b/filtering/filter_balsaq.hh index b172b3b..6277e84 100644 --- a/filtering/filter_balsaq.hh +++ b/filtering/filter_balsaq.hh @@ -1,18 +1,16 @@ #pragma once -#include -#include - #include "infrastructure/balsa_queue/balsa_queue.hh" #include "infrastructure/comms/mqtt_comms_factory.hh" -//%deps(jet_filter) -#include "third_party/experiments/estimation/jet/jet_filter.hh" +//%deps(jet_filter_manager) +#include "third_party/experiments/estimation/jet/jet_filter_manager.hh" namespace jet { namespace filtering { +namespace ejf = estimation::jet_filter; class FilterBq : public BalsaQ { public: FilterBq(); @@ -25,13 +23,13 @@ class FilterBq : public BalsaQ { private: SubscriberPtr fiducial_sub_; - SubscriberPtr imu_sub_; - PublisherPtr pose_pub_; + std::map imu_sub_from_id_; - SE3 camera_from_vehicle_; - SE3 tag_from_world_; - estimation::jet_filter::JetFilter jf_; + ejf::FilterManager filter_manager_; + + PublisherPtr pose_pub_; + PublisherPtr state_pub_; }; } // namespace filtering diff --git a/filtering/filter_viz_balsaq.cc b/filtering/filter_viz_balsaq.cc index b4b3677..bfb8678 100644 --- a/filtering/filter_viz_balsaq.cc +++ b/filtering/filter_viz_balsaq.cc @@ -5,7 +5,6 @@ #include "control/servo_interface.hh" #include "embedded/imu_driver/imu_message.hh" #include "embedded/servo_bq/set_servo_message.hh" -#include "filtering/pose_message.hh" #include "infrastructure/balsa_queue/bq_main_macro.hh" #include "vision/fiducial_detection_message.hh" #include "visualization/thrust_stand_visualizer.hh" @@ -21,6 +20,9 @@ #include "third_party/experiments/viewer/primitives/simple_geometry.hh" #include "third_party/experiments/viewer/window_3d.hh" +//%deps(jet_model) +#include "third_party/experiments/planning/jet/jet_model.hh" + namespace jet { namespace embedded { @@ -45,9 +47,16 @@ void FilterVizBq::init(const Config& config) { sensor_geo_ = view->add_primitive(); pose_geo_ = view->add_primitive(); servo_geo_ = view->add_primitive(); + jet_tree_ = view->add_primitive(); + + const planning::jet::JetModel jet_3dmodel; + constexpr bool DRAW_VEHICLE = true; + if constexpr (DRAW_VEHICLE) { + jet_3dmodel.insert(*jet_tree_); + } std::cout << "Subscribing IMU" << std::endl; - imu_sub_ = make_subscriber("imu"); + imu_sub_ = make_subscriber("imu_1"); std::cout << "Subscribing Fiducial" << std::endl; fiducial_sub_ = make_subscriber("fiducial_detection_channel"); @@ -56,8 +65,6 @@ void FilterVizBq::init(const Config& config) { std::cout << "Subscribing servos" << std::endl; servo_sub_ = make_subscriber("servo_command_channel"); - - tag_from_world_ = get_fiducial_pose().tag_from_world; camera_from_body_ = get_camera_extrinsics().camera_from_frame; std::cout << "Filter Viz starting" << std::endl; @@ -71,42 +78,17 @@ void FilterVizBq::draw_sensors() { FiducialDetectionMessage detection_msg; if (true && fiducial_sub_->read(detection_msg, 1)) { const SE3 fiducial_from_camera = detection_msg.fiducial_from_camera(); - const SE3 fiducial_from_body = fiducial_from_camera * camera_from_body_; - - // This is used to generate a config - const jcc::Vec3 log_translation_tag_from_world = fiducial_from_body.translation(); - const jcc::Vec3 log_rotation_tag_from_world = fiducial_from_body.so3().log(); - std::cout << "log_translation_tag_from_world: [" << log_translation_tag_from_world[0] << ", " - << log_translation_tag_from_world[1] << ", " << log_translation_tag_from_world[2] << "] " << std::endl; - - std::cout << "log_rotation_tag_from_world: [" << log_rotation_tag_from_world[0] << ", " - << log_rotation_tag_from_world[1] << ", " << log_rotation_tag_from_world[2] << "] " << std::endl; - - const auto fiducial_time_of_validity = to_time_point(detection_msg.timestamp); fiducial_history_.push_back(fiducial_from_camera); } - MatNd<3, 3> L; - { // clang-format off - L.row(0) << 9.67735, 0, 0; - L.row(1) << 0.136597, 9.59653, 0; - L.row(2) << -0.216635, 0.00400047, 9.64812; - // clang-format on - } - const jcc::Vec3 offset(0.0562102, 0.42847, -0.119841); - const geometry::shapes::Ellipse ellipse{L, offset}; - while (imu_sub_->read(imu_msg, 1)) { const jcc::Vec3 accel_mpss(imu_msg.accel_mpss_x, imu_msg.accel_mpss_y, imu_msg.accel_mpss_z); const jcc::Vec3 mag_utesla(imu_msg.mag_utesla_x, imu_msg.mag_utesla_y, imu_msg.mag_utesla_z); const jcc::Vec3 gyro_radps(imu_msg.gyro_radps_x, imu_msg.gyro_radps_y, imu_msg.gyro_radps_z); - accel_history_.push_back( - {geometry::shapes::deform_ellipse_to_unit_sphere(accel_mpss, ellipse) * 9.81, gyro_radps, mag_utesla}); + accel_history_.push_back({accel_mpss, gyro_radps, mag_utesla}); mag_utesla_.push_back({mag_utesla}); - - std::cout << accel_mpss.transpose() << ", " << accel_mpss.norm() << std::endl; } while (accel_history_.size() > 25000u) { @@ -120,21 +102,6 @@ void FilterVizBq::draw_sensors() { sensor_geo_->add_point({meas.accel_mpss, jcc::Vec4(0.1, 0.7, 0.3, 0.9)}); } - if (accel_history_.size() > 2500u) { - const auto visitor = [this, &view](const geometry::shapes::EllipseFit& fit) { - sensor_geo_->add_ellipsoid({fit.ellipse, jcc::Vec4(0.4, 0.6, 0.4, 0.7), 2.0}); - sensor_geo_->flush(); - }; - // std::cout << "Optimizing" << std::endl; - const auto result = geometry::shapes::fit_ellipse(accels); - sensor_geo_->add_ellipsoid({result.ellipse, jcc::Vec4(0.2, 0.9, 0.2, 1.0), 5.0}); - - std::cerr << "chol" << std::endl; - std::cerr << result.ellipse.cholesky_factor << std::endl; - std::cerr << "p0" << std::endl; - std::cerr << result.ellipse.p0.transpose() << std::endl; - } - while (fiducial_history_.size() > 10u) { fiducial_history_.pop_front(); } @@ -143,35 +110,15 @@ void FilterVizBq::draw_sensors() { mag_utesla_.pop_front(); } - sensor_geo_->add_sphere({tag_from_world_.inverse().translation(), 0.3}); - sensor_geo_->add_axes({tag_from_world_.inverse()}); + if (!fiducial_history_.empty()) { + const SE3 camera_from_fiducial = fiducial_history_.back().inverse(); - sensor_geo_->add_sphere({camera_from_body_.inverse().translation(), 0.3}); - sensor_geo_->add_axes({camera_from_body_.inverse()}); + const jcc::Vec3 jet_origin(1.0, 0.0, 0.0); + const Pose pose = last_pose_message_.to_pose(); + const SE3 world_from_jet(pose.world_from_jet.so3(), jet_origin); - if (!fiducial_history_.empty()) { - const SE3 fiducial_from_camera = fiducial_history_.back(); - - constexpr bool DRAW_FIDUCIAL_POSE = false; - constexpr bool DRAW_VEHICLE_POSE = true; - constexpr bool DRAW_FIDUCIAL_IN_BODY_FRAME = false; - - if (DRAW_FIDUCIAL_POSE) { // Draw the fiducial axes in the camera frame - sensor_geo_->add_sphere({fiducial_from_camera.inverse().translation(), 0.2, jcc::Vec4(1.0, 1.0, 0.2, 0.8)}); - sensor_geo_->add_axes({fiducial_from_camera.inverse(), 0.025, 3.0}); - } - - if (DRAW_VEHICLE_POSE) { // Draw the vehicle axes in the world frame - const SE3 world_from_vehicle = tag_from_world_.inverse() * fiducial_from_camera * camera_from_body_; - sensor_geo_->add_sphere({world_from_vehicle.translation(), 0.2, jcc::Vec4(1.0, 0.0, 0.2, 0.8)}); - sensor_geo_->add_axes({world_from_vehicle, 0.6, 3.0, true}); - } - - if (DRAW_FIDUCIAL_IN_BODY_FRAME) { // Draw the body axes in the fiducial frame - const SE3 fiducial_from_body = fiducial_from_camera * camera_from_body_; - sensor_geo_->add_axes({fiducial_from_body.inverse()}); - sensor_geo_->add_sphere({fiducial_from_body.inverse().translation(), 0.2, jcc::Vec4(0.0, 0.0, 1.0, 0.8)}); - } + sensor_geo_->add_sphere({world_from_jet * camera_from_fiducial.translation(), 0.2, jcc::Vec4(1.0, 1.0, 0.2, 0.8)}); + sensor_geo_->add_axes({world_from_jet * camera_from_fiducial, 0.025, 3.0}); } if (!accel_history_.empty()) { @@ -186,13 +133,23 @@ void FilterVizBq::draw_pose() { PoseMessage pose_msg; bool got_pose_msg = false; while (pose_sub_->read(pose_msg, 1)) { + last_pose_message_ = pose_msg; got_pose_msg = true; } if (got_pose_msg) { const Pose pose = pose_msg.to_pose(); - pose_geo_->add_axes({pose.world_from_jet, 0.055, 3.0, true}); + const jcc::Vec3 jet_origin(1.0, 0.0, 0.0); + + pose_geo_->add_axes({pose.world_from_jet, 0.55, 3.0, true}); + + const SE3 world_from_jet(pose.world_from_jet.so3(), jet_origin); + pose_geo_->add_axes({world_from_jet, 0.55, 3.0, true}); + pose_geo_->flip(); + + const SE3 jetmodel_from_body = jcc::exp_x(0.0); + jet_tree_->set_world_from_root(world_from_jet * jetmodel_from_body.inverse()); } } diff --git a/filtering/filter_viz_balsaq.hh b/filtering/filter_viz_balsaq.hh index 4abf2b4..6eb495b 100644 --- a/filtering/filter_viz_balsaq.hh +++ b/filtering/filter_viz_balsaq.hh @@ -4,6 +4,7 @@ #include #include +#include "filtering/pose_message.hh" #include "infrastructure/balsa_queue/balsa_queue.hh" #include "infrastructure/comms/mqtt_comms_factory.hh" @@ -11,6 +12,7 @@ #include "third_party/experiments/estimation/jet/jet_filter.hh" #include "third_party/experiments/viewer/primitives/simple_geometry.hh" +#include "third_party/experiments/viewer/primitives/scene_tree.hh" namespace jet { namespace embedded { @@ -35,12 +37,14 @@ class FilterVizBq : public BalsaQ { SubscriberPtr pose_sub_; SubscriberPtr servo_sub_; - SE3 tag_from_world_; SE3 camera_from_body_; + PoseMessage last_pose_message_; + std::shared_ptr sensor_geo_; std::shared_ptr pose_geo_; std::shared_ptr servo_geo_; + std::shared_ptr jet_tree_; struct AccelStuff { jcc::Vec3 accel_mpss; diff --git a/filtering/imu_model_from_yaml.cc b/filtering/imu_model_from_yaml.cc new file mode 100644 index 0000000..f72523b --- /dev/null +++ b/filtering/imu_model_from_yaml.cc @@ -0,0 +1,19 @@ +#include "filtering/imu_model_from_yaml.hh" + +namespace jet { + +ImuExtrinsicsIntrinsics imu_model_from_yaml(const YAML::Node& node) { + estimation::ImuIntrinsics intrinsics; + { + read_matrix(node["accelerometer_gains_p0"], intrinsics.imu_gains.p0); + read_matrix(node["accelerometer_gains_scaling"], intrinsics.imu_gains.cholesky_factor); + + read_matrix(node["magnetometer_gains_p0"], intrinsics.magnetometer_gains.p0); + read_matrix(node["magnetometer_gains_scaling"], intrinsics.magnetometer_gains.cholesky_factor); + } + const estimation::ImuModel model(intrinsics); + const ImuExtrinsicsIntrinsics calibration{.imu_model = model, .imu_id = node["id"].as()}; + return calibration; +} + +} // namespace jet \ No newline at end of file diff --git a/filtering/imu_model_from_yaml.hh b/filtering/imu_model_from_yaml.hh new file mode 100644 index 0000000..505c2a4 --- /dev/null +++ b/filtering/imu_model_from_yaml.hh @@ -0,0 +1,17 @@ +#pragma once + +#include "filtering/yaml_matrix.hh" + +#include "third_party/experiments/sophus.hh" +//%deps(estimate_imu_intrinsics) +#include "third_party/experiments/estimation/calibration/estimate_imu_intrinsics.hh" + +namespace jet { + +struct ImuExtrinsicsIntrinsics { + estimation::ImuModel imu_model; + int imu_id; +}; + +ImuExtrinsicsIntrinsics imu_model_from_yaml(const YAML::Node& node); +} // namespace jet \ No newline at end of file diff --git a/filtering/measurement_sim_bq.cc b/filtering/measurement_sim_bq.cc new file mode 100644 index 0000000..674c63c --- /dev/null +++ b/filtering/measurement_sim_bq.cc @@ -0,0 +1,106 @@ +//%bin(measurement_sim_bq_main) +#include "filtering/measurement_sim_bq.hh" + +#include "filtering/to_time_point.hh" +#include "infrastructure/balsa_queue/bq_main_macro.hh" +#include "third_party/experiments/estimation/time_point.hh" + +#include "filtering/imu_model_from_yaml.hh" + +#include +#include + +namespace jet { +namespace filtering { + +constexpr int IMU_1_ID = 78; +constexpr int IMU_2_ID = 36; + +MeasurementSimBq::MeasurementSimBq() { + set_comms_factory(std::make_unique()); +} + +void MeasurementSimBq::init(const Config& config) { + gonogo().nogo("Starting up"); + + transform_network_ = transform_network_from_yaml(config["transforms"]); + + const auto cfg_imu_1 = imu_model_from_yaml(config["imus"][IMU_1_ID]); + const auto cfg_imu_2 = imu_model_from_yaml(config["imus"][IMU_2_ID]); + + imu_model_from_id_[cfg_imu_1.imu_id] = cfg_imu_1.imu_model; + imu_model_from_id_[cfg_imu_2.imu_id] = cfg_imu_2.imu_model; + + imu_1_pub_ = make_publisher("imu_1"); + imu_2_pub_ = make_publisher("imu_2"); + fiducial_pub_ = make_publisher("fiducial_detection_channel"); +} + +ImuMessage imu_msg_from_acceleration(const jcc::Vec3& acceleration_mpss, const Timestamp& now_ts, const int imu_id) { + ImuMessage imu_msg; + { + imu_msg.accel_mpss_x = acceleration_mpss.x(); + imu_msg.accel_mpss_y = acceleration_mpss.y(); + imu_msg.accel_mpss_z = acceleration_mpss.z(); + imu_msg.gyro_radps_x = 0.0; + imu_msg.gyro_radps_y = 0.0; + imu_msg.gyro_radps_z = 0.0; + imu_msg.mag_utesla_x = 0.0; + imu_msg.mag_utesla_y = 0.0; + imu_msg.mag_utesla_z = 0.0; + imu_msg.timestamp = now_ts; + imu_msg.imu_id_leading_byte = imu_id; + } + return imu_msg; +} + +void MeasurementSimBq::loop() { + const auto now_ts = get_current_time(); + const auto now_tp = to_time_point(now_ts); + + const jcc::Vec3 g_camera = jcc::Vec3::UnitY() * 9.81; + + { + const SO3 camera_from_imu = transform_network_.find_source_from_destination("camera", "imu_78").so3(); + + const auto& imu_model = imu_model_from_id_[IMU_1_ID]; + const jcc::Vec3 true_accel_imu = camera_from_imu.inverse() * g_camera; + const jcc::Vec3 measured_accel_imu = imu_model.distort_true_accel(true_accel_imu); + auto imu_1_msg = imu_msg_from_acceleration(measured_accel_imu, now_ts, IMU_1_ID); + imu_1_pub_->publish(imu_1_msg); + } + { + const SO3 camera_from_imu = transform_network_.find_source_from_destination("camera", "imu_36").so3(); + const auto& imu_model = imu_model_from_id_[IMU_2_ID]; + const jcc::Vec3 true_accel_imu = camera_from_imu.inverse() * g_camera; + const jcc::Vec3 measured_accel_imu = imu_model.distort_true_accel(true_accel_imu); + auto imu_2_msg = imu_msg_from_acceleration(measured_accel_imu, now_ts, IMU_2_ID); + imu_2_pub_->publish(imu_2_msg); + } + + if (estimation::to_seconds(now_tp - to_time_point(last_publish_)) > fiducial_latency_s_) { + last_publish_ = now_ts; + FiducialDetectionMessage fiducial_msg; + + const jcc::Vec3 translation_jet_from_fiducial(1.0, 1.0, 2.0); + const SO3 R_jet_from_fiducial = SO3::exp(jcc::Vec3(-M_PI, 1.0, -1.0)); + const SE3 jet_from_fiducial(R_jet_from_fiducial, translation_jet_from_fiducial); + const jcc::Vec6 log_fiducial_from_camera = jet_from_fiducial.inverse().log(); + + fiducial_msg.fiducial_from_camera_log = + std::array{{log_fiducial_from_camera[0], log_fiducial_from_camera[1], log_fiducial_from_camera[2], + log_fiducial_from_camera[3], log_fiducial_from_camera[4], log_fiducial_from_camera[5]}}; + fiducial_msg.timestamp = Timestamp(now_tp - estimation::to_duration(fiducial_latency_s_)); + + fiducial_pub_->publish(fiducial_msg); + } +} + +void MeasurementSimBq::shutdown() { + std::cout << "Filter shutdown" << std::endl; +} + +} // namespace filtering +} // namespace jet + +BALSA_QUEUE_MAIN_FUNCTION(jet::filtering::MeasurementSimBq) diff --git a/filtering/measurement_sim_bq.hh b/filtering/measurement_sim_bq.hh new file mode 100644 index 0000000..9b9b0a5 --- /dev/null +++ b/filtering/measurement_sim_bq.hh @@ -0,0 +1,43 @@ + +#pragma once + +#include +#include + +#include "infrastructure/balsa_queue/balsa_queue.hh" +#include "infrastructure/comms/mqtt_comms_factory.hh" + +#include "embedded/imu_driver/imu_message.hh" +#include "vision/fiducial_detection_message.hh" + +#include "filtering/imu_model_from_yaml.hh" + +#include "filtering/transform_network_from_yaml.hh" + +namespace jet { +namespace filtering { + +class MeasurementSimBq : public BalsaQ { + public: + MeasurementSimBq(); + void init(const Config& config); + void loop(); + void shutdown(); + + // Every 10 millisecond + const static uint loop_delay_microseconds = 10000; + + private: + double fiducial_latency_s_ = 0.15; + + std::map imu_model_from_id_; + geometry::TransformNetwork transform_network_; + + PublisherPtr imu_1_pub_; + PublisherPtr imu_2_pub_; + PublisherPtr fiducial_pub_; + Timestamp last_publish_{}; +}; + +} // namespace filtering +} // namespace jet diff --git a/filtering/pose_message.hh b/filtering/pose_message.hh index 8ca34f4..9dc1c8d 100644 --- a/filtering/pose_message.hh +++ b/filtering/pose_message.hh @@ -9,6 +9,13 @@ namespace jet { +// +// Pose has the following contract: +// - Z points in the gravity direction +// - Derivatives and displacements will be smooth +// - Derivatives are *not* guaranteed to correspond +// to the forward difference of displacements +// struct Pose { SE3 world_from_jet; jcc::Vec3 v_world_frame; diff --git a/filtering/transform_network_from_yaml.cc b/filtering/transform_network_from_yaml.cc new file mode 100644 index 0000000..fe91031 --- /dev/null +++ b/filtering/transform_network_from_yaml.cc @@ -0,0 +1,82 @@ +#include "filtering/transform_network_from_yaml.hh" + +#include "filtering/yaml_matrix.hh" + +#include + +namespace jet { +namespace { +void se3_to_yaml(YAML::Node& parent_yaml, const SE3& source_from_destination) { + set_matrix(parent_yaml, "log_R_source_from_destination", source_from_destination.so3().log()); + set_matrix(parent_yaml, "translation_source_from_destination", source_from_destination.translation()); +} +} // namespace + +geometry::TransformNetwork transform_network_from_yaml(const YAML::Node& node) { + geometry::TransformNetwork tfn; + for (const auto& subnode : node) { + const auto source = subnode["source"].as(); + const auto destination = subnode["destination"].as(); + + if (subnode["log_source_from_destination"]) { + const auto log_source_from_destination = subnode["log_source_from_destination"].as>(); + if (log_source_from_destination.size() == 3u) { + // + // SO3 - log + // + const SE3 source_from_destination = + SE3(SO3::exp(read_matrix<3, 1>(subnode["log_source_from_destination"])), jcc::Vec3::Zero()); + tfn.add_edge(source, destination, source_from_destination); + } else if (log_source_from_destination.size() == 6u) { + // + // SE3 - log + // + const SE3 source_from_destination = SE3::exp(read_matrix<6, 1>(subnode["log_source_from_destination"])); + tfn.add_edge(source, destination, source_from_destination); + } + } else if (subnode["source_from_destination"]) { + // + // SE3 - log(SO3) & translation + // + const YAML::Node se3_node = subnode["source_from_destination"]; + const SO3 R_source_from_destination = SO3::exp(read_matrix<3, 1>(se3_node["log_R_source_from_destination"])); + const jcc::Vec3 translation_source_from_destination = + read_matrix<3, 1>(se3_node["translation_source_from_destination"]); + const SE3 source_from_destination = SE3(R_source_from_destination, translation_source_from_destination); + tfn.add_edge(source, destination, source_from_destination); + } + // TODO(jpanikulam): Support for storing as {log(rot), translation-vector} pair + } + return tfn; +} +void transform_network_to_yaml(YAML::Node& parent_yaml, const geometry::TransformNetwork& tfn) { + // Only store each edge once + std::set visited; + + YAML::Node tf_yamls; + parent_yaml["transforms"] = tf_yamls; + for (const auto& node : tfn.edges_from_node_tag()) { + YAML::Node node_yaml; + + bool added_any = false; + for (const auto& edge : node.second) { + const bool should_add = + (visited.find(node.first) != visited.end()) || (visited.find(edge.first) != visited.end()); + if (should_add) { + node_yaml["source"] = node.first; + node_yaml["destination"] = edge.first; + YAML::Node se3_yaml; + node_yaml["source_from_destination"] = se3_yaml; + se3_to_yaml(se3_yaml, edge.second.source_from_destination); + added_any = should_add; + } + visited.insert(edge.first); + } + if(added_any) { + tf_yamls.push_back(node_yaml); + } + visited.insert(node.first); + } +} + +} // namespace jet diff --git a/filtering/transform_network_from_yaml.hh b/filtering/transform_network_from_yaml.hh new file mode 100644 index 0000000..30b8504 --- /dev/null +++ b/filtering/transform_network_from_yaml.hh @@ -0,0 +1,14 @@ +#pragma once + +// %deps(transform_network) +#include "third_party/experiments/geometry/kinematics/transform_network.hh" + +// %deps(yaml-cpp) +#include + +namespace jet { + +// Create a transform network from a yaml +geometry::TransformNetwork transform_network_from_yaml(const YAML::Node& node); +void transform_network_to_yaml(YAML::Node& parent_yaml, const geometry::TransformNetwork& tfn); +} // namespace jet diff --git a/filtering/yaml_matrix.hh b/filtering/yaml_matrix.hh new file mode 100644 index 0000000..8e2987e --- /dev/null +++ b/filtering/yaml_matrix.hh @@ -0,0 +1,39 @@ +#include "third_party/experiments/eigen.hh" + +//%deps(yaml-cpp) +#include +#include + +namespace jet { + +template +void set_matrix(YAML::Node& node, const std::string& id, const MatNd& mat) { + for (int j = 0; j < cols; ++j) { + for (int i = 0; i < rows; ++i) { + node[id].push_back(mat(i, j)); + } + } +} + +template +MatNd read_matrix(const YAML::Node& node) { + using Mat = const MatNd; + assert(node); + const std::vector vv = node.as>(); + const Eigen::Map mmap_mat(vv.data()); + // Copy the map + const Mat mat = mmap_mat; + return mat; +} + +template +void read_matrix(const YAML::Node& node, MatNd& mat) { + using Mat = const MatNd; + assert(node); + const std::vector vv = node.as>(); + const Eigen::Map mmap_mat(vv.data()); + // Copy the map + mat = mmap_mat; +} + +} // namespace jet \ No newline at end of file diff --git a/filtering/yaml_matrix_test.cc b/filtering/yaml_matrix_test.cc new file mode 100644 index 0000000..89274b7 --- /dev/null +++ b/filtering/yaml_matrix_test.cc @@ -0,0 +1,35 @@ +//%deps(${GTEST_LIBRARIES}, pthread) +//%deps(yaml-cpp) +#include "filtering/yaml_matrix.hh" +// #include "third_party/experiments/testing/gtest.hh" + +#include + +namespace jet { + +// Can't make this work, todo: figure out +// TEST(YamlMatrix, yaml_matrix) { +// const MatNd<3, 5> xx = MatNd<3, 5>::Random(); +// +// YAML::Node node; +// set_matrix(node, "my_matrix", xx); +// +// const MatNd<3, 5> result = read_matrix<3, 5>(node, "my_matrix"); +// } + +void go() { + const MatNd<3, 5> xx = MatNd<3, 5>::Random(); + + YAML::Node node; + set_matrix(node, "my_matrix", xx); + + const MatNd<3, 5> result = read_matrix<3, 5>(node["my_matrix"]); + + assert((xx - result).norm() < 1e-5); +} + +} // namespace jet + +int main() { + jet::go(); +} \ No newline at end of file diff --git a/infrastructure/comms/mqtt_subscriber.cc b/infrastructure/comms/mqtt_subscriber.cc index 83fa3f0..3bbe5ef 100644 --- a/infrastructure/comms/mqtt_subscriber.cc +++ b/infrastructure/comms/mqtt_subscriber.cc @@ -58,7 +58,7 @@ void MqttSubscriber::connect() { // from two threads // TODO(jake, ben): Figure that out if (connecting_) { - std::cerr << channel_name_ << ": Already connecting, not attempting" << std::endl; + // std::cerr << channel_name_ << ": Already connecting, not attempting" << std::endl; return; } connecting_ = true; @@ -85,9 +85,9 @@ void MqttSubscriber::connect() { bool MqttSubscriber::get_mqtt_message(mqtt::const_message_ptr& mqtt_message_ptr, const Duration& timeout) { const bool is_connected = mqtt_client_->is_connected(); if (!is_connected || connecting_) { - std::cerr << channel_name_ - << ": Not connected. Skipping read. Reason: " << (connecting_ ? "Connecting" : "Not connected") - << std::endl; + // std::cerr << channel_name_ + // << ": Not connected. Skipping read. Reason: " << (connecting_ ? "Connecting" : "Not connected") + // << std::endl; connect(); return false; } diff --git a/infrastructure/gonogo/gonogo.cc b/infrastructure/gonogo/gonogo.cc index e231d2d..cfe20b5 100644 --- a/infrastructure/gonogo/gonogo.cc +++ b/infrastructure/gonogo/gonogo.cc @@ -15,8 +15,8 @@ void GoNoGo::setName(std::string name) { } void GoNoGo::go(const std::string &status_message) { - // If it's changing state - if (!gonogomessage_.ready) { + // If it's changing state or changing message + if (!gonogomessage_.ready || status_message != gonogomessage_.status_message) { std::cout << gonogomessage_.bq_name << ": GO!" << ", msg=" << status_message << std::endl; gonogomessage_.ready = true; @@ -28,8 +28,8 @@ void GoNoGo::go(const std::string &status_message) { } void GoNoGo::nogo(const std::string &status_message) { - // If it's changing state - if (gonogomessage_.ready){ + // If it's changing state or changing message + if (gonogomessage_.ready || status_message != gonogomessage_.status_message) { std::cout << gonogomessage_.bq_name << ": NO GO!" << ", msg=" << status_message << std::endl; gonogomessage_.ready = false; diff --git a/third_party/experiments b/third_party/experiments index d19fe27..aa00f43 160000 --- a/third_party/experiments +++ b/third_party/experiments @@ -1 +1 @@ -Subproject commit d19fe279602e46ee922e2c259a4e0a91420c2342 +Subproject commit aa00f438f902d952516f26e00dc7daae6d81dd75 diff --git a/vision/fiducial_detection_and_pose.cc b/vision/fiducial_detection_and_pose.cc index 3c8cfef..b45417e 100644 --- a/vision/fiducial_detection_and_pose.cc +++ b/vision/fiducial_detection_and_pose.cc @@ -17,25 +17,24 @@ BoardIdsAndCorners get_ids_and_corners(const cv::Mat& input_image) { // CORNER_REFINE_SUBPIX, do subpixel refinement. // CORNER_REFINE_CONTOUR use contour-Points // CORNER_REFINE_APRILTAG use the AprilTag2 approach) - params->cornerRefinementWinSize = 5; - params->cornerRefinementMinAccuracy = .01; // deafult .1 - params->adaptiveThreshWinSizeMax = 8; // default 23 + // params->cornerRefinementWinSize = 5; + // params->cornerRefinementMinAccuracy = .01; // deafult .1 + // params->adaptiveThreshWinSizeMax = 8; // default 23 + params->aprilTagQuadSigma = 0; // default undocumented // TODO isaac make aruco_dictionary parameter of this method to allow for // multiple unique boards const auto t0 = time::get_current_time(); cv::aruco::detectMarkers(input_image, get_aruco_dictionary(), corners, ids, params); - std::cout << "to do cv::aruco::detectMarkers " << (float)(time::get_current_time() - t0) / 1000000 << "ms" - << std::endl; BoardIdsAndCorners result = {ids, corners}; return result; } std::vector obj_points_img_points_from_image(const BoardIdsAndCorners& ids_corners) { - cv::Mat boardPoints, imgPoints; - cv::aruco::getBoardObjectAndImagePoints(get_aruco_board(), ids_corners.corners, ids_corners.ids, boardPoints, - imgPoints); + cv::Mat board_points, img_points; + cv::aruco::getBoardObjectAndImagePoints(get_aruco_board(), ids_corners.corners, ids_corners.ids, board_points, + img_points); std::vector result; for (int i = 0; i < board_points.rows; i++) { diff --git a/vision/fiducial_detection_and_pose.hh b/vision/fiducial_detection_and_pose.hh index a240b05..ca3da72 100644 --- a/vision/fiducial_detection_and_pose.hh +++ b/vision/fiducial_detection_and_pose.hh @@ -57,7 +57,7 @@ struct MatWrapper { return true_mtx; } - std::array vals; + std::array vals; SERIALIZABLE_STRUCTURE(MatWrapper, vals); }; diff --git a/vision/fiducial_detection_message.hh b/vision/fiducial_detection_message.hh index 96cd751..b2c74d4 100644 --- a/vision/fiducial_detection_message.hh +++ b/vision/fiducial_detection_message.hh @@ -19,13 +19,13 @@ struct FiducialDetectionMessage : Message { std::vector board_points_image_points; SE3 fiducial_from_camera() const; - MESSAGE(FiducialDetectionMessage, fiducial_from_camera_log, timestamp,board_points_image_points); + MESSAGE(FiducialDetectionMessage, fiducial_from_camera_log, timestamp, board_points_image_points); }; // reconstruct with eg // board_from_camera = SE3::exp(Eigen::Map>(array)); std::optional create_fiducial_detection_message(const cv::Mat& camera_frame, - const Calibration& camera_calibration, - const Timestamp& timestamp); + const Calibration& camera_calibration, + const Timestamp& timestamp); } // namespace jet diff --git a/visualization/thrust_stand_visualizer.cc b/visualization/thrust_stand_visualizer.cc index a6446dc..c52e833 100644 --- a/visualization/thrust_stand_visualizer.cc +++ b/visualization/thrust_stand_visualizer.cc @@ -27,9 +27,7 @@ jcc::Vec4 white() { return jcc::Vec4(1.0, 1.0, 1.0, 0.8); } -void put_plane(viewer::SimpleGeometry& geo, - const SE3& world_from_plane, - const jcc::Vec4& color) { +void put_vane_shape(viewer::SimpleGeometry& geo, const SE3& world_from_plane, const jcc::Vec4& color) { viewer::Polygon poly; // The frame here looks like the following @@ -102,26 +100,27 @@ void put_quadraframe(viewer::SimpleGeometry& geo, const VaneConfiguration& vane_cfg) { const jcc::Vec4 normal_color(0.8, 0.8, 0.8, 0.8); const jcc::Vec4 vane_0_color(0.8, 0.3, 0.3, 0.8); + const jcc::Vec4 vane_2_color(0.3, 0.8, 0.3, 0.8); const SE3 com_from_oriented_vane_0 = quad_cfg.com_from_vane_unit_0 * vane_unit_from_vane(status.servo_0_angle_rad, vane_cfg); - put_plane(geo, com_from_oriented_vane_0, vane_0_color); + put_vane_shape(geo, com_from_oriented_vane_0, vane_0_color); const SE3 com_from_oriented_vane_1 = quad_cfg.com_from_vane_unit_1 * vane_unit_from_vane(status.servo_1_angle_rad, vane_cfg); - put_plane(geo, com_from_oriented_vane_1, normal_color); + put_vane_shape(geo, com_from_oriented_vane_1, normal_color); const SE3 com_from_oriented_vane_2 = quad_cfg.com_from_vane_unit_2 * vane_unit_from_vane(status.servo_2_angle_rad, vane_cfg); - put_plane(geo, com_from_oriented_vane_2, normal_color); + put_vane_shape(geo, com_from_oriented_vane_2, vane_2_color); const SE3 com_from_oriented_vane_3 = quad_cfg.com_from_vane_unit_3 * vane_unit_from_vane(status.servo_3_angle_rad, vane_cfg); - put_plane(geo, com_from_oriented_vane_3, normal_color); + put_vane_shape(geo, com_from_oriented_vane_3, normal_color); } struct ThrustStandModel { diff --git a/visualization/thrust_stand_visualizer.hh b/visualization/thrust_stand_visualizer.hh index c70bd1a..dc45f98 100644 --- a/visualization/thrust_stand_visualizer.hh +++ b/visualization/thrust_stand_visualizer.hh @@ -5,7 +5,6 @@ #include "third_party/experiments/viewer/primitives/simple_geometry.hh" #include "third_party/experiments/viewer/window_3d.hh" -// #include "visualization/test_stand_mocks.hh" #include "control/jet_vane_model.hh" #include "control/quadraframe_model.hh" #include "control/vanes_generated.hh" From 281b134082b6a8f92cfc83d8caebbfe7e231dd9c Mon Sep 17 00:00:00 2001 From: Jacob Panikulam Date: Sun, 14 Jul 2019 01:10:04 -0400 Subject: [PATCH 10/13] Debug prints --- filtering/filter_balsaq.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/filtering/filter_balsaq.cc b/filtering/filter_balsaq.cc index f543a66..0d9f788 100644 --- a/filtering/filter_balsaq.cc +++ b/filtering/filter_balsaq.cc @@ -74,6 +74,10 @@ void FilterBq::loop() { FiducialDetectionMessage detection_msg; while (fiducial_sub_->read(detection_msg, 1)) { filter_manager_.measure_fiducial(to_fiducial_meas(detection_msg)); + + std::cout << "Arrival Difference: " + << estimation::to_seconds(to_time_point(detection_msg.timestamp) - to_time_point(detection_msg.header.timestamp_ns)) + << std::endl; } const auto current_time = to_time_point(get_current_time()); From f9eb8fd98a65f98ac255f581553ea94ec43ea9ba Mon Sep 17 00:00:00 2001 From: Jacob Panikulam Date: Sun, 14 Jul 2019 01:10:17 -0400 Subject: [PATCH 11/13] Update experiments --- third_party/experiments | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/third_party/experiments b/third_party/experiments index aa00f43..aeac51a 160000 --- a/third_party/experiments +++ b/third_party/experiments @@ -1 +1 @@ -Subproject commit aa00f438f902d952516f26e00dc7daae6d81dd75 +Subproject commit aeac51a5bf9ae01f6659b11752e2429676b73a5e From d2ba65d80f0f6775ebd7b89a8b62f304d45aaa09 Mon Sep 17 00:00:00 2001 From: Jacob Panikulam Date: Sun, 14 Jul 2019 01:57:06 -0400 Subject: [PATCH 12/13] Updates --- filtering/calibrate_jet.cc | 25 ++++++++++++----------- filtering/filter_viz_balsaq.cc | 37 ++++++++++++++++++++++++++-------- filtering/filter_viz_balsaq.hh | 5 +++++ 3 files changed, 47 insertions(+), 20 deletions(-) diff --git a/filtering/calibrate_jet.cc b/filtering/calibrate_jet.cc index 3638d7c..eb3f72c 100644 --- a/filtering/calibrate_jet.cc +++ b/filtering/calibrate_jet.cc @@ -96,9 +96,9 @@ const estimation::CreateSingleImuModelConfig imu_cal_cfg{ }; const estimation::CameraCalibrationConfig camera_cal_cfg{ - .visualize_camera = false, // - .visualize_camera_distortion = false, // - .visualize_camera_frustum = false // + .visualize_camera = true, // + .visualize_camera_distortion = true, // + .visualize_camera_frustum = true // }; bool visualize_filter = true; @@ -319,18 +319,13 @@ void test_filter(const estimation::CalibrationMeasurements& cal_meas, const esti } } -void go() { +void calibrate_jet(const std::string path) { jcc::Success() << "Preparing to calibrate" << std::endl; // // Grab the calibration data from the log // - // const std::string path = "/jet/logs/calibration-log-jul4-2/"; - - // const std::string path = "/jet/logs/calibration-log-jul6-6"; - const std::string path = "/jet/logs/calibration-log-jul6-7"; - const TimeRange range{}; const ExtractionConfiguration default_cfg{}; const auto cal_measurements = extract_data_from_log(path, range, default_cfg); @@ -419,6 +414,12 @@ void go() { } // namespace filtering } // namespace jet -int main() { - jet::filtering::go(); -} \ No newline at end of file +int main(int argc, char** argv) { + assert(argc > 1); + const std::string path(argv[1]); + // const std::string path = "/jet/logs/calibration-log-jul4-2/"; + // const std::string path = "/jet/logs/calibration-log-jul6-6"; + // const std::string path = "/jet/logs/calibration-log-jul6-7"; + + jet::filtering::calibrate_jet(path); +} diff --git a/filtering/filter_viz_balsaq.cc b/filtering/filter_viz_balsaq.cc index bfb8678..8866404 100644 --- a/filtering/filter_viz_balsaq.cc +++ b/filtering/filter_viz_balsaq.cc @@ -5,13 +5,11 @@ #include "control/servo_interface.hh" #include "embedded/imu_driver/imu_message.hh" #include "embedded/servo_bq/set_servo_message.hh" +#include "filtering/transform_network_from_yaml.hh" #include "infrastructure/balsa_queue/bq_main_macro.hh" #include "vision/fiducial_detection_message.hh" #include "visualization/thrust_stand_visualizer.hh" -#include -#include - // %deps(simple_geometry) // %deps(window_3d) // %deps(fit_ellipse) @@ -20,9 +18,15 @@ #include "third_party/experiments/viewer/primitives/simple_geometry.hh" #include "third_party/experiments/viewer/window_3d.hh" +// %deps(put_transform_network) +#include "third_party/experiments/geometry/visualization/put_transform_network.hh" + //%deps(jet_model) #include "third_party/experiments/planning/jet/jet_model.hh" +#include +#include + namespace jet { namespace embedded { @@ -44,6 +48,8 @@ void FilterVizBq::init(const Config& config) { background->add_plane({ground, 0.1}); background->flip(); + transform_network_ = transform_network_from_yaml(config["transforms"]); + sensor_geo_ = view->add_primitive(); pose_geo_ = view->add_primitive(); servo_geo_ = view->add_primitive(); @@ -112,6 +118,7 @@ void FilterVizBq::draw_sensors() { if (!fiducial_history_.empty()) { const SE3 camera_from_fiducial = fiducial_history_.back().inverse(); + transform_network_.update_edge("camera", "fiducial", camera_from_fiducial); const jcc::Vec3 jet_origin(1.0, 0.0, 0.0); const Pose pose = last_pose_message_.to_pose(); @@ -123,6 +130,12 @@ void FilterVizBq::draw_sensors() { if (!accel_history_.empty()) { sensor_geo_->add_line({jcc::Vec3::Zero(), accel_history_.back().accel_mpss}); + + if (transform_network_.edges_from_node_tag().count("world") != 0) { + const SE3 world_from_imu1 = transform_network_.find_source_from_destination("world", "imu_78"); + sensor_geo_->add_line({world_from_imu1.translation(), world_from_imu1 * accel_history_.back().accel_mpss, + jcc::Vec4(0.7, 0.2, 0.2, 1.0)}); + } } sensor_geo_->add_sphere({jcc::Vec3::Zero(), 9.81}); @@ -139,17 +152,25 @@ void FilterVizBq::draw_pose() { if (got_pose_msg) { const Pose pose = pose_msg.to_pose(); - const jcc::Vec3 jet_origin(1.0, 0.0, 0.0); + + std::cout << "Pose Age: " << estimation::to_seconds(jcc::now() - to_time_point(pose_msg.timestamp)) << std::endl; pose_geo_->add_axes({pose.world_from_jet, 0.55, 3.0, true}); - const SE3 world_from_jet(pose.world_from_jet.so3(), jet_origin); - pose_geo_->add_axes({world_from_jet, 0.55, 3.0, true}); + // const jcc::Vec3 jet_origin(1.0, 0.0, 0.0); + // const SE3 world_from_jet(pose.world_from_jet.so3(), jet_origin); + // pose_geo_->add_axes({world_from_jet, 0.55, 3.0, true}); + + transform_network_.update_edge("world", "vehicle", pose.world_from_jet); + + geometry::put_transform_network(*pose_geo_, transform_network_, "world"); pose_geo_->flip(); - const SE3 jetmodel_from_body = jcc::exp_x(0.0); - jet_tree_->set_world_from_root(world_from_jet * jetmodel_from_body.inverse()); + // const SE3 body_from_model = jcc::exp_x(-M_PI * 0.5); + const SE3 body_from_model = jcc::exp_x(0.0); + + jet_tree_->set_world_from_root(pose.world_from_jet * body_from_model); } } diff --git a/filtering/filter_viz_balsaq.hh b/filtering/filter_viz_balsaq.hh index 6eb495b..f1d6ade 100644 --- a/filtering/filter_viz_balsaq.hh +++ b/filtering/filter_viz_balsaq.hh @@ -11,6 +11,9 @@ //%deps(jet_filter) #include "third_party/experiments/estimation/jet/jet_filter.hh" +//%deps(transform_network) +#include "third_party/experiments/geometry/kinematics/transform_network.hh" + #include "third_party/experiments/viewer/primitives/simple_geometry.hh" #include "third_party/experiments/viewer/primitives/scene_tree.hh" @@ -56,6 +59,8 @@ class FilterVizBq : public BalsaQ { std::deque fiducial_history_; std::deque mag_utesla_; + geometry::TransformNetwork transform_network_; + estimation::jet_filter::JetFilter jf_; }; From f0d4063c3ddcbfba61633316fd27c35a5fe9786c Mon Sep 17 00:00:00 2001 From: Jacob Panikulam Date: Sat, 20 Jul 2019 13:25:12 -0400 Subject: [PATCH 13/13] Push latest --- camera/cfg/camera_config_2.yaml | 32 +++++-- camera/cfg/camera_jet.yaml | 6 ++ config/hoverjet/camera.yaml | 4 +- filtering/calibrate_camera_radiometry.cc | 112 +++++++++++++++++++++++ filtering/calibrate_jet.cc | 79 +++++++--------- filtering/filter_viz_balsaq.cc | 6 +- third_party/experiments | 2 +- 7 files changed, 182 insertions(+), 59 deletions(-) create mode 100644 filtering/calibrate_camera_radiometry.cc diff --git a/camera/cfg/camera_config_2.yaml b/camera/cfg/camera_config_2.yaml index ec34181..2a58f4a 100644 --- a/camera/cfg/camera_config_2.yaml +++ b/camera/cfg/camera_config_2.yaml @@ -1,10 +1,30 @@ serial_number: CA38DB5E v4l_path: usb-046d_Logitech_Webcam_C930e_CA38DB5E-video-index0 note: these cal values were copied from camera 0 -camera_matrix: [499.7749869454186, 0, 309.3792706235992, - 0, 496.9300965132637, 241.6934416030273, - 0, 0, 1] -distortion_coefficients: [0.003861115403120386, 0.09541932181851349, 0.001898991151152847, - -0.003082742498836169, -0.2932184860155891] -resolution: [640, 480] \ No newline at end of file +distortion_coefficients: + - 0.06693324518972132 + - -0.2241512297832522 + - 0.009935840339065886 + - 0.008486955527599985 + - 0.1017895523815406 +# distortion_coefficients: +# - 0.0 +# - 0.0 +# - 0.0 +# - 0.0 +# - 0.0 +camera_matrix: + - 279.7596429981606 + - 0 + - 235.9919590561939 + - 0 + - 280.0340888659648 + - 141.9506134614783 + - 0 + - 0 + - 1 +resolution: + - 480 + - 270 + diff --git a/camera/cfg/camera_jet.yaml b/camera/cfg/camera_jet.yaml index f0b582a..045e5e2 100644 --- a/camera/cfg/camera_jet.yaml +++ b/camera/cfg/camera_jet.yaml @@ -4,6 +4,12 @@ distortion_coefficients: - 0.009935840339065886 - 0.008486955527599985 - 0.1017895523815406 +# distortion_coefficients: +# - 0.0 +# - 0.0 +# - 0.0 +# - 0.0 +# - 0.0 serial_number: 5CFD076E camera_matrix: - 279.7596429981606 diff --git a/config/hoverjet/camera.yaml b/config/hoverjet/camera.yaml index bb2e681..dbcc5b0 100644 --- a/config/hoverjet/camera.yaml +++ b/config/hoverjet/camera.yaml @@ -1,5 +1,5 @@ -width_pixels: 640 -height_pixels: 480 +width_pixels: 480 +height_pixels: 270 # Bold, I know! frames_per_second: 20 # Disable autofocus diff --git a/filtering/calibrate_camera_radiometry.cc b/filtering/calibrate_camera_radiometry.cc new file mode 100644 index 0000000..d16dc6b --- /dev/null +++ b/filtering/calibrate_camera_radiometry.cc @@ -0,0 +1,112 @@ +#include "camera/camera_manager.hh" +#include "camera/webcam_interface.hh" + +#include +#include + +//%deps(ui2d) +#include "third_party/experiments/viewer/interaction/ui2d.hh" +//%deps(simple_geometry) +#include "third_party/experiments/viewer/primitives/simple_geometry.hh" +//%deps(window_3d) +#include "third_party/experiments/viewer/window_3d.hh" + +#include +#include +#include + +namespace jet { + +void capture_frames(cv::VideoCapture& cap, int gamma) { + const auto view = viewer::get_window3d("Radiometry"); + const auto ui2d = view->add_primitive(); + + viewer::LinePlotBuilder builder("Radiometry"); + auto& red = builder.make_subplot("red", jcc::Vec4(1.0, 0.0, 0.0, 1.0), 3.0, false, viewer::LinePlotStyle::Scatter); + auto& green = + builder.make_subplot("green", jcc::Vec4(0.0, 1.0, 0.0, 1.0), 3.0, false, viewer::LinePlotStyle::Scatter); + auto& blue = builder.make_subplot("blue", jcc::Vec4(0.0, 0.0, 1.0, 1.0), 3.0, false, viewer::LinePlotStyle::Scatter); + auto& yellow = + builder.make_subplot("yellow", jcc::Vec4(1.0, 1.0, 0.0, 1.0), 3.0, false, viewer::LinePlotStyle::Scatter); + auto& teal = builder.make_subplot("teal", jcc::Vec4(0.0, 1.0, 1.0, 1.0), 3.0, false, viewer::LinePlotStyle::Scatter); + + int frames_per_exposure = 20; + cv::Mat camera_frame; + for (int exp = 1; exp <= 2048; exp *= 2) { + // Update exposure settings and clear buffer + cap.set(cv::CAP_PROP_EXPOSURE, exp); + for (int i = 0; i < 50; ++i) { + cap.read(camera_frame); + } + + for (int frame_num = 0; frame_num < frames_per_exposure; ++frame_num) { + if (cap.read(camera_frame)) { + // cv::imshow("banana", camera_frame); + + const int exposure_real = cap.get(cv::CAP_PROP_EXPOSURE); + + const cv::Vec3b bgr_pixel_1 = camera_frame.at(200, 200); + const cv::Vec3b bgr_pixel_2 = camera_frame.at(100, 200); + const cv::Vec3b bgr_pixel_3 = camera_frame.at(200, 100); + const cv::Vec3b bgr_pixel_4 = camera_frame.at(30, 30); + const cv::Vec3b bgr_pixel_5 = camera_frame.at(230, 430); + + const int b_1 = static_cast(bgr_pixel_1[0]); + const int b_2 = static_cast(bgr_pixel_2[0]); + const int b_3 = static_cast(bgr_pixel_3[0]); + + const int b_4 = static_cast(bgr_pixel_4[0]); + const int b_5 = static_cast(bgr_pixel_5[0]); + + std::cout << "[" << exposure_real << ", " << b_1 << ", " << b_2 << ", " << b_3 << ", " << b_4 << ", " << b_5 + << "]," << std::endl; + + // red << jcc::Vec2(exposure_real, static_cast(bgr_pixel[0])); + // green << jcc::Vec2(exposure_real, static_cast(bgr_pixel[1])); + // blue << jcc::Vec2(exposure_real, static_cast(bgr_pixel[2])); + red << jcc::Vec2(exposure_real, static_cast(b_1)); + green << jcc::Vec2(exposure_real, static_cast(b_2)); + blue << jcc::Vec2(exposure_real, static_cast(b_3)); + yellow << jcc::Vec2(exposure_real, static_cast(b_4)); + teal << jcc::Vec2(exposure_real, static_cast(b_5)); + + cv::waitKey(10); + } + } + ui2d->add_lineplot(builder); + ui2d->flip(); + // std::cout << "Exposure: " << exp << std::endl; + } + view->spin_until_step(); +} + +void go() { + const CameraManager camera_manager; + + const std::string serial_number = "CA38DB5E"; + const auto camera = camera_manager.get_camera(serial_number); + + CameraConfiguration camera_config; + + camera_config.frames_per_second = 10; + camera_config.exposure = 3; + camera_config.auto_focus = 0; + camera_config.auto_exposure = 1; + camera_config.width_pixels = 480; + camera_config.height_pixels = 270; + + auto cap = cv::VideoCapture(camera.v4l_path); + initialize_camera_hardware(camera_config, cap); + + // CAP_PROP_GAMMA + // CAP_PROP_GAIN + + int gamma = 1; + capture_frames(cap, gamma); +} + +} // namespace jet + +int main() { + jet::go(); +} \ No newline at end of file diff --git a/filtering/calibrate_jet.cc b/filtering/calibrate_jet.cc index eb3f72c..f686040 100644 --- a/filtering/calibrate_jet.cc +++ b/filtering/calibrate_jet.cc @@ -18,6 +18,9 @@ #include "third_party/experiments/estimation/visualization/visualize_calibration.hh" #include "third_party/experiments/estimation/visualization/visualize_camera_calibration.hh" +//%deps(bootstrap_jet) +#include "third_party/experiments/estimation/jet/bootstrap_jet.hh" + //%deps(form_coordinate_frame) //%deps(put_transform_network) #include "third_party/experiments/geometry/spatial/form_coordinate_frame.hh" @@ -35,11 +38,10 @@ //%deps(log) #include "third_party/experiments/logging/log.hh" -#include "filtering/extract_data_from_log.hh" - #include "camera/camera_manager.hh" #include "vision/fiducial_detection_and_pose.hh" +#include "filtering/extract_data_from_log.hh" #include "filtering/transform_network_from_yaml.hh" #include "filtering/yaml_matrix.hh" @@ -71,6 +73,17 @@ estimation::ProjectionCoefficients proj_coeffs_from_opencv(const Calibration& ca model.rows = cal.rows; model.cols = cal.cols; + std::cout << "model.fx = " << cal.camera_matrix.at(0, 0) << std::endl; + std::cout << "model.fy = " << cal.camera_matrix.at(1, 1) << std::endl; + std::cout << "model.cx = " << cal.camera_matrix.at(0, 2) << std::endl; + std::cout << "model.cy = " << cal.camera_matrix.at(1, 2) << std::endl; + std::cout << "model.k1 = " << cal.distortion_coefficients.at(0) << std::endl; + std::cout << "model.k3 = " << cal.distortion_coefficients.at(4) << std::endl; + std::cout << "model.k2 = " << cal.distortion_coefficients.at(1) << std::endl; + std::cout << "model.p1 = " << cal.distortion_coefficients.at(2) << std::endl; + std::cout << "model.p2 = " << cal.distortion_coefficients.at(3) << std::endl; + std::cout << "model.rows = " << cal.rows << std::endl; + std::cout << "model.cols = " << cal.cols << std::endl; return model; } @@ -160,41 +173,6 @@ jcc::Optional find_nearest_fiducial_in_time( return {}; } -SE3 compute_world_from_camera(const estimation::SingleImuCalibration& imu_cal) { - const geometry::Unit3 g_camera_frame = imu_cal.camera_from_gyro * imu_cal.g_estimate.direction; - const SO3 world_from_camera = geometry::spatial::form_coordinate_frame_from_zhat(g_camera_frame); - return SE3(world_from_camera, jcc::Vec3::Zero()); -} - -ejf::Parameters compute_filter_fixed_parameters( - const std::vector> fiducial_measurements, - const estimation::jet::JetModel& jet_model) { - const auto& imu_cal_1 = jet_model.imu_calibration_from_imu_id.at(IMU_1); - const auto& imu_cal_2 = jet_model.imu_calibration_from_imu_id.at(IMU_2); - - const SO3 world_from_camera_1 = compute_world_from_camera(imu_cal_1).so3(); - const SO3 world_from_camera_2 = compute_world_from_camera(imu_cal_2).so3(); - jcc::Warning() << "[Filter Setup] IMU->IMU Alignment Error: " - << (world_from_camera_1 * world_from_camera_2.inverse()).log().norm() << " Radians" << std::endl; - - const auto maybe_nearest_fiducial = find_nearest_fiducial_in_time(fiducial_measurements, imu_cal_1.g_estimate.time); - assert(maybe_nearest_fiducial); - const auto fiducial_at_g_estimate = *maybe_nearest_fiducial; - const SO3 fiducial_from_camera = fiducial_at_g_estimate.T_fiducial_from_camera.so3(); - const SO3 world_from_fiducial = world_from_camera_1 * fiducial_from_camera.inverse(); - - auto p = ejf::JetFilter::reasonable_parameters(); - { - p.T_world_from_fiducial = SE3(world_from_fiducial, jcc::Vec3::Zero()); - - p.T_imu1_from_vehicle = jet_model.transform_network.find_source_from_destination("imu_78", "vehicle"); - p.T_imu2_from_vehicle = jet_model.transform_network.find_source_from_destination("imu_36", "vehicle"); - p.T_camera_from_vehicle = jet_model.transform_network.find_source_from_destination("camera", "vehicle"); - } - - return p; -} - void test_filter(const estimation::CalibrationMeasurements& cal_meas, const estimation::jet::JetModel& jet_model) { const auto view = viewer::get_window3d("Calibration"); const auto geo = view->add_primitive(); @@ -220,17 +198,16 @@ void test_filter(const estimation::CalibrationMeasurements& cal_meas, const esti const auto t0 = cal_meas.first(); // const auto t0 = imu_1_meas.accel_meas.front().timestamp; - const auto p = compute_filter_fixed_parameters(cal_meas.fiducial_meas, jet_model); - - auto xp0 = ejf::JetFilter::reasonable_initial_state(t0); + auto tf_bootstrap = jet_model.transform_network; { - const SE3 world_from_vehicle = - compute_world_from_camera(jet_model.imu_calibration_from_imu_id.at(IMU_2)) * p.T_camera_from_vehicle; - xp0.x.R_world_from_body = world_from_vehicle.so3(); - xp0.x.x_world = world_from_vehicle.translation(); + const auto maybe_nearest_fiducial = + find_nearest_fiducial_in_time(cal_meas.fiducial_meas, imu_1_cal.g_estimate.time); + assert(maybe_nearest_fiducial); + tf_bootstrap.add_edge("fiducial", "camera", maybe_nearest_fiducial->T_fiducial_from_camera); } + const auto bootstrap_result = ejf::bootstrap_jet(imu_1_cal.g_estimate.direction, tf_bootstrap, t0); - ejf::JetFilter jf(xp0, p); + ejf::JetFilter jf(bootstrap_result.xp0, bootstrap_result.parameters); for (const auto& fiducial_meas : cal_meas.fiducial_meas) { jf.measure_fiducial(fiducial_meas.measurement, fiducial_meas.timestamp); @@ -264,7 +241,7 @@ void test_filter(const estimation::CalibrationMeasurements& cal_meas, const esti int k = 0; while (jf.next_measurement()) { ++k; - if (k % 2 != 0) { + if (k % 1 != 0) { continue; } const auto state = jf.state().x; @@ -298,13 +275,13 @@ void test_filter(const estimation::CalibrationMeasurements& cal_meas, const esti // Angular Velocity geo->add_line({jcc::Vec3::Zero(), -state.eps_dot.tail<3>(), jcc::Vec4(0.0, 1.0, 0.0, 1.0), 5.0}); - auto tfn2 = jet_model.transform_network; const auto maybe_nearest_fiducial = find_nearest_fiducial_in_time(cal_meas.fiducial_meas, jf.state().time_of_validity); // const SE3 visualized_world_from_vehicle = SE3(T_world_from_body.so3(), jet_origin); const SE3 visualized_world_from_vehicle = T_world_from_body; + auto tfn2 = jet_model.transform_network; tfn2.update_edge("world", "vehicle", visualized_world_from_vehicle); if (maybe_nearest_fiducial) { tfn2.update_edge("fiducial", "camera", maybe_nearest_fiducial->T_fiducial_from_camera); @@ -365,8 +342,10 @@ void calibrate_jet(const std::string path) { std::vector> collected_fiducial_meas; int i = 0; + estimation::TimePoint last_image_time; while (true) { const auto image = image_stream.next(); + last_image_time = image->time; ++i; if (!image) { @@ -394,6 +373,12 @@ void calibrate_jet(const std::string path) { } } + geo->clear(); + ui2d->clear(); + + jcc::Warning() << "Gap between last image and last fiducial: " + << estimation::to_seconds(last_image_time - cal_measurements.last()); + jcc::Success() << "[Filter] Testing filter..." << std::endl; estimation::CalibrationMeasurements new_cal_measurements = cal_measurements; { new_cal_measurements.fiducial_meas = collected_fiducial_meas; } diff --git a/filtering/filter_viz_balsaq.cc b/filtering/filter_viz_balsaq.cc index 8866404..1db0b42 100644 --- a/filtering/filter_viz_balsaq.cc +++ b/filtering/filter_viz_balsaq.cc @@ -134,7 +134,7 @@ void FilterVizBq::draw_sensors() { if (transform_network_.edges_from_node_tag().count("world") != 0) { const SE3 world_from_imu1 = transform_network_.find_source_from_destination("world", "imu_78"); sensor_geo_->add_line({world_from_imu1.translation(), world_from_imu1 * accel_history_.back().accel_mpss, - jcc::Vec4(0.7, 0.2, 0.2, 1.0)}); + jcc::Vec4(0.5, 0.5, 0.8, 1.0)}); } } @@ -167,8 +167,8 @@ void FilterVizBq::draw_pose() { pose_geo_->flip(); - // const SE3 body_from_model = jcc::exp_x(-M_PI * 0.5); - const SE3 body_from_model = jcc::exp_x(0.0); + const SE3 body_from_model = jcc::exp_z(-M_PI * 0.5); + // const SE3 body_from_model = jcc::exp_z(0.0); jet_tree_->set_world_from_root(pose.world_from_jet * body_from_model); } diff --git a/third_party/experiments b/third_party/experiments index aeac51a..b9398d8 160000 --- a/third_party/experiments +++ b/third_party/experiments @@ -1 +1 @@ -Subproject commit aeac51a5bf9ae01f6659b11752e2429676b73a5e +Subproject commit b9398d89ce28c2e35ab780681ec3e72641eac213