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/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..2a58f4a 100644 --- a/camera/cfg/camera_config_2.yaml +++ b/camera/cfg/camera_config_2.yaml @@ -1,8 +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] \ 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_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..045e5e2 100644 --- a/camera/cfg/camera_jet.yaml +++ b/camera/cfg/camera_jet.yaml @@ -1,7 +1,27 @@ +distortion_coefficients: + - 0.06693324518972132 + - -0.2241512297832522 + - 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 + - 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/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/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/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 0ed4200..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,10 +27,19 @@ 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 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) // @@ -50,9 +55,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 = -(Kp * target_from_jet.log()) + -(Kd * w_jet_frame); Wrench target_wrench; target_wrench.torque_Nm = desired_torque_jet_frame; @@ -98,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; } @@ -124,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/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_bq/servo_balsaq.cc b/embedded/servo_bq/servo_balsaq.cc index 3df4d18..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 @@ -16,39 +18,46 @@ namespace jet { void ServoBq::init(const Config& config) { - subscriber = make_subscriber("servo_command_channel"); + 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); + servos_.push_back(servo); } } 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++) { - auto 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); + 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"); } } 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..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" @@ -15,8 +14,9 @@ class ServoBq : public BalsaQ { void shutdown(); private: - std::vector servos; - SubscriberPtr subscriber; + 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 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/embedded/servo_driver/cfg/servo_configs.yaml b/embedded/servo_driver/cfg/servo_configs.yaml index 724b04d..0abafd7 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: 2713 + calibrated_min_pwm_count: 1239 + max_angle_radians: 0.174533 1: index: 1 - calibrated_center: 50 - calibrated_max: 71 - max_angle: 20 + calibrated_max_pwm_count: 2819 + calibrated_min_pwm_count: 1275 + 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: 2739 + calibrated_min_pwm_count: 1257 + max_angle_radians: 0.174533 diff --git a/embedded/servo_driver/servo_calibrate.cc b/embedded/servo_driver/servo_calibrate.cc index 5a967b3..f2c9476 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() << " 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() << 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() << 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() << 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" @@ -40,10 +40,11 @@ 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.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..281de03 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_vane_angle_radians(0); } -void ServoDriver::set_percentage(float unchecked_percentage) { - if (unchecked_percentage > 100 || 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(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 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); } -float ServoDriver::get_percentage() const { +double ServoDriver::get_percentage() const { return percentage_; } +int 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_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_); } void ServoDriver::shutdown_pwm() { diff --git a/embedded/servo_driver/servo_driver.hh b/embedded/servo_driver/servo_driver.hh index 19dd314..6f7aae6 100644 --- a/embedded/servo_driver/servo_driver.hh +++ b/embedded/servo_driver/servo_driver.hh @@ -17,20 +17,31 @@ class ServoDriver { public: ServoDriver(const Config& config); - void set_percentage(float percentage); - float get_percentage() const; + 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; + int get_pwm_count() const; int get_servo_index() const; - void set_angle_radians(float 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 + // 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. + int calibrated_max_pwm_count_; + // The count corresponding to zero angle. + int calibrated_min_pwm_count_; + // Max angle in radians that the servo can achieve. + double max_angle_radians_; + double percentage_; + int pwm_count_; }; 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; } diff --git a/filtering/calibrate_camera_from_log.cc b/filtering/calibrate_camera_from_log.cc new file mode 100644 index 0000000..21d66dc --- /dev/null +++ b/filtering/calibrate_camera_from_log.cc @@ -0,0 +1,175 @@ +//%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 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; + 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::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; + node["serial_number"] = serial_number; + + { + 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_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 new file mode 100644 index 0000000..f686040 --- /dev/null +++ b/filtering/calibrate_jet.cc @@ -0,0 +1,410 @@ +//%deps(jet_filter) +//%deps(jet_optimizer) +//%deps(nonlinear_camera_model) +//%deps(warn_sensor_rates) +//%deps(make_interpolator) +#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(create_static_jet_model) +//%deps(robust_pnp) +//%deps(visualize_calibration) +//%deps(visualize_camera_calibration) +#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(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" +#include "third_party/experiments/geometry/visualization/put_transform_network.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 "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" + +// - 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; + + 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; +} + +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; +} + +} // 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 = true, // + .visualize_camera_distortion = true, // + .visualize_camera_frustum = true // +}; + +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 || visualize_filter); +} + +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(); + } +} + +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"]; + + 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 {}; +} + +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(); + + 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; + 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 tf_bootstrap = jet_model.transform_network; + { + 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(bootstrap_result.xp0, bootstrap_result.parameters); + + for (const auto& fiducial_meas : cal_meas.fiducial_meas) { + jf.measure_fiducial(fiducial_meas.measurement, fiducial_meas.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); + } + + 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); + } + } + + int k = 0; + while (jf.next_measurement()) { + ++k; + if (k % 1 != 0) { + continue; + } + const auto state = jf.state().x; + + 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}); + + 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); + } + + 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(); + } +} + +void calibrate_jet(const std::string path) { + jcc::Success() << "Preparing to calibrate" << std::endl; + + // + // Grab the calibration data from the log + // + + 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; + + // + // Validate the fiducial reprojections + // + + jcc::Success() << "[Camera] Validating fiducial measurements" << std::endl; + ImageStream image_stream(path, range); + 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(); + + // Compute new fiducial measurements + 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) { + break; + } + const auto calibration = cam_mgr.get_calibration(image->serial_number); + 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; + 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 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); + } + } + + 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; } + + 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; +} + +} // namespace filtering +} // namespace jet + +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/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 new file mode 100644 index 0000000..9ac8f21 --- /dev/null +++ b/filtering/extract_data_from_log.cc @@ -0,0 +1,137 @@ +#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" +#include "filtering/convert_messages.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; + + 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) { + const auto time_of_validity = to_time_point(msg.timestamp); + + if ((time_of_validity < range.start) || (time_of_validity > range.end)) { + return; + } + + meas->fiducial_meas.push_back(to_fiducial_meas(msg)); +} + +} // namespace + +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"}; + + 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..273066d --- /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::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..0d9f788 100644 --- a/filtering/filter_balsaq.cc +++ b/filtering/filter_balsaq.cc @@ -1,121 +1,112 @@ //%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"]); + + 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]); - std::cout << "Reading fiducial" << std::endl; - 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_; + 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; - const auto fiducial_time_of_validity = to_time_point(detection_msg.timestamp); + 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"); + } - 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.time_of_validity = fiducial_time_of_validity; - jf_.reset(xp0); + filter_manager_.init(filter_cfg); +} - gonogo().go(); - } +void FilterBq::loop() { + // + // Update with IMU measurements + // - std::cout << "Free-running" << std::endl; - jf_.measure_fiducial(fiducial_meas, fiducial_time_of_validity); - jf_.free_run(); - const auto state = jf_.state().x; + 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); } - 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; + 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; } - 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); + const auto current_time = to_time_point(get_current_time()); + filter_manager_.update(current_time); - std::cout << "eps_dot: " << state.eps_dot.transpose() << std::endl; + // + // Report a state + // + 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; - pose.world_from_jet = (vehicle_real_from_vehicle_filtered * state.T_body_from_world).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; + const SE3 T_world_from_body = estimation::jet_filter::get_world_from_body(state.x); + pose.world_from_jet = T_world_from_body; - 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() * - state.eps_dot.head<3>(); - pose.w_world_frame = (vehicle_real_from_vehicle_filtered.so3() * state.T_body_from_world.so3()).inverse() * - state.eps_dot.tail<3>(); - pose.timestamp = imu_msg.timestamp; + 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); - std::cout << "Transmitting" << std::endl; 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_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..1db0b42 100644 --- a/filtering/filter_viz_balsaq.cc +++ b/filtering/filter_viz_balsaq.cc @@ -5,22 +5,28 @@ #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 "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) +// %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" #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 { @@ -42,14 +48,21 @@ 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(); - persistent_ = view->add_primitive(); servo_geo_ = view->add_primitive(); + jet_tree_ = view->add_primitive(); - std::cout << "Subscribing IMU" << std::endl; - imu_sub_ = make_subscriber("imu"); + 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_1"); std::cout << "Subscribing Fiducial" << std::endl; fiducial_sub_ = make_subscriber("fiducial_detection_channel"); @@ -58,8 +71,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,20 +82,8 @@ 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_; - - // 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); } @@ -98,10 +97,17 @@ void FilterVizBq::draw_sensors() { mag_utesla_.push_back({mag_utesla}); } - 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)}); + } + while (fiducial_history_.size() > 10u) { fiducial_history_.pop_front(); } @@ -110,41 +116,26 @@ 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()}); - if (!fiducial_history_.empty()) { - const SE3 fiducial_from_camera = fiducial_history_.back(); + const SE3 camera_from_fiducial = fiducial_history_.back().inverse(); + transform_network_.update_edge("camera", "fiducial", camera_from_fiducial); - constexpr bool DRAW_FIDUCIAL_POSE = true; - constexpr bool DRAW_VEHICLE_POSE = false; - constexpr bool DRAW_FIDUCIAL_IN_BODY_FRAME = false; + 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 (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()) { 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.5, 0.5, 0.8, 1.0)}); + } } sensor_geo_->add_sphere({jcc::Vec3::Zero(), 9.81}); @@ -155,13 +146,31 @@ 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}); + + 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 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 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/filtering/filter_viz_balsaq.hh b/filtering/filter_viz_balsaq.hh index 9c2c4c6..f1d6ade 100644 --- a/filtering/filter_viz_balsaq.hh +++ b/filtering/filter_viz_balsaq.hh @@ -4,13 +4,18 @@ #include #include +#include "filtering/pose_message.hh" #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(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" namespace jet { namespace embedded { @@ -35,24 +40,27 @@ 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 persistent_; + std::shared_ptr jet_tree_; 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_; + geometry::TransformNetwork transform_network_; + estimation::jet_filter::JetFilter jf_; }; 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/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/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/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..b9398d8 160000 --- a/third_party/experiments +++ b/third_party/experiments @@ -1 +1 @@ -Subproject commit c056c6c7503d087266e871fb68104d58c2fbf6b6 +Subproject commit b9398d89ce28c2e35ab780681ec3e72641eac213 diff --git a/vision/fiducial_detection_and_pose.cc b/vision/fiducial_detection_and_pose.cc index 576aba5..b45417e 100644 --- a/vision/fiducial_detection_and_pose.cc +++ b/vision/fiducial_detection_and_pose.cc @@ -17,40 +17,37 @@ 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 < 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..ca3da72 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,12 +52,12 @@ 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; } - std::array vals; + std::array vals; SERIALIZABLE_STRUCTURE(MatWrapper, vals); }; @@ -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 {}; } 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"