From bd2b2ae18c2c9416fd57a02b187d42b805969af0 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Fri, 24 Mar 2017 13:32:25 -0500
Subject: Move feature depth calculation to Feature class.

And also fixed F computation for features.

I realized that the features were transformed from image to body frame
prior to being passed to State. This makes sense because the State
really doesn't need to know about Camera objects. Since the camera was
no longer necessary inside of State it made sense to move the depth
computation from Camera::ref2body() into a method in the Feature class
that does not rely on camera parameters.

Additionally, the depth solver was changed from a simple matrix
inversion to a least squares calculation.

Fx in the feature class was fixed to take into account dt.

Updating Pkk1 is in the process of being modified to handle features.
---
 src/state.h | 6 ++++++
 1 file changed, 6 insertions(+)

(limited to 'src/state.h')

diff --git a/src/state.h b/src/state.h
index 8a9d318..0bb8215 100644
--- a/src/state.h
+++ b/src/state.h
@@ -5,6 +5,7 @@
 #include <vector>
 
 #include "body.h"
+#include "camera.h"
 #include "feature.h"
 #include "types.h"
 
@@ -14,6 +15,9 @@ using Eigen::MatrixXd;
 using Eigen::Dynamic;
 using Eigen::Vector3d;
 using Eigen::Quaterniond;
+using std::cout;
+using std::cerr;
+using std::endl;
 /*
  * =====================================================================================
  *        Class:  State
@@ -28,6 +32,7 @@ class State
 
         /* ====================  ACCESSORS     ======================================= */
         bool exists(int id);
+        MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt);
 
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias(const Vector3d &b);
@@ -53,6 +58,7 @@ class State
     private:
         /* ====================  METHODS       ======================================= */
         void expandP(int Nnew);
+        void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q);
 
         /* ====================  DATA MEMBERS  ======================================= */
         Body *body;
-- 
cgit v1.1