Get intrinsics from input file

.vscode/settings.json

@@ -10,7 +10,7 @@
         "**/__pycache__": true
     },
     "[cpp]": {
-        "editor.wordBasedSuggestions": false,
+        "editor.wordBasedSuggestions": "off",
         "editor.suggest.insertMode": "replace",
         "editor.semanticHighlighting.enabled": true,
         "editor.tabSize": 2,
@@ -150,7 +150,6 @@
         "--ignore=CNL100",
         "--max-line-length=100",
     ],
-    "ros.distro": "humble",
     "terminal.integrated.gpuAcceleration": "off",
     "cmake.debugConfig": {
         "args": [
@@ -185,5 +184,8 @@
         "docs/doxygen/latex/**",
         "docs/software/todo.md",
         "uncrustify.cfg"
-    ]
+    ],
+    "search.exclude": {
+        ".git/": true
+    }
 }

config/example.yaml

@@ -13,7 +13,7 @@
             run_number: 0
             truth_type: "cyclic"
             pos_frequency: [0.3, 0.5, 0.7]
-            ang_frequency: [0.2, 0.3, 0.4]
+            ang_frequency: [0.0, 0.0, 0.0]
             pos_offset: [0.0, 0.0, 0.0]
             ang_offset: [0.0, 0.0, 0.0]
             ang_amplitude: 0.1
@@ -34,7 +34,7 @@
             - "vn100"
 
         camera_list:
-            # - "ace"
+            - "ace"
 
         tracker_list:
             - "orb"
@@ -101,6 +101,15 @@
                 tracker: "orb"
                 pos_stability: 1.0e-9
                 ang_stability: 1.0e-9
+                intrinsics:
+                    F: 1.0
+                    c_x: 320.0
+                    c_y: 240.0
+                    k_1: 0.0
+                    k_2: 0.0
+                    p_1: 0.0
+                    p_2: 0.0
+                    pixel_size: 1.0e-2
                 sim_params:
                     time_bias_error: 1.0e-3
                     time_skew_error: 1.0e-9

eval/plot.py

@@ -269,7 +269,7 @@ def plot_imu_bias_updates(self, imu_dfs):
         return fig
 
     def plot_imu_pos(self, imu_dfs):
-        """TODO."""
+        # TODO(jhartzer): Add description
         fig, (axs_1, axs_2, axs_3) = plt.subplots(3, 1)
         a = calculate_alpha(len(imu_dfs))
         t_imu = np.zeros([len(imu_dfs), len(imu_dfs[0]['time'])])
@@ -330,7 +330,7 @@ def plot_imu_ang(self, imu_dfs):
         return fig
 
     def plot_imu_bias_acc(self, imu_dfs):
-        """TODO."""
+        # TODO(jhartzer): Add description
         fig, (axs_1, axs_2, axs_3) = plt.subplots(3, 1)
         a = calculate_alpha(len(imu_dfs))
         t_imu = np.zeros([len(imu_dfs), len(imu_dfs[0]['time'])])
@@ -366,7 +366,7 @@ def plot_imu_bias_acc(self, imu_dfs):
         return fig
 
     def plot_imu_bias_omg(self, imu_dfs):
-        """TODO."""
+        # TODO(jhartzer): Add description
         fig, (axs_1, axs_2, axs_3) = plt.subplots(3, 1)
         a = calculate_alpha(len(imu_dfs))
         t_imu = np.zeros([len(imu_dfs), len(imu_dfs[0]['time'])])
@@ -402,7 +402,7 @@ def plot_imu_bias_omg(self, imu_dfs):
         return fig
 
     def plot_imu_cov(self, imu_dfs):
-        """TODO."""
+        # TODO(jhartzer): Add description
         fig, (axs_1, axs_2, axs_3, axs_4) = plt.subplots(4, 1)
         a = calculate_alpha(len(imu_dfs))
         for imu_df in imu_dfs:
@@ -434,7 +434,7 @@ def plot_imu_cov(self, imu_dfs):
     def plot_camera_body_pos_updates(self, cam_dfs):
         """Plot updates to the body position states from camera measurements."""
         fig, (axs_1, axs_2, axs_3) = plt.subplots(3, 1)
-        a = calculate_alpha(len(imu_dfs))
+        a = calculate_alpha(len(cam_dfs))
         for cam_df in cam_dfs:
             time = cam_df['time'].to_list()
             axs_1.plot(time, cam_df['body_update_0'].to_list(), alpha=a, color='tab:blue')
@@ -457,7 +457,7 @@ def plot_camera_body_pos_updates(self, cam_dfs):
     def plot_camera_body_ang_updates(self, cam_dfs):
         """Plot updates to the body angular states from camera measurements."""
         fig, (axs_1, axs_2, axs_3) = plt.subplots(3, 1)
-        a = calculate_alpha(len(imu_dfs))
+        a = calculate_alpha(len(cam_dfs))
         for cam_df in cam_dfs:
             time = cam_df['time'].to_list()
             axs_1.plot(time, cam_df['body_update_9'].to_list(), alpha=a, color='tab:blue')
@@ -480,7 +480,7 @@ def plot_camera_body_ang_updates(self, cam_dfs):
     def plot_camera_offset_updates(self, cam_dfs):
         """Plot camera updates to extrinsic offsets."""
         fig, (axs_1, axs_2) = plt.subplots(2, 1)
-        a = calculate_alpha(len(imu_dfs))
+        a = calculate_alpha(len(cam_dfs))
         for cam_df in cam_dfs:
             t_cam = cam_df['time'].to_list()
             axs_1.plot(t_cam, cam_df['cam_update_0'].to_list(), alpha=a, color='tab:blue')
@@ -499,7 +499,7 @@ def plot_camera_offset_updates(self, cam_dfs):
     def plot_camera_pos(self, cam_dfs):
         """Plot camera position offsets."""
         fig, (axs_1, axs_2, axs_3) = plt.subplots(3, 1)
-        a = calculate_alpha(len(imu_dfs))
+        a = calculate_alpha(len(cam_dfs))
         for cam_df in cam_dfs:
             t_cam = cam_df['time'].to_list()
             axs_1.plot(t_cam, cam_df['cam_state_0'].to_list(), alpha=a, color='tab:blue')
@@ -516,7 +516,7 @@ def plot_camera_pos(self, cam_dfs):
     def plot_camera_ang(self, cam_dfs):
         """Plot camera angular offsets."""
         fig, (axs_1, axs_2, axs_3) = plt.subplots(3, 1)
-        a = calculate_alpha(len(imu_dfs))
+        a = calculate_alpha(len(cam_dfs))
         for cam_df in cam_dfs:
             t_cam = cam_df['time'].to_list()
             axs_1.plot(t_cam, cam_df['cam_state_3'].to_list(), alpha=a, color='tab:blue')

src/application/sim/simulation.cpp

@@ -282,6 +282,14 @@ int main(int argc, char * argv[])
     cam_params.output_directory = out_dir;
     cam_params.data_logging_on = data_logging_on;
     cam_params.tracker = cam_node["tracker"].as<std::string>();
+    cam_params.intrinsics.F = cam_node["intrinsics"]["F"].as<double>(1.0);
+    cam_params.intrinsics.c_x = cam_node["intrinsics"]["c_x"].as<double>(0.0);
+    cam_params.intrinsics.c_y = cam_node["intrinsics"]["c_y"].as<double>(0.0);
+    cam_params.intrinsics.k_1 = cam_node["intrinsics"]["k_1"].as<double>(0.0);
+    cam_params.intrinsics.k_2 = cam_node["intrinsics"]["k_2"].as<double>(0.0);
+    cam_params.intrinsics.p_1 = cam_node["intrinsics"]["p_1"].as<double>(0.0);
+    cam_params.intrinsics.p_2 = cam_node["intrinsics"]["p_2"].as<double>(0.0);
+    cam_params.intrinsics.pixel_size = cam_node["intrinsics"]["pixel_size"].as<double>(1e-2);
 
     // SimCamera::Parameters
     SimCamera::Parameters sim_cam_params;
@@ -297,6 +305,7 @@ int main(int argc, char * argv[])
     auto cam = std::make_shared<SimCamera>(sim_cam_params, truth_engine);
     auto trk_params = trackerMap[cam_params.tracker];
     trk_params.tracker_params.sensor_id = cam->GetId();
+    trk_params.tracker_params.intrinsics = cam_params.intrinsics;
     auto trk = std::make_shared<SimFeatureTracker>(
       trk_params, truth_engine, out_dir, data_logging_on);
     cam->AddTracker(trk);

src/ekf/ekf.cpp

@@ -125,7 +125,7 @@ void EKF::ProcessModel(double time)
   m_cov.block<g_body_state_size, g_body_state_size>(0, 0) =
     F * (m_cov.block<g_body_state_size, g_body_state_size>(0, 0)) * F.transpose();
 
-  m_cov += m_process_noise;
+  AddProccessNoise();
 
   /// @todo(jhartzer): Refine this bounding
   m_cov = MaxBoundMatrix(m_cov, 1e2);
@@ -185,7 +185,7 @@ void EKF::PredictModel(
   Eigen::MatrixXd dF = GetStateTransition(dT);
   Eigen::MatrixXd F = Eigen::MatrixXd::Identity(g_body_state_size, g_body_state_size) + dF;
 
-  m_cov += m_process_noise;
+  AddProccessNoise();
 
   Eigen::MatrixXd process_noise = Eigen::MatrixXd::Zero(g_body_state_size, g_body_state_size);
   process_noise.block<3, 3>(6, 6) = acceleration_covariance_global;
@@ -200,6 +200,59 @@ void EKF::PredictModel(
   LogBodyStateIfNeeded();
 }
 
+/// @todo(jhartzer): Adjust process noise for offsets and biases
+void EKF::AddProccessNoise()
+{
+  m_cov.block<g_body_state_size, g_body_state_size>(0, 0) += m_process_noise;
+
+  for (auto const & imu_iter : m_state.m_imu_states) {
+    if (imu_iter.second.is_intrinsic && imu_iter.second.is_extrinsic) {
+      unsigned int imu_state_start = GetImuStateStartIndex(imu_iter.first);
+      Eigen::MatrixXd process_noise = Eigen::MatrixXd::Identity(12, 12);
+
+      process_noise.block<3, 3>(0, 0) *= imu_iter.second.pos_stability;
+      process_noise.block<3, 3>(3, 3) *= imu_iter.second.ang_stability;
+      process_noise.block<3, 3>(6, 6) *= imu_iter.second.acc_bias_stability;
+      process_noise.block<3, 3>(9, 9) *= imu_iter.second.omg_bias_stability;
+
+      m_cov.block<12, 12>(imu_state_start, imu_state_start) += process_noise;
+
+    } else if (imu_iter.second.is_intrinsic) {
+      unsigned int imu_state_start = GetImuStateStartIndex(imu_iter.first);
+      Eigen::MatrixXd process_noise = Eigen::MatrixXd::Identity(6, 6);
+
+      process_noise.block<3, 3>(0, 0) *= imu_iter.second.acc_bias_stability;
+      process_noise.block<3, 3>(3, 3) *= imu_iter.second.omg_bias_stability;
+
+      m_cov.block<6, 6>(imu_state_start, imu_state_start) += process_noise;
+
+    } else if (imu_iter.second.is_extrinsic) {
+      unsigned int imu_state_start = GetImuStateStartIndex(imu_iter.first);
+      Eigen::MatrixXd process_noise = Eigen::MatrixXd::Identity(6, 6);
+
+      process_noise.block<3, 3>(0, 0) *= imu_iter.second.pos_stability;
+      process_noise.block<3, 3>(3, 3) *= imu_iter.second.ang_stability;
+
+      m_cov.block<6, 6>(imu_state_start, imu_state_start) += process_noise;
+    }
+
+    /// @todo(jhartzer): Should this bounding be happening?
+    // m_cov.block<12, 12>(imu_state_start, imu_state_start) =
+    //   MinBoundDiagonal(m_cov.block<12, 12>(imu_state_start, imu_state_start), 5e-6);
+  }
+
+  for (auto const & cam_iter : m_state.m_cam_states) {
+
+    unsigned int cam_state_start = GetCamStateStartIndex(cam_iter.first);
+    Eigen::MatrixXd process_noise = Eigen::MatrixXd::Identity(6, 6);
+
+    process_noise.block<3, 3>(0, 0) *= cam_iter.second.pos_stability;
+    process_noise.block<3, 3>(3, 3) *= cam_iter.second.ang_stability;
+
+    m_cov.block<6, 6>(cam_state_start, cam_state_start) += process_noise;
+  }
+}
+
 State & EKF::GetState()
 {
   return m_state;
@@ -270,28 +323,6 @@ void EKF::RegisterIMU(unsigned int imu_id, ImuState imu_state, Eigen::MatrixXd c
   if (imu_state.is_extrinsic) {m_stateSize += g_imu_extrinsic_state_size;}
   if (imu_state.is_intrinsic) {m_stateSize += g_imu_intrinsic_state_size;}
 
-  if (imu_state.is_extrinsic && imu_state.is_intrinsic) {
-    Eigen::MatrixXd imu_process_noise = Eigen::MatrixXd::Identity(12, 12);
-    imu_process_noise.block<3, 3>(0, 0) *= imu_state.pos_stability;
-    imu_process_noise.block<3, 3>(3, 3) *= imu_state.ang_stability;
-    imu_process_noise.block<3, 3>(6, 6) *= imu_state.acc_bias_stability;
-    imu_process_noise.block<3, 3>(9, 9) *= imu_state.omg_bias_stability;
-    m_process_noise =
-      InsertInMatrix(imu_process_noise, m_process_noise, imu_state_start, imu_state_start);
-  } else if (imu_state.is_extrinsic) {
-    Eigen::MatrixXd imu_process_noise = Eigen::MatrixXd::Identity(6, 6);
-    imu_process_noise.block<3, 3>(0, 0) *= imu_state.pos_stability;
-    imu_process_noise.block<3, 3>(3, 3) *= imu_state.ang_stability;
-    m_process_noise =
-      InsertInMatrix(imu_process_noise, m_process_noise, imu_state_start, imu_state_start);
-  } else if (imu_state.is_intrinsic) {
-    Eigen::MatrixXd imu_process_noise = Eigen::MatrixXd::Identity(6, 6);
-    imu_process_noise.block<3, 3>(0, 0) *= imu_state.acc_bias_stability;
-    imu_process_noise.block<3, 3>(3, 3) *= imu_state.omg_bias_stability;
-    m_process_noise =
-      InsertInMatrix(imu_process_noise, m_process_noise, imu_state_start, imu_state_start);
-  }
-
   m_logger->Log(
     LogLevel::DEBUG, "Register IMU: " + std::to_string(
       imu_id) + ", stateSize: " + std::to_string(m_stateSize));
@@ -308,12 +339,6 @@ void EKF::RegisterCamera(unsigned int cam_id, CamState cam_state, Eigen::MatrixX
 
   m_state.m_cam_states[cam_id] = cam_state;
   m_cov = InsertInMatrix(covariance, m_cov, g_cam_state_size, g_cam_state_size);
-
-  Eigen::MatrixXd cam_process_noise = Eigen::MatrixXd::Identity(6, 6);
-  cam_process_noise.block<3, 3>(0, 0) *= cam_state.pos_stability;
-  cam_process_noise.block<3, 3>(3, 3) *= cam_state.ang_stability;
-  m_process_noise = InsertInMatrix(cam_process_noise, m_process_noise, m_stateSize, m_stateSize);
-
   m_stateSize += g_cam_state_size;
 
   m_logger->Log(

src/ekf/ekf.hpp

@@ -225,6 +225,8 @@ class EKF
   ///
   void SetDataLogging(bool value);
 
+  void AddProccessNoise();
+
   void SetProcessNoise(Eigen::VectorXd process_noise);
 
   DataLogger m_data_logger;  ///< @brief Data logger

src/ekf/types.cpp

@@ -86,12 +86,17 @@ State & operator+=(State & l_state, Eigen::VectorXd & r_vector)
   unsigned int n = 18;
   for (auto & imu_iter : l_state.m_imu_states) {
     unsigned int imu_id = imu_iter.first;
-    l_state.m_imu_states[imu_id].pos_i_in_b += r_vector.segment<3>(n + 0);
-    l_state.m_imu_states[imu_id].ang_i_to_b =
-      l_state.m_imu_states[imu_id].ang_i_to_b * RotVecToQuat(r_vector.segment<3>(n + 3));
-    l_state.m_imu_states[imu_id].acc_bias += r_vector.segment<3>(n + 6);
-    l_state.m_imu_states[imu_id].omg_bias += r_vector.segment<3>(n + 9);
-    n += 12;
+    if (imu_iter.second.is_extrinsic) {
+      l_state.m_imu_states[imu_id].pos_i_in_b += r_vector.segment<3>(n + 0);
+      l_state.m_imu_states[imu_id].ang_i_to_b =
+        l_state.m_imu_states[imu_id].ang_i_to_b * RotVecToQuat(r_vector.segment<3>(n + 3));
+      n += 6;
+    }
+    if (imu_iter.second.is_intrinsic) {
+      l_state.m_imu_states[imu_id].acc_bias += r_vector.segment<3>(n + 0);
+      l_state.m_imu_states[imu_id].omg_bias += r_vector.segment<3>(n + 3);
+      n += 6;
+    }
   }
 
   for (auto & cam_iter : l_state.m_cam_states) {
@@ -249,11 +254,16 @@ Eigen::VectorXd State::ToVector()
   unsigned int n = 18;
 
   for (auto const & imu_iter : m_imu_states) {
-    out_vec.segment<3>(n + 0) = imu_iter.second.pos_i_in_b;
-    out_vec.segment<3>(n + 3) = QuatToRotVec(imu_iter.second.ang_i_to_b);
-    out_vec.segment<3>(n + 6) = imu_iter.second.acc_bias;
-    out_vec.segment<3>(n + 9) = imu_iter.second.omg_bias;
-    n += 12;
+    if (imu_iter.second.is_extrinsic) {
+      out_vec.segment<3>(n + 0) = imu_iter.second.pos_i_in_b;
+      out_vec.segment<3>(n + 3) = QuatToRotVec(imu_iter.second.ang_i_to_b);
+      n += 6;
+    }
+    if (imu_iter.second.is_intrinsic) {
+      out_vec.segment<3>(n + 6) = imu_iter.second.acc_bias;
+      out_vec.segment<3>(n + 9) = imu_iter.second.omg_bias;
+      n += 6;
+    }
   }
 
   for (auto const & cam_iter : m_cam_states) {
@@ -276,7 +286,14 @@ Eigen::VectorXd State::ToVector()
 unsigned int State::GetStateSize()
 {
   unsigned int state_size = 18;
-  state_size += 12 * m_imu_states.size();
+
+  for (auto const & imu_iter : m_imu_states) {
+    if (imu_iter.second.is_extrinsic && imu_iter.second.is_intrinsic) {
+      state_size += 12;
+    } else if (imu_iter.second.is_extrinsic || imu_iter.second.is_intrinsic) {
+      state_size += 6;
+    }
+  }
 
   for (auto const & cam_iter : m_cam_states) {
     state_size += 6 + 12 * cam_iter.second.augmented_states.size();