OKVIS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
ode.hpp
Go to the documentation of this file.
1 /*********************************************************************************
2  * OKVIS - Open Keyframe-based Visual-Inertial SLAM
3  * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
14  * its contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Created on: Jan 7, 2014
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 #ifndef INCLUDE_OKVIS_CERES_ODE_ODE_HPP_
40 #define INCLUDE_OKVIS_CERES_ODE_ODE_HPP_
41 
42 #include <Eigen/Core>
45 #include <okvis/FrameTypedefs.hpp>
46 #include <okvis/Measurements.hpp>
47 #include <okvis/Variables.hpp>
48 #include <okvis/assert_macros.hpp>
49 
51 namespace okvis {
53 namespace ceres {
55 namespace ode {
56 
57 // to make things a bit faster than using angle-axis conversion:
58 __inline__ double sinc(double x) {
59  if (fabs(x) > 1e-6) {
60  return sin(x) / x;
61  } else{
62  static const double c_2 = 1.0 / 6.0;
63  static const double c_4 = 1.0 / 120.0;
64  static const double c_6 = 1.0 / 5040.0;
65  const double x_2 = x * x;
66  const double x_4 = x_2 * x_2;
67  const double x_6 = x_2 * x_2 * x_2;
68  return 1.0 - c_2 * x_2 + c_4 * x_4 - c_6 * x_6;
69  }
70 }
71 
72 // world-centric velocities
73 __inline__ void evaluateContinuousTimeOde(const Eigen::Vector3d& gyr, const Eigen::Vector3d& acc, double g,
74  const Eigen::Vector3d& p_WS_W, const Eigen::Quaterniond& q_WS,
75  const okvis::SpeedAndBias& sb, Eigen::Vector3d& p_WS_W_dot,
76  Eigen::Vector4d& q_WS_dot, okvis::SpeedAndBias& sb_dot,
77  Eigen::Matrix<double, 15, 15>* F_c_ptr = 0){
78  // "true" rates and accelerations
79  const Eigen::Vector3d omega_S = gyr - sb.segment<3>(3);
80  const Eigen::Vector3d acc_S = acc - sb.tail<3>();
81 
82  // nonlinear states
83  // start with the pose
84  p_WS_W_dot = sb.head<3>();
85 
86  // now the quaternion
87  Eigen::Vector4d dq;
88  q_WS_dot.head<3>() = 0.5 * omega_S;
89  q_WS_dot[3] = 0.0;
90  Eigen::Matrix3d C_WS = q_WS.toRotationMatrix();
91 
92  // the rest is straightforward
93  // consider Earth's radius. Model the Earth as a sphere, since we neither
94  // know the position nor yaw (except if coupled with GPS and magnetometer).
95  Eigen::Vector3d G = -p_WS_W - Eigen::Vector3d(0, 0, 6371009); // vector to Earth center
96  sb_dot.head<3>() = (C_WS * acc_S + g * G.normalized()); // s
97  // biases
98  sb_dot.tail<6>().setZero();
99 
100  // linearized system:
101  if (F_c_ptr) {
102  F_c_ptr->setZero();
103  F_c_ptr->block<3, 3>(0, 6) += Eigen::Matrix3d::Identity();
104  F_c_ptr->block<3, 3>(3, 9) -= C_WS;
105  F_c_ptr->block<3, 3>(6, 3) -= okvis::kinematics::crossMx(C_WS * acc_S);
106  F_c_ptr->block<3, 3>(6, 12) -= C_WS;
107  }
108 }
109 
110 
111 /*/ robo-centric velocities
112 __inline__ void evaluateContinuousTimeOde(
113  const Eigen::Vector3d& gyr, const Eigen::Vector3d& acc, double g,
114  const Eigen::Vector3d& p_WS_W, const Eigen::Quaterniond& q_WS,
115  const okvis::SpeedAndBias& sb, Eigen::Vector3d& p_WS_W_dot,
116  Eigen::Vector4d& q_WS_dot, okvis::SpeedAndBias& sb_dot,
117  Eigen::Matrix<double, 15, 15>* F_c_ptr = 0) {
118 
119  // "true" rates and accelerations
120  const Eigen::Vector3d omega_S = gyr - sb.segment<3>(3);
121  const Eigen::Vector3d acc_S = acc - sb.tail<3>();
122 
123  // rotation matrix
124  Eigen::Matrix3d C_WS = q_WS.toRotationMatrix();
125  Eigen::Matrix3d C_SW = C_WS.transpose();
126 
127  // nonlinear states
128  // start with the pose
129  p_WS_W_dot = C_WS*sb.head<3>();
130 
131  // now the quaternion
132  Eigen::Vector4d dq;
133  q_WS_dot.head<3>() = 0.5 * omega_S;
134  q_WS_dot[3] = 0.0;
135 
136  // the rest is straightforward
137  // consider Earth's radius. Model the Earth as a sphere, since we neither
138  // know the position nor yaw (except if coupled with GPS and magnetometer).
139  Eigen::Vector3d G = -p_WS_W - Eigen::Vector3d(0, 0, 6371009); // vector to Earth center
140  Eigen::Vector3d g_W = g * G.normalized();
141  sb_dot.head<3>() = acc_S - okvis::kinematics::crossMx(omega_S)*sb.head<3>() + C_SW * g_W;
142  // biases
143  sb_dot.tail<6>().setZero();
144  //sb_dot.tail<3>()=-sb.tail<3>()/360.0;
145 
146  // linearized system:
147  if (F_c_ptr) {
148  F_c_ptr->setZero();
149  F_c_ptr->block<3, 3>(0, 3) += -okvis::kinematics::crossMx(C_WS * sb.head<3>());
150  F_c_ptr->block<3, 3>(0, 6) += C_WS;
151  F_c_ptr->block<3, 3>(3, 9) += -C_WS;
152  F_c_ptr->block<3, 3>(6, 3) += C_SW * okvis::kinematics::crossMx(g_W);
153  F_c_ptr->block<3, 3>(6, 6) += -okvis::kinematics::crossMx(omega_S);
154  F_c_ptr->block<3, 3>(6, 9) += -okvis::kinematics::crossMx(sb.head<3>());
155  F_c_ptr->block<3, 3>(6, 12) += -Eigen::Matrix3d::Identity();
156  //F_c_ptr->block<3, 3>(12, 12) = - 1.0/360.0*Eigen::Matrix3d::Identity();
157  }
158 }*/
159 
161  const Eigen::Vector3d& gyr_0, const Eigen::Vector3d& acc_0,
162  const Eigen::Vector3d& gyr_1, const Eigen::Vector3d& acc_1, double g,
163  double sigma_g_c, double sigma_a_c, double sigma_gw_c, double sigma_aw_c,
164  double dt, Eigen::Vector3d& p_WS_W, Eigen::Quaterniond& q_WS,
165  okvis::SpeedAndBias& sb, Eigen::Matrix<double, 15, 15>* P_ptr = 0,
166  Eigen::Matrix<double, 15, 15>* F_tot_ptr = 0) {
167 
168  Eigen::Vector3d k1_p_WS_W_dot;
169  Eigen::Vector4d k1_q_WS_dot;
170  okvis::SpeedAndBias k1_sb_dot;
171  Eigen::Matrix<double, 15, 15> k1_F_c;
172  evaluateContinuousTimeOde(gyr_0, acc_0, g, p_WS_W, q_WS, sb, k1_p_WS_W_dot,
173  k1_q_WS_dot, k1_sb_dot, &k1_F_c);
174 
175  Eigen::Vector3d p_WS_W1 = p_WS_W;
176  Eigen::Quaterniond q_WS1 = q_WS;
177  okvis::SpeedAndBias sb1 = sb;
178  // state propagation:
179  p_WS_W1 += k1_p_WS_W_dot * 0.5 * dt;
180  Eigen::Quaterniond dq;
181  double theta_half = k1_q_WS_dot.head<3>().norm() * 0.5 * dt;
182  double sinc_theta_half = sinc(theta_half);
183  double cos_theta_half = cos(theta_half);
184  dq.vec() = sinc_theta_half * k1_q_WS_dot.head<3>() * 0.5 * dt;
185  dq.w() = cos_theta_half;
186  q_WS1 = q_WS * dq;
187  sb1 += k1_sb_dot * 0.5 * dt;
188 
189  Eigen::Vector3d k2_p_WS_W_dot;
190  Eigen::Vector4d k2_q_WS_dot;
191  okvis::SpeedAndBias k2_sb_dot;
192  Eigen::Matrix<double, 15, 15> k2_F_c;
193  evaluateContinuousTimeOde(0.5 * (gyr_0 + gyr_1), 0.5 * (acc_0 + acc_1), g,
194  p_WS_W1, q_WS1, sb1, k2_p_WS_W_dot, k2_q_WS_dot,
195  k2_sb_dot, &k2_F_c);
196 
197  Eigen::Vector3d p_WS_W2 = p_WS_W;
198  Eigen::Quaterniond q_WS2 = q_WS;
199  okvis::SpeedAndBias sb2 = sb;
200  // state propagation:
201  p_WS_W2 += k2_p_WS_W_dot * dt;
202  theta_half = k2_q_WS_dot.head<3>().norm() * dt;
203  sinc_theta_half = sinc(theta_half);
204  cos_theta_half = cos(theta_half);
205  dq.vec() = sinc_theta_half * k2_q_WS_dot.head<3>() * dt;
206  dq.w() = cos_theta_half;
207  //std::cout<<dq.transpose()<<std::endl;
208  q_WS2 = q_WS2 * dq;
209  sb2 += k1_sb_dot * dt;
210 
211  Eigen::Vector3d k3_p_WS_W_dot;
212  Eigen::Vector4d k3_q_WS_dot;
213  okvis::SpeedAndBias k3_sb_dot;
214  Eigen::Matrix<double, 15, 15> k3_F_c;
215  evaluateContinuousTimeOde(0.5 * (gyr_0 + gyr_1), 0.5 * (acc_0 + acc_1), g,
216  p_WS_W2, q_WS2, sb2, k3_p_WS_W_dot, k3_q_WS_dot,
217  k3_sb_dot, &k3_F_c);
218 
219  Eigen::Vector3d p_WS_W3 = p_WS_W;
220  Eigen::Quaterniond q_WS3 = q_WS;
221  okvis::SpeedAndBias sb3 = sb;
222  // state propagation:
223  p_WS_W3 += k3_p_WS_W_dot * dt;
224  theta_half = k3_q_WS_dot.head<3>().norm() * dt;
225  sinc_theta_half = sinc(theta_half);
226  cos_theta_half = cos(theta_half);
227  dq.vec() = sinc_theta_half * k3_q_WS_dot.head<3>() * dt;
228  dq.w() = cos_theta_half;
229  //std::cout<<dq.transpose()<<std::endl;
230  q_WS3 = q_WS3 * dq;
231  sb3 += k3_sb_dot * dt;
232 
233  Eigen::Vector3d k4_p_WS_W_dot;
234  Eigen::Vector4d k4_q_WS_dot;
235  okvis::SpeedAndBias k4_sb_dot;
236  Eigen::Matrix<double, 15, 15> k4_F_c;
237  evaluateContinuousTimeOde(gyr_1, acc_1, g, p_WS_W3, q_WS3, sb3, k4_p_WS_W_dot,
238  k4_q_WS_dot, k4_sb_dot, &k4_F_c);
239 
240  // now assemble
241  p_WS_W +=
242  (k1_p_WS_W_dot + 2 * (k2_p_WS_W_dot + k3_p_WS_W_dot) + k4_p_WS_W_dot) * dt
243  / 6.0;
244  Eigen::Vector3d theta_half_vec = (k1_q_WS_dot.head<3>()
245  + 2 * (k2_q_WS_dot.head<3>() + k3_q_WS_dot.head<3>())
246  + k4_q_WS_dot.head<3>()) * dt / 6.0;
247  theta_half = theta_half_vec.norm();
248  sinc_theta_half = sinc(theta_half);
249  cos_theta_half = cos(theta_half);
250  dq.vec() = sinc_theta_half * theta_half_vec;
251  dq.w() = cos_theta_half;
252  q_WS = q_WS * dq;
253  sb += (k1_sb_dot + 2 * (k2_sb_dot + k3_sb_dot) + k4_sb_dot) * dt / 6.0;
254 
255  q_WS.normalize(); // do not accumulate errors!
256 
257  if (F_tot_ptr) {
258  // compute state transition matrix
259  Eigen::Matrix<double, 15, 15>& F_tot = *F_tot_ptr;
260  const Eigen::Matrix<double, 15, 15>& J1 = k1_F_c;
261  const Eigen::Matrix<double, 15, 15> J2=k2_F_c*(Eigen::Matrix<double, 15, 15>::Identity()+0.5*dt*J1);
262  const Eigen::Matrix<double, 15, 15> J3=k3_F_c*(Eigen::Matrix<double, 15, 15>::Identity()+0.5*dt*J2);
263  const Eigen::Matrix<double, 15, 15> J4=k4_F_c*(Eigen::Matrix<double, 15, 15>::Identity()+dt*J3);
264  Eigen::Matrix<double, 15, 15> F = Eigen::Matrix<double, 15, 15>::Identity()
265  + dt * (J1+2*(J2+J3)+J4) / 6.0;
266  //+ dt * J1;
267  //std::cout<<F<<std::endl;
268  F_tot = (F * F_tot).eval();
269 
270  if (P_ptr) {
271  Eigen::Matrix<double, 15, 15>& cov = *P_ptr;
272  cov = F * (cov * F.transpose()).eval();
273 
274  // add process noise
275  const double Q_g = sigma_g_c * sigma_g_c * dt;
276  const double Q_a = sigma_a_c * sigma_a_c * dt;
277  const double Q_gw = sigma_gw_c * sigma_gw_c * dt;
278  const double Q_aw = sigma_aw_c * sigma_aw_c * dt;
279  cov(3, 3) += Q_g;
280  cov(4, 4) += Q_g;
281  cov(5, 5) += Q_g;
282  cov(6, 6) += Q_a;
283  cov(7, 7) += Q_a;
284  cov(8, 8) += Q_a;
285  cov(9, 9) += Q_gw;
286  cov(10, 10) += Q_gw;
287  cov(11, 11) += Q_gw;
288  cov(12, 12) += Q_aw;
289  cov(13, 13) += Q_aw;
290  cov(14, 14) += Q_aw;
291 
292  // force symmetric - TODO: is this really needed here?
293  // cov = 0.5 * cov + 0.5 * cov.transpose().eval();
294  }
295  }
296 
297 }
298 
299 }
300 
301 } // namespace ceres
302 } // namespace okvis
303 
304 #endif /* INCLUDE_OKVIS_CERES_ODE_ODE_HPP_ */
__inline__ void evaluateContinuousTimeOde(const Eigen::Vector3d &gyr, const Eigen::Vector3d &acc, double g, const Eigen::Vector3d &p_WS_W, const Eigen::Quaterniond &q_WS, const okvis::SpeedAndBias &sb, Eigen::Vector3d &p_WS_W_dot, Eigen::Vector4d &q_WS_dot, okvis::SpeedAndBias &sb_dot, Eigen::Matrix< double, 15, 15 > *F_c_ptr=0)
Definition: ode.hpp:73
Eigen::Matrix< Scalar_T, 3, 3 > crossMx(Scalar_T x, Scalar_T y, Scalar_T z)
Cross matrix of a vector [x,y,z]. Adopted from Schweizer-Messer by Paul Furgale.
Definition: operators.hpp:62
__inline__ void integrateOneStep_RungeKutta(const Eigen::Vector3d &gyr_0, const Eigen::Vector3d &acc_0, const Eigen::Vector3d &gyr_1, const Eigen::Vector3d &acc_1, double g, double sigma_g_c, double sigma_a_c, double sigma_gw_c, double sigma_aw_c, double dt, Eigen::Vector3d &p_WS_W, Eigen::Quaterniond &q_WS, okvis::SpeedAndBias &sb, Eigen::Matrix< double, 15, 15 > *P_ptr=0, Eigen::Matrix< double, 15, 15 > *F_tot_ptr=0)
Definition: ode.hpp:160
Eigen::Matrix< double, 9, 1 > SpeedAndBias
Definition: FrameTypedefs.hpp:234
This file contains the templated measurement structs, structs encapsulating Sensor data and related t...
__inline__ double sinc(double x)
Definition: ode.hpp:58
This file contains some useful assert macros.
This file contains some typedefs related to state variables.
Header file for the Transformation class.
File with some helper functions related to matrix/vector operations.
This file contains useful typedefs and structs related to frames.