More Filter Changes

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.

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<double> distortion_coefficient_values = cfg["distortion_coefficients"].as<std::vector<double>>();
   calibration.distortion_coefficients = cv::Mat(1, 5, CV_64F, distortion_coefficient_values.data()).clone();
+
+  const std::vector<double> resolution = cfg["resolution"].as<std::vector<double>>();
+  calibration.cols = resolution.at(0);
+  calibration.rows = resolution.at(1);
   return calibration;
 }
 

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 {

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]
+                   -0.003082742498836169, -0.2932184860155891]
+resolution: [640, 480]

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]
+
+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
+

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]
+                   -0.003082742498836169, -0.2932184860155891]
+
+resolution: [640, 480]

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]

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

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<std::vector<double>>();
-  const auto log_rotation_tag_from_world =    node["log_rotation_tag_from_world"].as<std::vector<double>>();
+  const auto log_rotation_tag_from_world = node["log_rotation_tag_from_world"].as<std::vector<double>>();
 
   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<std::vector<double>>();
-  const auto log_rotation_camera_from_frame =    node["log_rotation_camera_from_frame"].as<std::vector<double>>();
+  const auto log_rotation_camera_from_frame = node["log_rotation_camera_from_frame"].as<std::vector<double>>();
   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;
 }
 

config/fiducial_map/read_fiducial_map.hh

@@ -8,21 +8,21 @@
 namespace jet {
 
 
-struct fiducial_pose
+struct FiducialDescription
 {
     SE3 tag_from_world;
     int tag_size_squares;
     double arcode_width_mm;
     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
 

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

config/hoverjet/controller.yaml

@@ -0,0 +1,9 @@
+k_proportional:
+  - 0.2
+  - 0.2
+  - 0.4
+
+k_derivative:
+  - 0.2
+  - 0.2
+  - 0.4

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

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