├── docs
    └── sympy_derivations
    │   ├── __init__.py
    │   ├── common_ops.py
    │   ├── skew_identities.py
    │   └── transition_matrix_analysis.py
├── executables
    └── main.cpp
├── CMakeLists.txt
├── .gitignore
├── LICENSE.txt
├── src
    ├── core.h
    ├── core.cpp
    ├── hamiltonian_msckf.cpp
    └── hamiltonian_msckf.h
└── README.md


/docs/sympy_derivations/__init__.py:
--------------------------------------------------------------------------------
1 | 


--------------------------------------------------------------------------------
/executables/main.cpp:
--------------------------------------------------------------------------------
 1 | 
 2 | 
 3 | 
 4 | #include "hamiltonian_msckf.h"
 5 | 
 6 | int main(int argc,char** argv){
 7 | 
 8 | 
 9 | 
10 |     return 0;
11 | }


--------------------------------------------------------------------------------
/docs/sympy_derivations/common_ops.py:
--------------------------------------------------------------------------------
 1 | import sympy as sp
 2 | 
 3 | 
 4 | def skew_matrix(o):
 5 |     return sp.Matrix([[0, -o[2], o[1]],
 6 |                          [o[2], 0, -o[0]],
 7 |                          [-o[1], o[0], 0]])
 8 | 
 9 | 
10 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.15)
 2 | project(hamiltonian_msckf)
 3 | 
 4 | set(CMAKE_CXX_STANDARD 14)
 5 | 
 6 | find_package(Eigen3)
 7 | include_directories(${EIGEN3_INCLUDE_DIR})
 8 | 
 9 | include_directories(src)
10 | 
11 | add_library(right_msckf
12 |         src/hamiltonian_msckf.h
13 |         src/hamiltonian_msckf.cpp
14 |         src/core.cpp src/core.h)
15 | 
16 | 
17 | 
18 | 
19 | add_executable(main executables/main.cpp)
20 | 
21 | target_link_libraries(main right_msckf)
22 | 


--------------------------------------------------------------------------------
/docs/sympy_derivations/skew_identities.py:
--------------------------------------------------------------------------------
 1 | from sympy import MatrixSymbol, Matrix,symbols,init_printing,pprint,simplify
 2 | 
 3 | 
 4 | def skew_matrix(o):
 5 |     return Matrix([[0, -o[2], o[1]],
 6 |                          [o[2], 0, -o[0]],
 7 |                          [-o[1], o[0], 0]])
 8 | 
 9 | 
10 | init_printing()
11 | 
12 | R = MatrixSymbol('R', 3, 3)
13 | R=Matrix(R)
14 | 
15 | 
16 | a1, a2, a3 = symbols('a1 a2 a3')
17 | a = Matrix([[a1, a2, a3]])
18 | a=a.transpose()
19 | 
20 | sola=R*skew_matrix(a)
21 | 
22 | bloesch=skew_matrix(R*a)
23 | 
24 | bloesch_expanded=R*skew_matrix(a)*R.transpose()
25 | 
26 | #pprint(simplify(bloesch_expanded))
27 | 
28 | pprint(simplify(bloesch-bloesch_expanded))
29 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | 
 2 | 
 3 | #Specific folders:
 4 | x86_64_windows_msvc/*
 5 | x86_64_linux_gnu/*
 6 | bin/*
 7 | build/*
 8 | cmake-build-debug/*
 9 | 
10 | 
11 | # C++ files
12 | *.a
13 | *.so
14 | *.so
15 | *.o
16 | 
17 | # python files
18 | *.pyc
19 | *.npz
20 | *.npy
21 | *.pytest_cache
22 | *.p
23 | 
24 | # Editor files
25 | 
26 | #QT
27 | *.files
28 | *.includes
29 | *.creator
30 | 
31 | # Visual studio
32 | *.sln
33 | *.VC.db
34 | scripts/\.vs/scripts/v15/\.suo
35 | \.vs/nl/v15/
36 | \.vs/
37 | 
38 | # CLion
39 | .idea
40 | .idea/*
41 | 
42 | 
43 | #Other filetypes
44 | *.xml
45 | *.iml
46 | *.orig
47 | *.jpg
48 | *.png
49 | *.autosave
50 | *.zip
51 | *.dir
52 | *.pyproj
53 | *.csv
54 | 
55 | 
56 | 
57 | 
58 | 
59 | 


--------------------------------------------------------------------------------
/LICENSE.txt:
--------------------------------------------------------------------------------
 1 | Copyright <2019> <Nikolaus Mitchell and Adrià Serra Moral>
 2 | 
 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 4 | following conditions are met:
 5 | 
 6 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 7 | disclaimer.
 8 | 
 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
10 | disclaimer in the documentation and/or other materials provided with the distribution.
11 | 
12 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products
13 | derived from this software without specific prior written permission.
14 | 
15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
16 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
17 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
18 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
19 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
20 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
21 | USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


--------------------------------------------------------------------------------
/docs/sympy_derivations/transition_matrix_analysis.py:
--------------------------------------------------------------------------------
 1 | from sympy import *
 2 | from common_ops import *
 3 | 
 4 | # Symbols for standard process(F) matrix derivations
 5 | 
 6 | skew_gyro=Symbol('w_x')
 7 | R=Symbol('R')
 8 | I=Symbol('I')
 9 | acc=Symbol('a')
10 | acc_skew=Symbol('a_x')
11 | tau=Symbol('tau')
12 | 
13 | # Equation for phi is
14 | # phi_dot=I + Integration(F*phi)
15 | 
16 | phi=MatrixSymbol('phi',6,6)
17 | phi=Matrix(phi)
18 | 
19 | initial_phi=Matrix(eye(6))
20 | initial_phi=initial_phi*I
21 | 
22 | 
23 | 
24 | F=Matrix(zeros(6))
25 | F[0,0]=skew_gyro
26 | F[0,1]=-I
27 | F[2,0]=R*acc_skew
28 | F[2,3]=-R
29 | F[4,2]=I
30 | 
31 | #Closed form equation
32 | #The transition matrix phi=e^(A*tau)*phi(0,0)
33 | #phi(0,0)=Identity
34 | 
35 | 
36 | # Dumb answer anything not Zero or Identity in this equation is what we have to analyze
37 | exp=simplify(exp(F*tau)*initial_phi)
38 | pprint(exp)
39 | 
40 | analyze=initial_phi+F*phi
41 | 
42 | #Set all the values to 0 that are zero in the exponent
43 | zero_set=[]
44 | for i in range(0,6):
45 |     for j in range(0,6):
46 |         if exp[i,j]==0:
47 |             zero_set.append((phi[i, j], 0))
48 |         #if exp[i,j]==I:
49 |          #   zero_set.append((phi[i, j], I))
50 | analyze=analyze.subs(zero_set)
51 | 
52 | 
53 | pprint(simplify(analyze))
54 | 
55 | #ans=F*phi
56 | #ans=simplify(ans)
57 | 
58 | 
59 | # zero_set=[]
60 | # for i in range(0,6):
61 | #     for j in range(0,6):
62 | #         # if i==j:
63 | #         #     continue
64 | #         # zero_set.append((phi[i,j],0))
65 | #         if ans[i,j]==0:
66 | #             zero_set.append((phi[i, j], 0))
67 | #
68 | #
69 | # ans=ans.subs(zero_set)
70 | # zero_set.clear()
71 | #
72 | # for i in range(0,6):
73 | #     for j in range(0,6):
74 | #         if i==j:
75 | #             continue
76 | #         zero_set.append((phi[i,j],0))
77 | #
78 | #
79 | # ans=ans.subs(zero_set)
80 | # pprint(simplify(exp(F*tau)*initial_phi))
81 | 
82 | 


--------------------------------------------------------------------------------
/src/core.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <Eigen/Geometry>
 4 | #include <vector>
 5 | #include <unordered_map>
 6 | 
 7 | namespace msckf {
 8 | 
 9 | using Vec3d=Eigen::Vector3d;
10 | using Vec2d=Eigen::Vector2d;
11 | using Vec4d=Eigen::Vector4d;
12 | using Vec6d=Eigen::Matrix<double, 6, 1>;
13 | using VecXd=Eigen::Matrix<double, Eigen::Dynamic, 1>;
14 | using Mat33d=Eigen::Matrix3d;
15 | using Mat44d=Eigen::Matrix4d;
16 | using Mat66d=Eigen::Matrix<double, 6, 6>;
17 | using MatXd=Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
18 | 
19 | using Mat15x15d=Eigen::Matrix<double, 15, 15>;
20 | using Mat15x12d=Eigen::Matrix<double, 15, 12>;
21 | 
22 | 
23 | using VectorVec3d=std::vector<Vec3d, Eigen::aligned_allocator<Vec3d>>;
24 | using VectorVec2d=std::vector<Vec2d, Eigen::aligned_allocator<Vec2d>>;
25 | 
26 | template<typename KeyType, typename ValueType>
27 | struct AlignedUnorderedMap {
28 |   typedef std::unordered_map<KeyType, ValueType,
29 |                              std::hash<KeyType>, std::equal_to<KeyType>,
30 |                              Eigen::aligned_allocator<std::pair<const KeyType, ValueType> > > type;
31 | };
32 | 
33 | const double deg2rad = M_PI / 180;
34 | 
35 | Mat33d CreateSkew(const Vec3d& input);
36 | 
37 | Eigen::Quaterniond RotVec2Quat(const Vec3d& rot_vec);
38 | 
39 | template<typename Scalar>
40 | constexpr Scalar Square(Scalar val) {
41 |   return val * val;
42 | }
43 | 
44 | /**
45 |  * Creates a 4x4 homogenous Transform
46 |  * @param quat
47 |  * @param trans
48 |  * @return
49 |  */
50 | Mat44d BuildTransform(const Eigen::Quaterniond& quat, const Vec3d& trans);
51 | 
52 | /**
53 |  * Inverts a homogenous transform efficiently. And is numerically safer
54 |  */
55 | Mat44d InvertTransform(const Mat44d& transform);
56 | 
57 | /**
58 |  * Equation 198 from Sola
59 |  * @param gyro_meas
60 |  * @return
61 |  */
62 | Mat44d Omega(const Vec3d& gyro_meas);
63 | 
64 | /**
65 |  * Creates Omega for Eigen::Quaternion which is x,y,z,w
66 |  */
67 | Mat44d OmegaEigenQuat(const Vec3d& gyro_meas);
68 | 
69 | }
70 | 


--------------------------------------------------------------------------------
/src/core.cpp:
--------------------------------------------------------------------------------
 1 | #include "core.h"
 2 | 
 3 | namespace msckf{
 4 | 
 5 | Eigen::Quaterniond RotVec2Quat(const Vec3d &rot_vec) {
 6 |   double theta = rot_vec.norm();
 7 |   if (theta < 1e-10) {
 8 |     return Eigen::Quaterniond::Identity();
 9 |   }
10 | 
11 |   Eigen::Quaterniond quat;
12 |   quat.w() = cos(theta / 2);
13 |   double scale = sin(theta / 2);
14 |   Vec3d copy = rot_vec / theta;
15 |   quat.x() = scale * copy[0];
16 |   quat.y() = scale * copy[1];
17 |   quat.z() = scale * copy[2];
18 |   return quat;
19 | 
20 | }
21 | Mat33d CreateSkew(const Vec3d &input) {
22 |   Mat33d output = Mat33d::Zero();
23 |   output(0, 0) = 0;
24 |   output(0, 1) = -input(2);
25 |   output(0, 2) = input(1);
26 |   output(1, 0) = input(2);
27 |   output(1, 1) = 0;
28 |   output(1, 2) = -input(0);
29 |   output(2, 0) = -input(1);
30 |   output(2, 1) = input(0);
31 |   output(2, 2) = 0;
32 |   return output;
33 | }
34 | Mat44d BuildTransform(const Eigen::Quaterniond &quat, const Vec3d &trans) {
35 |   Mat44d T=Mat44d::Identity();
36 |   T.block<3,3>(0,0)=quat.toRotationMatrix();
37 |   T.block<3,1>(0,3)=trans;
38 |   return T;
39 | 
40 | }
41 | Mat44d InvertTransform(const Mat44d &transform) {
42 |   Mat44d Tinv = Mat44d::Identity();
43 |   Tinv.block<3,3>(0,0) = transform.block<3,3>(0,0).transpose();
44 |   Tinv.block<3,1>(0,3) = -Tinv.block<3,3>(0,0)*transform.block<3,1>(0,3);
45 |   return Tinv;
46 | }
47 | Mat44d Omega(const Vec3d& gyro_meas) {
48 |   Mat44d omega;
49 |   double wx = gyro_meas(0);
50 |   double wy = gyro_meas(1);
51 |   double wz = gyro_meas(2);
52 |   omega <<
53 |         0, -wx, -wy, -wz,
54 |       wx, 0, wz, -wy,
55 |       wy, -wz, 0, wx,
56 |       wz, wy, -wx, 0;
57 |   return omega;
58 | 
59 | }
60 | 
61 | Mat44d OmegaEigenQuat(const Vec3d& gyro_meas) {
62 |   Mat44d omega;
63 |   double wx = gyro_meas(0);
64 |   double wy = gyro_meas(1);
65 |   double wz = gyro_meas(2);
66 |   omega <<
67 |         -wx, -wy, -wz, 0,
68 |       0, wz, -wy, wx,
69 |       -wz, 0, wx, wy,
70 |       wy, -wx, 0, wz;
71 |   return omega;
72 | 
73 | }
74 | 
75 | }


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # RIGHT MSCKF
 2 | 
 3 | This implements the Right MSCKF called so cause it is the right way to implement
 4 |  it with proper Hamiltonian Quaternions. Also a nice play on words as Hamiltonian Quaternions are also in the right handed
 5 |  coordinate system hence the name.
 6 |  
 7 | Main purpose is to serve as a reference for people that want to implement a MSCKF with Hamiltonian style quaternions, and not
 8 |  have to resort to using JPL style.
 9 |  
10 |  Most of the derivations are taken from [2] a fantastic resource for ESKF derivation with hamiltonian style
11 |  quaternions.
12 |  
13 |  
14 |  
15 |  ### Rant
16 |  The original MSCKF was first published in [1]. It is a fantastic piece of work, the only
17 |   downside being that Stergios and his collaborators(Mostly University of Minnesota students/alumni) use JPL style quaternions, in contrast
18 |   to much of the robotics, math, and physics literature. Sadly everyone that decides to implement
19 |   a MSCKF directly pulls their equations from the original source, or one of its derivatives written
20 |   by someone associated with Stergios. This has resulted in a large number of JPL style quaternions, which I am not a
21 |    fan of. I very much dislike this existence of two styles/coordinate systems. I believe it brings confusion, and harms
22 |    collaboration efforts, especially when there isn't really a benefit. For more about the existence of these two styles and their history I
23 |    recommend [4].
24 |  
25 | ### Other comments
26 |  
27 |  - Note that there is some evidence in [3] that the JPL style aka Global error performs better. 
28 |  
29 | ### TODO
30 |  
31 |  
32 |  - [ ] Finish all parts of MSCKF
33 |  - [ ] Create pdf with derivations
34 |  - [ ] Create Visual Inertial Simulator
35 |  
36 |  
37 |  
38 |  ## References
39 | 
40 |     1)
41 |     @article{Mourikis2007a,
42 |       author = {Mourikis, Anastasios I. and Roumeliotis, Stergios I.},
43 |       title = {{A multi-state constraint Kalman filter for vision-aided inertial navigation}},
44 |       year = {2007}
45 |       }
46 | 
47 |     2)
48 |     @techreport{JoanSola2012,
49 |     author = {{Joan Sol{\'{a}}}},
50 |     title = {{Quaternion Kinematics for Error-State KF}},
51 |     year = {2012}
52 |     }
53 |     
54 |     3)
55 |     @article{Li2012,
56 |     author = {Li, Mingyang and Mourikis, Anastasios I.},
57 |     title = {{Improving the accuracy of EKF-based visual-inertial odometry}},
58 |     year = {2012}
59 |     }
60 |     
61 |     4)
62 |     @article{Sommer2018,
63 |     author = {Sommer, Hannes and Gilitschenski, Igor and Bloesch, Michael and Weiss, Stephan and Siegwart, Roland and Nieto, Juan},
64 |     title = {{Why and how to avoid the flipped quaternion multiplication}},
65 |     year = {2018}
66 |     }
67 | 
68 | 
69 | 


--------------------------------------------------------------------------------
/src/hamiltonian_msckf.cpp:
--------------------------------------------------------------------------------
  1 | //
  2 | // Created by niko on 11/26/19.
  3 | //
  4 | 
  5 | #include "hamiltonian_msckf.h"
  6 | 
  7 | namespace msckf{
  8 | 
  9 | State::State() {
 10 | 
 11 | }
 12 | State::State(int num_cam_clones) : num_cam_clones(num_cam_clones) {
 13 | 
 14 |   int state_size_clones = 7 * num_cam_clones;
 15 |   int state_size = state_size_clones + 13;
 16 | 
 17 |   state_vec.resize(state_size);
 18 |   state_vec.setZero();
 19 | 
 20 |   state_vec[3] = 1.0; //quat set to identity
 21 | }
 22 | Eigen::Quaterniond State::GetQuat() {
 23 |   return Eigen::Quaterniond(state_vec.data() + quat_idx);
 24 | }
 25 | Vec3d State::GetGyroBias() {
 26 |   return state_vec.segment(bg_idx, 3);
 27 | }
 28 | Vec3d State::GetAccelBias() {
 29 |   return state_vec.segment(ba_idx, 3);
 30 | }
 31 | Vec3d State::GetPos() {
 32 |   return state_vec.segment(pos_idx, 3);
 33 | }
 34 | Vec3d State::GetVel() {
 35 |   return state_vec.segment(vel_idx, 3);
 36 | }
 37 | Eigen::Quaterniond State::GetCloneQuat(int n_clone) {
 38 |   assert(n_clone <= num_cam_clones);
 39 | 
 40 |   return Eigen::Quaterniond(state_vec.data() + clone_start_idx + 7 * n_clone);
 41 | }
 42 | Vec3d State::GetClonePos(int n_clone) {
 43 |   assert(n_clone <= num_cam_clones);
 44 | 
 45 |   return state_vec.segment(clone_start_idx + 7 * n_clone + 4, 3);
 46 | }
 47 | 
 48 | 
 49 | 
 50 | 
 51 | Vec2d ReprojectionError(const Eigen::Isometry3d &pose,
 52 |                         const Vec3d &pt_msckf,
 53 |                         const Vec2d &proj) {
 54 |   const double alpha = pt_msckf(0);
 55 |   const double beta = pt_msckf(1);
 56 |   const double rho = pt_msckf(2);
 57 | 
 58 |   Vec3d h = pose.linear() * Vec3d(alpha, beta, 1.0) + rho * pose.translation();
 59 | 
 60 |   Vec2d z_hat(h[0] / h[2], h[1] / h[2]);
 61 |   return (z_hat - proj);
 62 | }
 63 | 
 64 | void FeatureOptJacobian(const Eigen::Isometry3d &T_c0_ci, const Vec3d &x,
 65 |                         const Vec2d &z, Eigen::Matrix<double,2,3> &J) {
 66 | 
 67 |   // Compute hi1, hi2, and hi3 as Equation (37).
 68 |   const double &alpha = x(0);
 69 |   const double &beta = x(1);
 70 |   const double &rho = x(2);
 71 | 
 72 |   Vec3d h =
 73 |       T_c0_ci.linear() * Vec3d(alpha, beta, 1.0) + rho * T_c0_ci.translation();
 74 |   double &h1 = h(0);
 75 |   double &h2 = h(1);
 76 |   double &h3 = h(2);
 77 | 
 78 |   // Compute the Jacobian.
 79 |   Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
 80 |   W.leftCols<2>() = T_c0_ci.linear().leftCols<2>();
 81 |   W.rightCols<1>() = T_c0_ci.translation();
 82 | 
 83 |   J = MatXd::Zero(2, 3);
 84 |   J.row(0) = 1 / h3 * W.row(0) - h1 / (h3 * h3) * W.row(2);
 85 |   J.row(1) = 1 / h3 * W.row(1) - h2 / (h3 * h3) * W.row(2);
 86 | 
 87 | //  // Compute the residual.
 88 | //  Vec2d z_hat(h1 / h3, h2 / h3);
 89 | //  r = z_hat - z;
 90 | 
 91 | 
 92 | //  // Compute the weight based on the residual.
 93 | //  double e             = r.norm();
 94 | //  double huber_epsilon = 0.01;
 95 | //  if (e <= huber_epsilon) {
 96 | //    w = 1.0;
 97 | //  } else {
 98 | //    w = huber_epsilon / (2 * e);
 99 | //  }
100 | }
101 | 
102 | bool TriangulateNViewETH(const std::vector<Vec2d,Eigen::aligned_allocator<Vec2d>>& projs,
103 |     const std::vector<Eigen::Isometry3d,Eigen::aligned_allocator<Eigen::Isometry3d>>& cam_pos, Vec3d& final_pos) {
104 | 
105 |   int                                      num_projs = projs.size();
106 |   Eigen::Matrix<double, Eigen::Dynamic, 3> A;
107 |   Eigen::VectorXd                          b;
108 |   A.resize(2 * num_projs, Eigen::NoChange);
109 |   b.resize(2 * num_projs);
110 | 
111 |   for (int i = 0; i < num_projs; ++i) {
112 |     Eigen::Isometry3d C_T_G = cam_pos[i].inverse();
113 |     Eigen::Matrix3d   C_R_G = C_T_G.rotation();
114 |     const Eigen::Vector3d& C_p_G = C_T_G.translation();
115 | 
116 |     A.row(2 * i + 0) = projs[i](0) * C_R_G.row(2) - C_R_G.row(0);
117 |     A.row(2 * i + 1) = projs[i](1) * C_R_G.row(2) - C_R_G.row(1);
118 | 
119 |     b(2 * i + 0) = -(projs[i](0) * C_p_G(2) - C_p_G(0));
120 |     b(2 * i + 1) = -(projs[i](1) * C_p_G(2) - C_p_G(1));
121 |   }
122 | 
123 |   Eigen::JacobiSVD<Eigen::MatrixXd> svd(
124 |       A, Eigen::ComputeThinU | Eigen::ComputeThinV);
125 |   final_pos = svd.solve(b);
126 | 
127 |   Eigen::MatrixXd singularValues;
128 |   singularValues.resize(svd.singularValues().rows(), 1);
129 |   singularValues = svd.singularValues();
130 |   double condA = singularValues(0, 0) / singularValues(singularValues.rows() - 1, 0);
131 |   if (std::abs(condA) > 1000) {
132 |     return false;
133 |   }
134 | 
135 |   return true;
136 | }
137 | 
138 | bool TriangulateNViewRefine(const std::vector<Vec2d,Eigen::aligned_allocator<Vec2d>>& projs,
139 |                          const std::vector<Eigen::Isometry3d,Eigen::aligned_allocator<Eigen::Isometry3d>>& cam_pos, Vec3d& pos) {
140 | 
141 | 
142 |   return true;
143 | }
144 | 
145 | 
146 | bool HamiltonianMSCKF::TriangulateFeature(Landmark_To_Residualize& track) {
147 | 
148 |   //Find cameras with the feature id
149 | 
150 |   std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>
151 |       camera_poses;
152 | 
153 |   std::vector<Vec2d,Eigen::aligned_allocator<Vec2d>> valid_projs;
154 | 
155 | 
156 |   auto& cam_states=track.connected_cams;
157 | 
158 |   for(int idx=0; idx<track.landmark.projs.size(); ++idx){
159 |     auto& l=track.landmark;
160 |     int64_t cam_id=l.camera_ids[idx];
161 |     if(cam_states.find(cam_id)!=cam_states.end()){
162 |       valid_projs.push_back(l.projs[idx]);
163 |       //Convert Quat,pos to 4x4 rigid body Transform -> Eigen::Isometry3d
164 |       Mat44d pose;
165 |       pose.block<3,3>(0,0)=cam_states[cam_id].quat.toRotationMatrix();
166 |       pose.block<3,1>(0,3)=cam_states[cam_id].pos;
167 |       camera_poses.emplace_back(Eigen::Isometry3d(pose));
168 |     }
169 |   }
170 | 
171 |   //Convert the poses so that they are relative to the first pose so that
172 |   // pose[0] is the identity matrix
173 |   Eigen::Isometry3d T_c0_w = camera_poses[0];
174 |   for (auto &pose : camera_poses) {
175 |     pose = pose.inverse() * T_c0_w;
176 |   }
177 | 
178 |   //First use SVD triangulation to get initial guess
179 |   Vec3d initial_guess;
180 |   bool valid=TriangulateNViewETH(valid_projs,camera_poses,initial_guess);
181 |   if(!valid){
182 |     return false;
183 |   }
184 | 
185 | 
186 |   //Now refine triangulation with Gauss Newton
187 | 
188 |   valid =TriangulateNViewRefine(valid_projs,camera_poses,initial_guess);
189 | 
190 |   track.landmark.pos_W=initial_guess;
191 |   track.landmark.have_pos=valid;
192 |   return valid;
193 | 
194 | 
195 | 
196 | 
197 | 
198 | 
199 | 
200 | }
201 | }


--------------------------------------------------------------------------------
/src/hamiltonian_msckf.h:
--------------------------------------------------------------------------------
  1 | 
  2 | 
  3 | #pragma once
  4 | 
  5 | #include <Eigen/Geometry>
  6 | #include <unordered_map>
  7 | #include <unordered_set>
  8 | #include <vector>
  9 | #include <list>
 10 | #include <deque>
 11 | 
 12 | #include "core.h"
 13 | 
 14 | namespace msckf {
 15 | 
 16 | struct IMUMeas {
 17 |   Vec3d acc;
 18 |   Vec3d gyro;
 19 | };
 20 | 
 21 | struct Feature2D {
 22 |   int64_t id = 0;
 23 | 
 24 |   Vec2d norm_coord; //img coordinates in normalized form
 25 | };
 26 | 
 27 | struct Landmark {
 28 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 29 | 
 30 |   int64_t id = 0; // same as Feature
 31 |   std::vector<Vec2d, Eigen::aligned_allocator<Vec2d>> projs;
 32 |   std::vector<int64_t> camera_ids;
 33 | 
 34 |   int64_t cam_id_first;
 35 |   int64_t cam_id_last;
 36 | 
 37 |   int GetNumTracks() {
 38 |     return projs.size();
 39 |   }
 40 | 
 41 |   Vec3d pos_W; //Position in World
 42 |   bool have_pos = false;
 43 | 
 44 | };
 45 | 
 46 | struct CamState {
 47 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 48 | 
 49 |   int64_t cam_id = -1;
 50 |   double time = 0;
 51 |   std::vector<int64_t> landmark_ids;
 52 | 
 53 |   Vec3d pos;
 54 |   Eigen::Quaterniond quat;
 55 | 
 56 | };
 57 | 
 58 | struct Landmark_To_Residualize {
 59 |   Landmark landmark;
 60 |   std::unordered_map<int64_t, CamState> connected_cams;
 61 | };
 62 | 
 63 | struct State {
 64 | 
 65 |   int num_cam_clones;
 66 | 
 67 |   // order follows MSCKF paper
 68 |   // quat,b_g,vel,b_a,pos,clone1(quat,pos),clone2(quat,pos),...,cloneN(quat,pos)
 69 |   VecXd state_vec;
 70 | 
 71 |   MatXd imu_covar; //covariance of quat-imu_pos
 72 |   MatXd cams_covar;
 73 | 
 74 |   MatXd imu_cam_covar;
 75 | 
 76 |   MatXd P;
 77 | 
 78 |   std::deque<CamState> cam_states;
 79 | 
 80 |   //state indexes to get values
 81 |   const int quat_idx = 0;
 82 |   const int bg_idx = 4;
 83 |   const int vel_idx = 7;
 84 |   const int ba_idx = 10;
 85 |   const int pos_idx = 13;
 86 |   const int clone_start_idx = 16;
 87 | 
 88 |   //indexes in error state. Almost exact same except the quaternion error state is of size 3
 89 |   const int quat_error_idx = 0;
 90 |   const int bg_error_idx = 3;
 91 |   const int vel_error_idx = 6;
 92 |   const int ba_error_idx = 9;
 93 |   const int pos_error_idx = 12;
 94 | 
 95 |   State();
 96 | 
 97 |   State(int num_cam_clones);
 98 | 
 99 |   Eigen::Quaterniond GetQuat();
100 | 
101 |   Vec3d GetGyroBias();
102 | 
103 |   Vec3d GetAccelBias();
104 | 
105 |   Vec3d GetPos();
106 | 
107 |   Vec3d GetVel();
108 | 
109 |   Eigen::Quaterniond GetCloneQuat(int n_clone);
110 | 
111 |   Vec3d GetClonePos(int n_clone);
112 | 
113 | };
114 | 
115 | class HamiltonianMSCKF {
116 | public:
117 | 
118 |   State state;
119 | 
120 |   int64_t latest_cam_id;
121 | 
122 |   //Camera offsets
123 |   Eigen::Quaterniond q_IMU_C;
124 |   Vec3d p_IMU_C;
125 | 
126 |   int max_track_length = 10;
127 | 
128 |   double total_time = 0;
129 | 
130 |   // ---------------------------- IMU ---------------------------------------------------
131 |   double sigma_gyr_2 = Square(1e-4);
132 |   double sigma_acc_2 = Square(1e-2);
133 |   double sigma_bg_drift_2 = Square(3.6733e-5);
134 |   double sigma_ba_drift_2 = Square(7e-4);
135 |   double tau_bg{50.0};
136 |   double tau_ba{100.0};
137 | 
138 |   // Initial Uncertainty
139 |   double sigma_pos_init{1e-12};
140 |   double sigma_vel_init{1e-6};
141 |   double sigma_att_init{1e-6 * deg2rad};
142 |   double sigma_ba_init{1e-6};
143 |   double sigma_bg_init{3.6733e-5};
144 | 
145 |   double tau_acc_bias{0};
146 |   double tau_gyr_bias{0};
147 |   Vec3d pos_imu_b_B{Vec3d::Zero()};
148 | 
149 |   VecXd noise_vec;
150 | 
151 |   // Matrices
152 |   Eigen::Matrix<double, 12, 12> R_imu{Eigen::Matrix<double, 12, 12>::Identity()};
153 | 
154 |   // ---------------------------- VO and LCVO ---------------------------------------------
155 |   double sigma_vo_feature_u{0.01};  //FIXME get this value
156 |   double sigma_vo_feature_v{0.01};  //FIXME get this value
157 |   double sigma_lcvo_pos{0.05};
158 |   double sigma_lcvo_att{2 * deg2rad};
159 | 
160 |   // Adaptation in MSC-EKF
161 |   double sigma_vo_fu_ad{0.01};
162 |   double sigma_vo_fv_ad{0.01};
163 |   VecXd error_vo_pred{VecXd::Zero(2, 1)};
164 | 
165 |   bool use_observability_augmentation = false;
166 | 
167 | 
168 |   //================================ MSC EKF Necessary Matrices ==========================================================
169 | 
170 |   //------------ Propagate --------------------------
171 | 
172 |   Mat15x15d F_k;
173 | 
174 |   Mat15x12d G_k;
175 | 
176 |   Mat15x15d Phi_k;
177 | 
178 |   Mat15x15d Q_k;
179 | 
180 |   Mat15x15d Phi_k_propagated;
181 | 
182 |   //--------------- Update --------------------------
183 | 
184 |   MatXd S_k;
185 | 
186 |   MatXd H_k;
187 | 
188 |   MatXd R_k;
189 | 
190 |   MatXd K_k;
191 | 
192 |   VecXd residual;
193 | 
194 |   VecXd delta_state;
195 | 
196 |   std::unordered_map<int64_t, Landmark> map_id_landmark;
197 | 
198 |   std::unordered_set<int64_t> feature_ids_to_remove;
199 | 
200 |   double noise_pixel_sigma;
201 | 
202 |   void Propogate(const IMUMeas& meas, double dt) {
203 | 
204 |     Vec3d acc_meas = meas.acc;
205 |     Vec3d gyro_meas = meas.gyro;
206 | 
207 |     Vec3d unbiased_accel_B = acc_meas - state.GetAccelBias();
208 |     Vec3d unbiased_gyro_B = gyro_meas - state.GetGyroBias();
209 | 
210 |     Mat33d rot_matrix = state.GetQuat().toRotationMatrix();
211 | 
212 | 
213 | 
214 | 
215 | 
216 | 
217 |     //RK4
218 | 
219 | 
220 | 
221 | 
222 | 
223 | 
224 | 
225 |   }
226 | 
227 |   Vec3d grav;
228 | 
229 |   void Update();
230 | 
231 |   void PropogateStateRK4(State& x, Vec3d& unbiased_acc, Vec3d& unbiased_gyro, Vec3d& gravity, double dt) {
232 | 
233 | 
234 |     // Current state
235 |     Eigen::Quaterniond q0 = state.GetQuat();
236 |     Vec3d pos0 = state.GetPos();
237 |     Vec3d vel0 = state.GetVel();
238 |     Mat33d Rwb0 = q0.toRotationMatrix();
239 | 
240 |     //RK4
241 | 
242 |     //q_dot=0.5*q*w=0.5*omega(w)*q
243 |     // v_dot=R*(acc_m-acc_b)+gravity
244 |     // p_dot=v
245 | 
246 |     //k1
247 |     Vec4d q1_dot = 0.5 * OmegaEigenQuat(unbiased_gyro) * q0.coeffs();
248 |     Vec3d p1_dot = vel0;
249 |     Vec3d v1_dot = Rwb0 * (unbiased_acc) + gravity;
250 | 
251 |     Vec4d q_k1 = q1_dot * dt;
252 |     Vec3d p_k1 = p1_dot * dt;
253 |     Vec3d v_k1 = v1_dot * dt;
254 | 
255 |     Vec4d q1 = q0.coeffs() + q_k1 * 0.5;
256 |     Vec3d v1 = vel0 + v_k1 * 0.5;
257 | //
258 |     Eigen::Quaterniond q1_eig(q1.data());
259 |     q1_eig.normalize();
260 | //
261 | //    //k2
262 |     Vec4d q2_dot = 0.5 * OmegaEigenQuat(unbiased_gyro) * q1_eig.coeffs();
263 |     Vec3d p2_dot = v1;
264 |     Vec3d v2_dot = q1_eig.toRotationMatrix() * unbiased_acc + gravity;
265 | 
266 |     Vec4d q_k2 = q2_dot * dt;
267 |     Vec3d p_k2 = p2_dot * dt;
268 |     Vec3d v_k2 = v2_dot * dt;
269 | 
270 |     Vec4d q2 = q0.coeffs() + q_k2 * 0.5;
271 |     Vec3d v2 = vel0 + v_k2 * 0.5;
272 | //
273 |     Eigen::Quaterniond q2_eig(q2.data());
274 |     q2_eig.normalize();
275 |     //k3
276 |     Vec4d q3_dot = 0.5 * OmegaEigenQuat(unbiased_gyro) * q2_eig.coeffs();
277 |     Vec3d p3_dot = v2;
278 |     Vec3d v3_dot = q2_eig.toRotationMatrix() * unbiased_acc + gravity;
279 | 
280 |     Vec4d q_k3 = q3_dot * dt;
281 |     Vec3d p_k3 = p3_dot * dt;
282 |     Vec3d v_k3 = v3_dot * dt;
283 | 
284 |     Vec4d q3 = q0.coeffs() + q_k3;
285 |     Vec3d v3 = vel0 + v_k3;
286 | 
287 |     Eigen::Quaterniond q3_eig(q3.data());
288 |     q3_eig.normalize();
289 | 
290 |     Vec4d q4_dot = 0.5 * OmegaEigenQuat(unbiased_gyro) * q3_eig.coeffs();
291 |     Vec3d p4_dot = v3;
292 |     Vec3d v4_dot = q3_eig.toRotationMatrix() * unbiased_acc + gravity;
293 | 
294 |     Vec4d q_k4 = q4_dot * dt;
295 |     Vec3d p_k4 = p4_dot * dt;
296 |     Vec3d v_k4 = v4_dot * dt;
297 | 
298 | 
299 |     // Sum
300 | 
301 |     Vec3d p = pos0 + (1 / 6.0) * (p_k1 + 2 * p_k2 + 2 * p_k3 + p_k4);
302 |     Vec3d v = vel0 + (1 / 6.0) * (v_k1 + 2 * v_k2 + 2 * v_k3 + v_k4);
303 |     Vec4d q = q0.coeffs() + (1 / 6.0) * (q_k1 + 2 * q_k2 + 2 * q_k3 + q_k4);
304 |     Eigen::Quaterniond q_eig(q.data());
305 |   }
306 | 
307 |   void
308 |   CalcIMUDerivatives(State& x, Vec3d& pos_dx, Vec3d& vel_dx, Eigen::Quaterniond& quat_dx, Vec3d& unbiased_acc,
309 |                      Vec3d& unbiased_gyro) {
310 |     // From equations 236 in Sola Page 54
311 | 
312 |     pos_dx = x.GetVel();
313 |     vel_dx = x.GetQuat().toRotationMatrix() * (unbiased_acc) + grav;
314 |     //quat_dx=x.GetQuat()*unbiased_gyro*0.5;
315 | 
316 | 
317 | 
318 | 
319 | 
320 | //    Vec3 gyro = gyro_accel.head<3>();
321 | //    Vec3 accel = gyro_accel.tail<3>();
322 | //
323 | //    Vec3 gyro_calib = imu_.Cg() * gyro - X.bg;   // \hat\omega in the doc
324 | //    Vec3 accel_calib = imu_.Ca() * accel - X.ba; // \hat\alpha in the doc
325 | //
326 | //    // jacobian w.r.t. error state
327 | //    Mat3 R = X.Rsb.matrix();
328 | //
329 | //    Eigen::Matrix<number_t, 3, 9> dW_dCg;
330 | //    for (int i = 0; i < 3; ++i) {
331 | //      // NOTE: use the raw measurement (gyro) here. NOT the calibrated one
332 | //      // (gyro_calib)!!!
333 | //      dW_dCg.block<1, 3>(i, 3 * i) = gyro;
334 | //    }
335 | //
336 | //    Eigen::Matrix<number_t, 3, 9> dV_dRCa = dAB_dA<3, 3>(accel);
337 | //    Eigen::Matrix<number_t, 9, 9> dRCa_dCafm = dAB_dB<3, 3>(R); // fm: full matrix
338 | //    Eigen::Matrix<number_t, 9, 6> dCafm_dCa = dA_dAu<number_t, 3>(); // full matrix w.r.t. upper triangle
339 | //    Eigen::Matrix<number_t, 3, 6> dV_dCa = dV_dRCa * dRCa_dCafm * dCafm_dCa;
340 | //
341 | //    Mat3 dW_dW = -hat(gyro_calib);
342 | //    // static Mat3 dW_dbg = -I3;
343 | //
344 | //    // static Mat3 dT_dV = I3;
345 | //
346 | //    Mat3 dV_dW = -R * hat(accel_calib);
347 | //    Mat3 dV_dba = -R;
348 | //
349 | //    Mat3 dV_dWg = -R * hat(g_); // effective dimension: 3x2, since Wg is 2-dim
350 | //    // Mat2 dWg_dWg = Mat2::Identity();
351 | 
352 | 
353 | 
354 |   }
355 | 
356 |   void CalcProcessCovDerivatives(State& x, MatXd& F, Vec3d& unbiased_acc, Vec3d& unbiased_gyro) {
357 | 
358 |     MatXd F_k(18, 18);
359 | 
360 |     Mat33d gyro_skew = CreateSkew(unbiased_gyro);
361 |     Mat33d acc_skew = CreateSkew(unbiased_acc);
362 | 
363 |     Mat33d R = x.GetQuat().toRotationMatrix();
364 | 
365 |     F_k.setZero();
366 | 
367 | 
368 | 
369 |     // Velocity Jacobians
370 | 
371 |     // Jacobian velocity w.r.t Rotation body to earth
372 |     Mat33d dV_dtheta = -R * acc_skew;
373 |     // Jacobian velocity w.r.t. bias_acc
374 |     Mat33d dV_daccb = -R;
375 |     // Rotation Jacobians
376 | 
377 |     Mat33d dQ_dtheta = -gyro_skew;
378 |     Mat33d dQ_dgyrob = -Mat33d::Identity();
379 | 
380 |     // Position Jacobians
381 |     Mat33d dP_dv = Mat33d::Identity();
382 | 
383 |     // Set the blocks in the F matrix
384 |     F_k.block<3, 3>(x.pos_error_idx, x.vel_error_idx) = dP_dv;
385 |     F_k.block<3, 3>(state.quat_error_idx, state.quat_error_idx) = dQ_dtheta;
386 |     F_k.block<3, 3>(state.quat_error_idx, state.bg_error_idx) = dQ_dgyrob;
387 |     F_k.block<3, 3>(state.vel_error_idx, state.quat_error_idx) = dV_dtheta;
388 |     F_k.block<3, 3>(state.vel_error_idx, state.ba_error_idx) = dV_daccb;
389 | 
390 |     double dt;// FIXEME
391 |     MatXd Fdt = F * dt;
392 |     auto Fdt2 = Fdt * Fdt;
393 |     auto Fdt3 = Fdt2 * Fdt;
394 | 
395 |     MatXd Phi = Fdt + 0.5 * Fdt2 + (1. / 6.) * Fdt3;
396 | 
397 |     MatXd G_k(15, 12);
398 | 
399 |     G_k.setZero();
400 | 
401 |     // Jacobians with respect to noise
402 |     G_k.block<3, 3>(0, 0) = -Mat33d::Identity(); // dQ_dng
403 |     G_k.block<3, 3>(3, 3) = Mat33d::Identity(); // dGyroBias_dng
404 |     G_k.block<3, 3>(6, 6) = -R; // dV_dnacc
405 |     G_k.block<3, 3>(9, 9) = Mat33d::Identity(); // dAccBias_dnacc
406 | 
407 | 
408 |     MatXd Qc(12, 12);
409 | 
410 |     double dt2 = dt * dt; //According to Sola
411 |     Qc.block(0, 0, 3, 3) = sigma_gyr_2 / dt2 * Eigen::Matrix<double, 3, 3>::Identity();
412 |     Qc.block(3, 3, 3, 3) = sigma_bg_drift_2 / dt * Eigen::Matrix<double, 3, 3>::Identity();
413 |     Qc.block(6, 6, 3, 3) = sigma_acc_2 / dt2 * Eigen::Matrix<double, 3, 3>::Identity();
414 |     Qc.block(9, 9, 3, 3) = sigma_ba_drift_2 / dt * Eigen::Matrix<double, 3, 3>::Identity();
415 | 
416 |     MatXd P = Phi * state.imu_covar * Phi.transpose() + G_k * Qc * G_k.transpose();
417 | 
418 |   }
419 | 
420 |   void UpdateFeatures(const std::vector<Vec2d, Eigen::aligned_allocator<Vec2d>>& projs,
421 |                       const std::vector<int64_t>& ids) {
422 | 
423 |     assert(ids.size() == projs.size());
424 | 
425 |     for (int idx = 0; idx < projs.size(); ++idx) {
426 |       const int64_t id = ids[idx];
427 |       const Vec2d& proj = projs[idx];
428 | 
429 |       auto iter = map_id_landmark.find(id);
430 | 
431 |       //Means new feature. Landmark is created
432 |       if (iter == map_id_landmark.end()) {
433 |         Landmark l;
434 |         l.id = id;
435 |         l.projs.push_back(proj);
436 |         l.cam_id_first = latest_cam_id + 1;
437 |         l.cam_id_last = latest_cam_id + 1;
438 | 
439 |         auto& cur_cam_state = (state.cam_states.back());
440 |         cur_cam_state.landmark_ids.emplace_back(id);
441 | 
442 |       } else {
443 |         auto& landmark = iter->second;
444 |         landmark.projs.push_back(proj);
445 |         auto& cur_cam_state = (state.cam_states.back());
446 |         cur_cam_state.landmark_ids.emplace_back(id);
447 |         landmark.cam_id_last = cur_cam_state.cam_id;
448 |       }
449 | 
450 |     }
451 | 
452 | 
453 |     //Check if any Features have reached max length and mark them to be removed
454 |     for (auto& iter: map_id_landmark) {
455 |       auto& landmark = iter.second;
456 | 
457 |       if (landmark.projs.size() > max_track_length) {
458 |         feature_ids_to_remove.insert(landmark.id);
459 |       }
460 | 
461 |     }
462 | 
463 |   }
464 | 
465 |   bool CheckLandMarkValidity(const Landmark_To_Residualize& l) {
466 |     return true;
467 |   }
468 | 
469 |   void Marginilize() {
470 | 
471 |     int total_obs = 0;
472 | 
473 |     if (feature_ids_to_remove.size() == 0) {
474 |       return;
475 |     }
476 | 
477 |     std::vector<Landmark_To_Residualize> valid_tracks;
478 | 
479 |     for (const auto feature_id:feature_ids_to_remove) {
480 |       auto iter = map_id_landmark.find(feature_id);
481 |       assert(iter != map_id_landmark.end());
482 | 
483 |       Landmark_To_Residualize track;
484 | 
485 |       Landmark l = std::move(iter->second);
486 |       map_id_landmark.erase(iter);
487 | 
488 | 
489 | 
490 | 
491 | 
492 |       //Find all cameras connected to this landmark. Also remove any projections that don't have a camera state.
493 |       // This can happen if the camera state has been pruned
494 |       std::vector<Vec2d, Eigen::aligned_allocator<Vec2d>> valid_projs;
495 |       std::vector<int64_t> valid_cam_ids;
496 |       for (int idx = 0; idx < l.camera_ids.size(); ++idx) {
497 |         int cam_id = l.camera_ids[idx];
498 |         for (auto& cam:state.cam_states) {
499 |           if (cam_id == cam.cam_id) {
500 |             valid_projs.emplace_back(l.projs[idx]);
501 |             valid_cam_ids.emplace_back(cam_id);
502 |             track.connected_cams[cam.cam_id] = cam;
503 |             break;
504 |           }
505 | 
506 |         }
507 |       }
508 | 
509 |       //Copy over landmark info and valid projs to track
510 |       track.landmark = l;
511 |       track.landmark.projs = valid_projs;
512 |       track.landmark.camera_ids = valid_cam_ids;
513 | 
514 |       bool valid = CheckLandMarkValidity(track);
515 | 
516 |       if (!valid) {
517 |         continue;
518 |       }
519 | 
520 |       bool triang_result = TriangulateFeature(track);
521 | 
522 |       if (triang_result) {
523 |         valid_tracks.push_back(track);
524 |       }
525 | 
526 |     }
527 | 
528 |     if (valid_tracks.empty()) {
529 |       return;
530 |     }
531 | 
532 |     int num_valid = valid_tracks.size();
533 | 
534 |     MatXd H_o = MatXd::Zero(2 * total_obs - 3 * num_valid,
535 |                             15 + 6 * state.cam_states.size());
536 |     MatXd R_o = MatXd::Zero(2 * total_obs - 3 * num_valid,
537 |                             2 * total_obs - 3 * num_valid);
538 |     VecXd r_o(2 * total_obs - 3 * num_valid);
539 | 
540 |     int matrix_index = 0; // The index to navigate the above matrices
541 |     for (auto& track:valid_tracks) {
542 | 
543 |       int num_obs = track.landmark.projs.size();
544 | 
545 |       //Calculate Residual
546 | 
547 |       VecXd residuals = ComputeTrackResidual(track);
548 | 
549 |       // Noise Matrix
550 |       MatXd R_j = MatXd::Identity(residuals.size() / 2, residuals.size() / 2) * noise_pixel_sigma;
551 | 
552 |       MatXd H_x_j, H_f_j;
553 | 
554 |       ComputeJacobians(track, H_x_j, H_f_j);
555 | 
556 |       // Perform the NullSpace Marginilization. Computes A as seen in MSCKF Eq 23
557 |       Eigen::JacobiSVD<MatXd> svd_helper(H_f_j, Eigen::ComputeFullU | Eigen::ComputeThinV);
558 |       MatXd A = svd_helper.matrixU().rightCols(H_f_j.rows() - 3);
559 | 
560 |       MatXd H_o_j = A.transpose() * H_x_j;
561 |       VecXd r_o_j = A.transpose() * residuals;
562 |       MatXd R_o_j = A.transpose() * R_j * A;
563 | 
564 |       assert(r_o_j.size() == (2 * num_obs - 3)); // See MSCKF paper right after Eq 24
565 |       assert(H_o_j.rows() == r_o_j.rows());
566 | 
567 |       if (true) { // add the gating test here
568 |         r_o.segment(matrix_index, r_o_j.size()) = r_o_j;
569 |         H_o.block(matrix_index, 0, H_o_j.rows(), H_o_j.cols()) = H_o_j;
570 |         R_o.block(matrix_index, matrix_index, R_o_j.rows(), R_o_j.cols()) = R_o_j;
571 | 
572 |         matrix_index += H_o_j.rows();
573 |       }
574 |     }
575 |     H_o.conservativeResize(matrix_index, H_o.cols());
576 |     r_o.conservativeResize(matrix_index);
577 |     R_o.conservativeResize(matrix_index, matrix_index);
578 | 
579 |     // Perform Measurement Update
580 |     MeasurementUpdate(H_o, r_o, R_o);
581 | 
582 |   }
583 | 
584 |   VecXd ComputeTrackResidual(Landmark_To_Residualize& track) {
585 | 
586 |     VecXd residuals(track.landmark.projs.size() * 2);
587 | 
588 |     int iter = 0;
589 |     for (int idx = 0; idx < track.landmark.projs.size(); idx++) {
590 |       const auto& obs = track.landmark.projs[idx];
591 |       auto cam_id = track.landmark.camera_ids[idx];
592 | 
593 |       const Vec3d& pt3D = track.landmark.pos_W;
594 |       const auto& cam_state = track.connected_cams[cam_id];
595 | 
596 |       Mat44d T_wc = BuildTransform(cam_state.quat, cam_state.pos);
597 |       Mat44d T_cw = InvertTransform(T_wc);
598 |       Vec3d p_f_C = T_wc.block<3, 3>(0, 0) * pt3D + T_wc.block<3, 1>(0, 3);
599 |       Vec2d projection = p_f_C.template head<2>() / p_f_C(2);
600 | 
601 |       Vec2d res = obs - projection;
602 |       residuals[2 * idx] = res[0];
603 |       residuals[2 * idx + 1] = res[1];
604 |     }
605 | 
606 |     return residuals;
607 |   }
608 | 
609 |   void ComputeJacobians(const Landmark_To_Residualize& track, MatXd& H_x_j, MatXd& H_f_j) {
610 | 
611 |     int n_obs = track.landmark.projs.size();
612 | 
613 |     H_f_j = MatXd::Zero(2 * n_obs, 3);
614 |     H_x_j = MatXd::Zero(2 * n_obs, 15 + 6 * state.cam_states.size());
615 | 
616 |     Mat33d R_wi = state.GetQuat().toRotationMatrix(); //Rotation of IMU from World
617 |     Mat33d R_ic = q_IMU_C.toRotationMatrix(); //Rotation of Camera from IMU
618 | 
619 |     Mat33d R_wc = R_wi * R_ic;
620 | 
621 |     for (int c_i = 0; c_i < n_obs; c_i++) {
622 |       const int64_t cam_id = track.landmark.camera_ids[c_i];
623 | 
624 |       const Vec3d& pt3D = track.landmark.pos_W;
625 |       const auto& camera_state = track.connected_cams.at(cam_id); // need at for const
626 | 
627 |       // Find index of camera in current list
628 |       int cam_index = -1;
629 |       for (int idx = 0; idx < state.cam_states.size(); ++idx) {
630 |         if (state.cam_states[idx].cam_id == cam_id) {
631 |           cam_index = idx;
632 |           break;
633 |         }
634 |       }
635 |       assert(cam_index != -1);
636 | 
637 |       Vec3d p_f_C = camera_state.quat.toRotationMatrix() *
638 |           (pt3D - camera_state.pos);
639 | 
640 |       double X, Y, Z;
641 | 
642 |       X = p_f_C(0);
643 |       Y = p_f_C(1);
644 |       Z = p_f_C(2);
645 | 
646 |       // See Jacobian in MSCKF paper. Also is same in VINS-Mono Projection Factor
647 |       Eigen::Matrix<double, 2, 3> J_i;
648 |       J_i << 1, 0, -X / Z,
649 |           0, 1, -Y / Z;
650 |       J_i *= 1 / Z;
651 | 
652 |       Eigen::Matrix<double, 3, 6> dPcam_dstate = Eigen::Matrix<double, 3, 6>::Zero();
653 | 
654 |       // Jacobian w.r.t. Camera Orientation
655 |       dPcam_dstate.leftCols(3) = -R_wc.transpose();
656 | 
657 |       // Jacobian w.r.t. Camera Position
658 |       dPcam_dstate.rightCols(3) = CreateSkew(p_f_C);
659 | 
660 |       Eigen::Matrix<double, 2, 6> Jac = J_i * dPcam_dstate;
661 | 
662 |       // Jacobian w.r.t to the point in world
663 |       Eigen::Matrix<double, 2, 3> dPc_dPw = J_i * R_wc.transpose();
664 | 
665 |       if (use_observability_augmentation) {
666 |         return;
667 |       } else {
668 |         H_f_j.block<2, 3>(2 * c_i, 0) = dPc_dPw;
669 |         H_x_j.block<2, 6>(2 * c_i, 15 + 6 * (cam_index)) = Jac;
670 |       }
671 |     }
672 |   }
673 | 
674 |   bool TriangulateFeature(Landmark_To_Residualize& track);
675 | 
676 |   void MeasurementUpdate(const MatXd& H_o, const MatXd& r_o, const MatXd& R_o) {
677 |     if (r_o.size() != 0) {
678 |       // Build MSCKF covariance matrix
679 |       MatXd P = MatXd::Zero(15 + state.cams_covar.rows(), 15 + state.cams_covar.cols());
680 |       P.template block<15, 15>(0, 0) = state.imu_covar;
681 |       if (state.cams_covar.rows() != 0) {
682 |         P.block(0, 15, 15, state.imu_cam_covar.cols()) = state.imu_cam_covar;
683 |         P.block(15, 0, state.imu_cam_covar.cols(), 15) = state.imu_cam_covar.transpose();
684 |         P.block(15, 15, state.cams_covar.rows(), state.cams_covar.cols()) = state.cams_covar;
685 |       }
686 | 
687 |       MatXd T_H, Q_1, R_n;
688 |       VecXd r_n;
689 | 
690 |       // Put residuals in update-worthy form
691 |       // Calculates T_H matrix according to Mourikis 2007
692 |       Eigen::HouseholderQR<MatXd> qr(H_o);
693 |       MatXd Q = qr.householderQ();
694 |       MatXd R = qr.matrixQR().template triangularView<Eigen::Upper>();
695 | 
696 |       VecXd nonZeroRows = R.rowwise().any();
697 |       int numNonZeroRows = nonZeroRows.sum();
698 | 
699 |       T_H = MatXd::Zero(numNonZeroRows, R.cols());
700 |       Q_1 = MatXd::Zero(Q.rows(), numNonZeroRows);
701 | 
702 |       size_t counter = 0;
703 |       for (size_t r_ind = 0; r_ind < R.rows(); r_ind++) {
704 |         if (nonZeroRows(r_ind) == 1.0) {
705 |           T_H.row(counter) = R.row(r_ind);
706 |           Q_1.col(counter) = Q.col(r_ind);
707 |           counter++;
708 |           if (counter > numNonZeroRows) {
709 |             //ROS_ERROR("More non zero rows than expected in QR decomp");
710 |           }
711 |         }
712 |       }
713 | 
714 |       r_n = Q_1.transpose() * r_o;
715 |       R_n = Q_1.transpose() * R_o * Q_1;
716 | 
717 |       // Calculate Kalman Gain
718 |       MatXd temp = T_H * P * T_H.transpose() + R_n;
719 |       MatXd K = (P * T_H.transpose()) * temp.inverse();
720 | 
721 |       // State Correction
722 |       VecXd deltaX = K * r_n;
723 | 
724 | //      // Update IMU state (from updateState matlab function defined in MSCKF.m)
725 | //      Eigen::Quaterniond q_IG_up = buildUpdateQuat(deltaX.template head<3>()) * state..q_IG;
726 | //
727 | //      imu_state_.q_IG = q_IG_up;
728 | //
729 | //      imu_state_.b_g += deltaX.template segment<3>(3);
730 | //      imu_state_.b_a += deltaX.template segment<3>(9);
731 | //      imu_state_.v_I_G += deltaX.template segment<3>(6);
732 | //      imu_state_.p_I_G += deltaX.template segment<3>(12);
733 | //
734 | //      // Update Camera<_S> states
735 | //      for (size_t c_i = 0; c_i < cam_states_.size(); c_i++) {
736 | //        Quaterniond q_CG_up = buildUpdateQuat(deltaX.template segment<3>(15 + 6 * c_i)) *
737 | //            cam_states_[c_i].q_CG;
738 | //        cam_states_[c_i].q_CG = q_CG_up.normalized();
739 | //        cam_states_[c_i].p_C_G += deltaX.template segment<3>(18 + 6 * c_i);
740 | //      }
741 | //
742 | //      // Covariance correction
743 | //      MatXd tempMat = MatXd::Identity(15 + 6 * cam_states_.size(),
744 | //                                      15 + 6 * cam_states_.size()) -
745 | //          K * T_H;
746 | //
747 | //      MatXd P_corrected, P_corrected_transpose;
748 | //      P_corrected = tempMat * P * tempMat.transpose() + K * R_n * K.transpose();
749 | //      // Enforce symmetry
750 | //      P_corrected_transpose = P_corrected.transpose();
751 | //      P_corrected += P_corrected_transpose;
752 | //      P_corrected /= 2;
753 | //
754 | //      if (P_corrected.rows() - 15 != state.cams_covar.rows()) {
755 | //        std::cout << "[P:" << P_corrected.rows() << "," << P_corrected.cols() << "]";
756 | //        std::cout << "[state.cams_covar:" << state.cams_covar.rows() << "," << state.cams_covar.cols() << "]";
757 | //        std::cout << std::endl;
758 | //      }
759 | //
760 | //      // TODO : Verify need for eig check on P_corrected here (doesn't seem too
761 | //      // important for now)
762 | //      state.imu_covar = P_corrected.template block<15, 15>(0, 0);
763 | //
764 | //      // TODO: Check here
765 | //      state.cams_covar = P_corrected.block(15, 15, P_corrected.rows() - 15,
766 | //                                     P_corrected.cols() - 15);
767 | //      state.imu_cam_covar = P_corrected.block(0, 15, 15, P_corrected.cols() - 15);
768 | 
769 |       return;
770 |     } else
771 |       return;
772 |   }
773 | 
774 |   void AugmentState() {
775 |     // T_wc = T_wi*T_ic
776 |     // w=world,c=camera,i=imu/body
777 |     // T=Rt
778 |     // Q_wc=Q_wi*Q_ic
779 |     Eigen::Quaterniond cam_quat = state.GetQuat() * q_IMU_C; //
780 |     cam_quat.normalize(); // ensure it is unit quaternion
781 | 
782 |     // t_wc= t_wi+ R_wi*t_ic
783 |     Vec3d cam_pose = state.GetPos() + state.GetQuat() * p_IMU_C;
784 | 
785 |     CamState cam;
786 | 
787 |     cam.pos = cam_pose;
788 |     cam.quat = cam_quat;
789 | 
790 |     state.cam_states.push_back(cam);
791 | 
792 |     if (state.cam_states.size() > 0) {
793 |       state.P.resize(15 + state.cams_covar.rows(), 15 + state.cams_covar.cols());
794 | 
795 |       state.P.block<15, 15>(0, 0) = state.imu_covar;
796 |       state.P.block(0, 15, 15, state.cams_covar.cols()) = state.imu_covar;
797 |       state.P.block(15, 0, state.cams_covar.rows(), 15) = state.imu_covar.transpose();
798 | 
799 |       state.P.block(15, 15, state.cams_covar.rows(), state.cams_covar.cols()) = state.cams_covar;
800 | 
801 |     } else {
802 |       state.P = state.imu_covar;
803 |     }
804 | 
805 |     // Size is 6 since minimal Jacobian pose is 6(quat=3,pos=3)
806 | 
807 |     MatXd Jac = MatXd::Zero(6, 15 + 6 * state.cam_states.size());
808 | 
809 |     // Follows the format seen in Mourikis MSCKF Eq 16
810 |     // Jacobians are with Body to Earth using Q_wc=Q_wi*Q_ic and t_wc= t_wi+ R_wi*t_ic
811 | 
812 |     Mat33d dQC_dQI = q_IMU_C.toRotationMatrix(); // Jac of cam_quat w.r.t imu_quat
813 | 
814 |     Mat33d s = CreateSkew(p_IMU_C);
815 |     Mat33d dPC_dQI = -state.GetQuat().toRotationMatrix() * s; // Jac of cam_position with respect to imu_quat
816 |     // derivative of this can be found in Bloesh primer
817 | 
818 |     Mat33d dPC_dPI = Mat33d::Identity(); // Jac of cam_position w.r.t imu position
819 | 
820 |     Jac.block<3, 3>(0, state.quat_error_idx) = dQC_dQI;
821 |     Jac.block<3, 3>(3, state.quat_error_idx) = dPC_dQI;
822 |     Jac.block<3, 3>(3, state.pos_error_idx) = dPC_dPI;
823 | 
824 |     int n_cams = state.cam_states.size();
825 | 
826 |     MatXd tempMat = MatXd::Identity(15 + 6 * n_cams + 6,
827 |                                     15 + 6 * n_cams);
828 |     tempMat.block(15 + 6 * n_cams, 0, 6,
829 |                   15 + 6 * n_cams) = Jac;
830 | 
831 |     MatXd P_aug = tempMat * state.P * tempMat.transpose();
832 | 
833 |     MatXd P_aug_sym = (P_aug + P_aug.transpose()) / 2.0;
834 | 
835 |     state.cam_states.push_back(cam);
836 | 
837 |     state.imu_covar = P_aug_sym.block<15, 15>(0, 0);
838 | 
839 |     state.cams_covar.resize(P_aug_sym.rows() - 15, P_aug_sym.cols() - 15);
840 |     state.cams_covar = P_aug.block(15, 15, P_aug_sym.rows() - 15, P_aug_sym.cols() - 15);
841 | 
842 |   }
843 | 
844 | };
845 | 
846 | }
847 | 


--------------------------------------------------------------------------------