Skip to content

Commit a97029c

Browse files
goldbattlegoldbattle
authored
Merge pull request #1 from rpng/feat_types
Types, State, and Propagator
2 parents 29ed63a + 9b7bc74 commit a97029c

39 files changed

+4735
-2215
lines changed

‎CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
cmake_minimum_required(VERSION 2.8.8)
22

33
add_subdirectory(ov_core)
4-
#add_subdirectory(ov_msckf)
4+
add_subdirectory(ov_msckf)
55

‎Doxyfile-mcss‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@ HTML_EXTRA_STYLESHEET = \
99
docs/css/custom.css
1010

1111

12+
1213
## No need to expose to-do list or bug list in public docs.
1314
GENERATE_TODOLIST = NO
1415
GENERATE_BUGLIST = NO
@@ -36,7 +37,7 @@ ALIASES += \
3637
##! M_FAVICON = favicon-dark.png
3738

3839

39-
##! M_LINKS_NAVBAR1 = pages annotated files
40+
##! M_LINKS_NAVBAR1 = pages namespaceov__core namespaceov__msckf annotated
4041
##! M_LINKS_NAVBAR2 = \
41-
##! "<a href=\"https://github.com/\">GitHub</a>"
42+
##! "<a href=\"https://github.com/rpng/open_vins/">GitHub</a>"
4243
##! M_SEARCH_DOWNLOAD_BINARY = NO

‎ov_core/src/cpi/CpiBase.h‎

Lines changed: 138 additions & 133 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
#ifndef CPI_BASE_H
22
#define CPI_BASE_H
3+
34
/*
45
* MIT License
56
* Copyright (c) 2018 Kevin Eckenhoff
@@ -29,145 +30,149 @@
2930
#include <Eigen/Dense>
3031
#include "utils/quat_ops.h"
3132

32-
3333
/**
34-
* @brief Base class for continuous preintegration integrators.
35-
*
36-
* This is the base class that both continuous-time preintegrators extend.
37-
* Please take a look at the derived classes CpiV1 and CpiV2 for the actual implementation.
38-
* Please see the following publication for details on the theory:
39-
* > Continuous Preintegration Theory for Graph-based Visual-Inertial Navigation
40-
* > Authors: Kevin Eckenhoff, Patrick Geneva, and Guoquan Huang
41-
* > http://udel.edu/~ghuang/papers/tr_cpi.pdf
42-
*
43-
* The steps to use this preintegration class are as follows:
44-
* 1. call setLinearizationPoints() to set the bias/orientation linearization point
45-
* 2. call feed_IMU() will all IMU measurements you want to precompound over
46-
* 3. access public varibles, to get means, Jacobians, and measurement covariance
34+
* @namespace ov_core
35+
* @brief Core algorithms for Open VINS
4736
*/
48-
class CpiBase {
49-
50-
public:
51-
37+
namespace ov_core {
5238

5339
/**
54-
* @brief Default constructor
55-
* @param sigma_w gyroscope white noise density (rad/s/sqrt(hz))
56-
* @param sigma_wb gyroscope random walk (rad/s^2/sqrt(hz))
57-
* @param sigma_a accelerometer white noise density (m/s^2/sqrt(hz))
58-
* @param sigma_ab accelerometer random walk (m/s^3/sqrt(hz))
59-
* @param imu_avg_ if we want to average the imu measurements (IJRR paper did not do this)
60-
*/
61-
CpiBase(double sigma_w, double sigma_wb, double sigma_a, double sigma_ab, bool imu_avg_= false) {
62-
// Calculate our covariance matrix
63-
Q_c.block(0,0,3,3) = std::pow(sigma_w,2)*eye3;
64-
Q_c.block(3,3,3,3) = std::pow(sigma_wb,2)*eye3;
65-
Q_c.block(6,6,3,3) = std::pow(sigma_a,2)*eye3;
66-
Q_c.block(9,9,3,3) = std::pow(sigma_ab,2)*eye3;
67-
imu_avg = imu_avg_;
68-
// Calculate our unit vectors, and their skews (used in bias jacobian calcs)
69-
e_1 << 1,0,0;
70-
e_2 << 0,1,0;
71-
e_3 << 0,0,1;
72-
e_1x = skew_x(e_1);
73-
e_2x = skew_x(e_2);
74-
e_3x = skew_x(e_3);
75-
}
76-
77-
78-
/**
79-
* @brief Set linearization points of the integration.
80-
* @param[in] b_w_lin_ gyroscope bias linearization point
81-
* @param[in] b_a_lin_ accelerometer bias linearization point
82-
* @param[in] q_k_lin_ orientation linearization point (only model 2 uses)
83-
* @param[in] grav_ global gravity at the current timestep
40+
* @brief Base class for continuous preintegration integrators.
8441
*
85-
* This function sets the linearization points we are to preintegrate about.
86-
* For model 2 we will also pass the q_GtoK and current gravity estimate.
87-
*/
88-
void setLinearizationPoints(Eigen::Matrix<double,3,1> b_w_lin_, Eigen::Matrix<double,3,1> b_a_lin_,
89-
Eigen::Matrix<double,4,1> q_k_lin_ = Eigen::Matrix<double,4,1>::Zero(),
90-
Eigen::Matrix<double,3,1> grav_ = Eigen::Matrix<double,3,1>::Zero()) {
91-
b_w_lin = b_w_lin_;
92-
b_a_lin = b_a_lin_;
93-
q_k_lin = q_k_lin_;
94-
grav = grav_;
95-
}
96-
97-
98-
/**
99-
* @brief Main function that will sequentially compute the preintegration measurement.
100-
* @param[in] t_0 first IMU timestamp
101-
* @param[in] t_1 second IMU timestamp
102-
* @param[in] w_m_0 first imu gyroscope measurement
103-
* @param[in] a_m_0 first imu acceleration measurement
104-
* @param[in] w_m_1 second imu gyroscope measurement
105-
* @param[in] a_m_1 second imu acceleration measurement
42+
* This is the base class that both continuous-time preintegrators extend.
43+
* Please take a look at the derived classes CpiV1 and CpiV2 for the actual implementation.
44+
* Please see the following publication for details on the theory:
45+
* > Continuous Preintegration Theory for Graph-based Visual-Inertial Navigation
46+
* > Authors: Kevin Eckenhoff, Patrick Geneva, and Guoquan Huang
47+
* > http://udel.edu/~ghuang/papers/tr_cpi.pdf
10648
*
107-
* This new IMU messages and will precompound our measurements, jacobians, and measurement covariance.
108-
* Please see both CpiV1 and CpiV2 classes for implementation details on how this works.
49+
* The steps to use this preintegration class are as follows:
50+
* 1. call setLinearizationPoints() to set the bias/orientation linearization point
51+
* 2. call feed_IMU() will all IMU measurements you want to precompound over
52+
* 3. access public varibles, to get means, Jacobians, and measurement covariance
10953
*/
110-
virtual void feed_IMU(double t_0, double t_1, Eigen::Matrix<double,3,1> w_m_0, Eigen::Matrix<double,3,1> a_m_0,
111-
Eigen::Matrix<double,3,1> w_m_1 = Eigen::Matrix<double,3,1>::Zero(),
112-
Eigen::Matrix<double,3,1> a_m_1 = Eigen::Matrix<double,3,1>::Zero()) = 0;
113-
114-
115-
116-
// Flag if we should perform IMU averaging or not
117-
// For version 1 we should average the measurement
118-
// For version 2 we average the local true
119-
bool imu_avg = false;
120-
121-
122-
// Measurement Means
123-
double DT = 0; ///< measurement integration time
124-
Eigen::Matrix<double,3,1> alpha_tau = Eigen::Matrix<double,3,1>::Zero(); ///< alpha measurement mean
125-
Eigen::Matrix<double,3,1> beta_tau = Eigen::Matrix<double,3,1>::Zero(); ///< beta measurement mean
126-
Eigen::Matrix<double,4,1> q_k2tau; ///< orientation measurement mean
127-
Eigen::Matrix<double,3,3> R_k2tau = Eigen::Matrix<double,3,3>::Identity(); ///< orientation measurement mean
128-
129-
// Jacobians
130-
Eigen::Matrix<double,3,3> J_q = Eigen::Matrix<double,3,3>::Zero(); ///< orientation Jacobian wrt b_w
131-
Eigen::Matrix<double,3,3> J_a = Eigen::Matrix<double,3,3>::Zero(); ///< alpha Jacobian wrt b_w
132-
Eigen::Matrix<double,3,3> J_b = Eigen::Matrix<double,3,3>::Zero(); ///< beta Jacobian wrt b_w
133-
Eigen::Matrix<double,3,3> H_a = Eigen::Matrix<double,3,3>::Zero(); ///< alpha Jacobian wrt b_a
134-
Eigen::Matrix<double,3,3> H_b = Eigen::Matrix<double,3,3>::Zero(); ///< beta Jacobian wrt b_a
135-
136-
// Linearization points
137-
Eigen::Matrix<double,3,1> b_w_lin; ///< b_w linearization point (gyroscope)
138-
Eigen::Matrix<double,3,1> b_a_lin; ///< b_a linearization point (accelerometer)
139-
Eigen::Matrix<double,4,1> q_k_lin; ///< q_k linearization point (only model 2 uses)
140-
141-
/// Global gravity
142-
Eigen::Matrix<double,3,1> grav = Eigen::Matrix<double,3,1>::Zero();
143-
144-
/// Our continous-time measurement noise matrix (computed from contructor noise values)
145-
Eigen::Matrix<double,12,12> Q_c = Eigen::Matrix<double,12,12>::Zero();
146-
147-
/// Our final measurement covariance
148-
Eigen::Matrix<double,15,15> P_meas = Eigen::Matrix<double,15,15>::Zero();
149-
150-
151-
//==========================================================================
152-
// HELPER VARIABLES
153-
//==========================================================================
154-
155-
// 3x3 identity matrix
156-
Eigen::Matrix<double,3,3> eye3 = Eigen::Matrix<double,3,3>::Identity();
157-
158-
// Simple unit vectors (used in bias jacobian calculations)
159-
Eigen::Matrix<double,3,1> e_1;// = Eigen::Matrix<double,3,1>::Constant(1,0,0);
160-
Eigen::Matrix<double,3,1> e_2;// = Eigen::Matrix<double,3,1>::Constant(0,1,0);
161-
Eigen::Matrix<double,3,1> e_3;// = Eigen::Matrix<double,3,1>::Constant(0,0,1);
162-
163-
// Calculate the skew-symetric of our unit vectors
164-
Eigen::Matrix<double,3,3> e_1x;// = skew_x(e_1);
165-
Eigen::Matrix<double,3,3> e_2x;// = skew_x(e_2);
166-
Eigen::Matrix<double,3,3> e_3x;// = skew_x(e_3);
167-
168-
169-
};
170-
171-
54+
class CpiBase {
55+
56+
public:
57+
58+
59+
/**
60+
* @brief Default constructor
61+
* @param sigma_w gyroscope white noise density (rad/s/sqrt(hz))
62+
* @param sigma_wb gyroscope random walk (rad/s^2/sqrt(hz))
63+
* @param sigma_a accelerometer white noise density (m/s^2/sqrt(hz))
64+
* @param sigma_ab accelerometer random walk (m/s^3/sqrt(hz))
65+
* @param imu_avg_ if we want to average the imu measurements (IJRR paper did not do this)
66+
*/
67+
CpiBase(double sigma_w, double sigma_wb, double sigma_a, double sigma_ab, bool imu_avg_ = false) {
68+
// Calculate our covariance matrix
69+
Q_c.block(0, 0, 3, 3) = std::pow(sigma_w, 2) * eye3;
70+
Q_c.block(3, 3, 3, 3) = std::pow(sigma_wb, 2) * eye3;
71+
Q_c.block(6, 6, 3, 3) = std::pow(sigma_a, 2) * eye3;
72+
Q_c.block(9, 9, 3, 3) = std::pow(sigma_ab, 2) * eye3;
73+
imu_avg = imu_avg_;
74+
// Calculate our unit vectors, and their skews (used in bias jacobian calcs)
75+
e_1 << 1, 0, 0;
76+
e_2 << 0, 1, 0;
77+
e_3 << 0, 0, 1;
78+
e_1x = skew_x(e_1);
79+
e_2x = skew_x(e_2);
80+
e_3x = skew_x(e_3);
81+
}
82+
83+
84+
/**
85+
* @brief Set linearization points of the integration.
86+
* @param[in] b_w_lin_ gyroscope bias linearization point
87+
* @param[in] b_a_lin_ accelerometer bias linearization point
88+
* @param[in] q_k_lin_ orientation linearization point (only model 2 uses)
89+
* @param[in] grav_ global gravity at the current timestep
90+
*
91+
* This function sets the linearization points we are to preintegrate about.
92+
* For model 2 we will also pass the q_GtoK and current gravity estimate.
93+
*/
94+
void setLinearizationPoints(Eigen::Matrix<double, 3, 1> b_w_lin_, Eigen::Matrix<double, 3, 1> b_a_lin_,
95+
Eigen::Matrix<double, 4, 1> q_k_lin_ = Eigen::Matrix<double, 4, 1>::Zero(),
96+
Eigen::Matrix<double, 3, 1> grav_ = Eigen::Matrix<double, 3, 1>::Zero()) {
97+
b_w_lin = b_w_lin_;
98+
b_a_lin = b_a_lin_;
99+
q_k_lin = q_k_lin_;
100+
grav = grav_;
101+
}
102+
103+
104+
/**
105+
* @brief Main function that will sequentially compute the preintegration measurement.
106+
* @param[in] t_0 first IMU timestamp
107+
* @param[in] t_1 second IMU timestamp
108+
* @param[in] w_m_0 first imu gyroscope measurement
109+
* @param[in] a_m_0 first imu acceleration measurement
110+
* @param[in] w_m_1 second imu gyroscope measurement
111+
* @param[in] a_m_1 second imu acceleration measurement
112+
*
113+
* This new IMU messages and will precompound our measurements, jacobians, and measurement covariance.
114+
* Please see both CpiV1 and CpiV2 classes for implementation details on how this works.
115+
*/
116+
virtual void feed_IMU(double t_0, double t_1, Eigen::Matrix<double, 3, 1> w_m_0, Eigen::Matrix<double, 3, 1> a_m_0,
117+
Eigen::Matrix<double, 3, 1> w_m_1 = Eigen::Matrix<double, 3, 1>::Zero(),
118+
Eigen::Matrix<double, 3, 1> a_m_1 = Eigen::Matrix<double, 3, 1>::Zero()) = 0;
119+
120+
121+
// Flag if we should perform IMU averaging or not
122+
// For version 1 we should average the measurement
123+
// For version 2 we average the local true
124+
bool imu_avg = false;
125+
126+
127+
// Measurement Means
128+
double DT = 0; ///< measurement integration time
129+
Eigen::Matrix<double, 3, 1> alpha_tau = Eigen::Matrix<double, 3, 1>::Zero(); ///< alpha measurement mean
130+
Eigen::Matrix<double, 3, 1> beta_tau = Eigen::Matrix<double, 3, 1>::Zero(); ///< beta measurement mean
131+
Eigen::Matrix<double, 4, 1> q_k2tau; ///< orientation measurement mean
132+
Eigen::Matrix<double, 3, 3> R_k2tau = Eigen::Matrix<double, 3, 3>::Identity(); ///< orientation measurement mean
133+
134+
// Jacobians
135+
Eigen::Matrix<double, 3, 3> J_q = Eigen::Matrix<double, 3, 3>::Zero(); ///< orientation Jacobian wrt b_w
136+
Eigen::Matrix<double, 3, 3> J_a = Eigen::Matrix<double, 3, 3>::Zero(); ///< alpha Jacobian wrt b_w
137+
Eigen::Matrix<double, 3, 3> J_b = Eigen::Matrix<double, 3, 3>::Zero(); ///< beta Jacobian wrt b_w
138+
Eigen::Matrix<double, 3, 3> H_a = Eigen::Matrix<double, 3, 3>::Zero(); ///< alpha Jacobian wrt b_a
139+
Eigen::Matrix<double, 3, 3> H_b = Eigen::Matrix<double, 3, 3>::Zero(); ///< beta Jacobian wrt b_a
140+
141+
// Linearization points
142+
Eigen::Matrix<double, 3, 1> b_w_lin; ///< b_w linearization point (gyroscope)
143+
Eigen::Matrix<double, 3, 1> b_a_lin; ///< b_a linearization point (accelerometer)
144+
Eigen::Matrix<double, 4, 1> q_k_lin; ///< q_k linearization point (only model 2 uses)
145+
146+
/// Global gravity
147+
Eigen::Matrix<double, 3, 1> grav = Eigen::Matrix<double, 3, 1>::Zero();
148+
149+
/// Our continous-time measurement noise matrix (computed from contructor noise values)
150+
Eigen::Matrix<double, 12, 12> Q_c = Eigen::Matrix<double, 12, 12>::Zero();
151+
152+
/// Our final measurement covariance
153+
Eigen::Matrix<double, 15, 15> P_meas = Eigen::Matrix<double, 15, 15>::Zero();
154+
155+
156+
//==========================================================================
157+
// HELPER VARIABLES
158+
//==========================================================================
159+
160+
// 3x3 identity matrix
161+
Eigen::Matrix<double, 3, 3> eye3 = Eigen::Matrix<double, 3, 3>::Identity();
162+
163+
// Simple unit vectors (used in bias jacobian calculations)
164+
Eigen::Matrix<double, 3, 1> e_1;// = Eigen::Matrix<double,3,1>::Constant(1,0,0);
165+
Eigen::Matrix<double, 3, 1> e_2;// = Eigen::Matrix<double,3,1>::Constant(0,1,0);
166+
Eigen::Matrix<double, 3, 1> e_3;// = Eigen::Matrix<double,3,1>::Constant(0,0,1);
167+
168+
// Calculate the skew-symetric of our unit vectors
169+
Eigen::Matrix<double, 3, 3> e_1x;// = skew_x(e_1);
170+
Eigen::Matrix<double, 3, 3> e_2x;// = skew_x(e_2);
171+
Eigen::Matrix<double, 3, 3> e_3x;// = skew_x(e_3);
172+
173+
174+
};
175+
176+
}
172177

173178
#endif /* CPI_BASE_H */

0 commit comments

Comments
 (0)