From e3ac604dbf95824bfb08d312b56b737590f33fb9 Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Fri, 26 Apr 2019 10:45:10 +0200
Subject: [PATCH] changed structure for sublime folding

---
 src/msckf_vio.cpp | 41 ++++++++++++++++++++++-------------------
 1 file changed, 22 insertions(+), 19 deletions(-)

diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index c87bfd4..9cd6334 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -292,7 +292,7 @@ bool MsckfVio::initialize() {
   for (int i = 1; i < 100; ++i) {
     boost::math::chi_squared chi_squared_dist(i);
     chi_squared_test_table[i] =
-      boost::math::quantile(chi_squared_dist, 0.2);
+      boost::math::quantile(chi_squared_dist, 0.05);
   }
 
   if (!createRosIO()) return false;
@@ -301,9 +301,7 @@ bool MsckfVio::initialize() {
   return true;
 }
 
-void MsckfVio::imuCallback(
-    const sensor_msgs::ImuConstPtr& msg)
-{
+void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg){
 
   // IMU msgs are pushed backed into a buffer instead of
   // being processed immediately. The IMU msgs are processed
@@ -491,7 +489,8 @@ void MsckfVio::initializeGravityAndBias() {
 
 bool MsckfVio::resetCallback(
     std_srvs::Trigger::Request& req,
-    std_srvs::Trigger::Response& res) {
+    std_srvs::Trigger::Response& res) 
+{
 
   cout << "reset" << endl;
   ROS_WARN("Start resetting msckf vio...");
@@ -559,17 +558,16 @@ bool MsckfVio::resetCallback(
   imu_sub = nh.subscribe("imu", 100,
       &MsckfVio::imuCallback, this);
 
-//  feature_sub = nh.subscribe("features", 40,
-//      &MsckfVio::featureCallback, this);
+  //  feature_sub = nh.subscribe("features", 40,
+  //      &MsckfVio::featureCallback, this);
 
-// TODO: When can the reset fail?
+  // TODO: When can the reset fail?
   res.success = true;
   ROS_WARN("Resetting msckf vio completed...");
   return true;
 }
 
-void MsckfVio::mocapOdomCallback(
-    const nav_msgs::OdometryConstPtr& msg) {
+void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) {
   static bool first_mocap_odom_msg = true;
 
   // If this is the first mocap odometry messsage, set
@@ -668,7 +666,8 @@ void MsckfVio::batchImuProcessing(const double& time_bound) {
 
 void MsckfVio::processModel(const double& time,
     const Vector3d& m_gyro,
-    const Vector3d& m_acc) {
+    const Vector3d& m_acc) 
+{
 
   // Remove the bias from the measured gyro and acceleration
   IMUState& imu_state = state_server.imu_state;
@@ -739,7 +738,8 @@ void MsckfVio::processModel(const double& time,
 
 void MsckfVio::predictNewState(const double& dt,
     const Vector3d& gyro,
-    const Vector3d& acc) {
+    const Vector3d& acc)
+{
 
   // TODO: Will performing the forward integration using
   //    the inverse of the quaternion give better accuracy?
@@ -802,7 +802,8 @@ void MsckfVio::predictNewState(const double& dt,
   return;
 }
 
-void MsckfVio::stateAugmentation(const double& time) {
+void MsckfVio::stateAugmentation(const double& time) 
+{
 
   const Matrix3d& R_i_c = state_server.imu_state.R_imu_cam0;
   const Vector3d& t_c_i = state_server.imu_state.t_cam0_imu;
@@ -866,7 +867,8 @@ void MsckfVio::stateAugmentation(const double& time) {
   return;
 }
 
-void MsckfVio::PhotometricStateAugmentation(const double& time) {
+void MsckfVio::PhotometricStateAugmentation(const double& time) 
+{
 
   const Matrix3d& R_i_c = state_server.imu_state.R_imu_cam0;
   const Vector3d& t_c_i = state_server.imu_state.t_cam0_imu;
@@ -936,10 +938,9 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) {
   return;
 }
 
-
-
 void MsckfVio::addFeatureObservations(
-    const CameraMeasurementConstPtr& msg) {
+    const CameraMeasurementConstPtr& msg) 
+{
 
   StateIDType state_id = state_server.imu_state.id;
   int curr_feature_num = map_server.size();
@@ -973,7 +974,8 @@ void MsckfVio::addFeatureObservations(
 void MsckfVio::PhotometricMeasurementJacobian(
     const StateIDType& cam_state_id,
     const FeatureIDType& feature_id,
-    MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) {
+    MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) 
+{
 
   // Prepare all the required data.
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
@@ -1125,7 +1127,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
 void MsckfVio::PhotometricFeatureJacobian(
     const FeatureIDType& feature_id,
     const std::vector<StateIDType>& cam_state_ids,
-    MatrixXd& H_x, VectorXd& r) {
+    MatrixXd& H_x, VectorXd& r) 
+{
 
   const auto& feature = map_server[feature_id];