From 750d8ecfb1b4377dd111f1e85aade1cbfd64fc75 Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Fri, 26 Apr 2019 14:42:31 +0200
Subject: [PATCH] sublime folding changes

---
 include/msckf_vio/feature.hpp | 27 ++++++++++++++-------------
 1 file changed, 14 insertions(+), 13 deletions(-)

diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp
index 75bd36b..e84a847 100644
--- a/include/msckf_vio/feature.hpp
+++ b/include/msckf_vio/feature.hpp
@@ -240,7 +240,8 @@ typedef std::map<FeatureIDType, Feature, std::less<int>,
 
 void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
     const Eigen::Vector3d& x, const Eigen::Vector2d& z,
-    double& e) const {
+    double& e) const 
+{
   // Compute hi1, hi2, and hi3 as Equation (37).
   const double& alpha = x(0);
   const double& beta = x(1);
@@ -263,7 +264,8 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
 void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
     const Eigen::Vector3d& x, const Eigen::Vector2d& z,
     Eigen::Matrix<double, 2, 3>& J, Eigen::Vector2d& r,
-    double& w) const {
+    double& w) const 
+{
 
   // Compute hi1, hi2, and hi3 as Equation (37).
   const double& alpha = x(0);
@@ -300,7 +302,8 @@ void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
 
 void Feature::generateInitialGuess(
     const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
-    const Eigen::Vector2d& z2, Eigen::Vector3d& p) const {
+    const Eigen::Vector2d& z2, Eigen::Vector3d& p) const 
+{
   // Construct a least square problem to solve the depth.
   Eigen::Vector3d m = T_c1_c2.linear() * Eigen::Vector3d(z1(0), z1(1), 1.0);
 
@@ -320,8 +323,8 @@ void Feature::generateInitialGuess(
   return;
 }
 
-bool Feature::checkMotion(
-    const CamStateServer& cam_states) const {
+bool Feature::checkMotion(const CamStateServer& cam_states) const 
+{
 
   const StateIDType& first_cam_id = observations.begin()->first;
   const StateIDType& last_cam_id = (--observations.end())->first;
@@ -399,7 +402,6 @@ bool Feature::estimate_FrameIrradiance(
     float irradiance = (anchorPixel - b_A) / a_A ;
     anchorPatch_estimate.push_back(irradiance);
   }
-
 }
 
 
@@ -441,6 +443,7 @@ bool Feature::VisualizePatch(
 
 float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
 {
+  
   return ((float)image.at<uint8_t>(pose.x, pose.y))/256;
 }
 
@@ -476,8 +479,7 @@ cv::Point2f Feature::projectPositionToCamera(
   return my_p;
 }
 
-Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
-          const CameraCalibration& cam)
+Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam)
 {
   // use undistorted position of point of interest
   // project it back into 3D space using pinhole model
@@ -490,8 +492,7 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
 }
 
 //@test center projection must always be initial feature projection
-bool Feature::initializeAnchor(
-   const CameraCalibration& cam, int N)
+bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
 {
 
   //initialize patch Size
@@ -532,7 +533,7 @@ bool Feature::initializeAnchor(
                                 cam.distortion_model,
                                 0,
                                 und_v);
-//create distorted pixel points
+  //create distorted pixel points
   std::vector<cv::Point2f> vec = image_handler::distortPoints(und_v,
                                                              cam.intrinsics,
                                                              cam.distortion_model,
@@ -561,8 +562,7 @@ bool Feature::initializeAnchor(
   return true;
 }
 
-bool Feature::initializePosition(
-    const CamStateServer& cam_states) {
+bool Feature::initializePosition(const CamStateServer& cam_states) {
 
   // Organize camera poses and feature observations properly.
   std::vector<Eigen::Isometry3d,
@@ -711,6 +711,7 @@ bool Feature::initializePosition(
 
   return is_valid_solution;
 }
+
 } // namespace msckf_vio
 
 #endif // MSCKF_VIO_FEATURE_H