summaryrefslogtreecommitdiff
path: root/src/feature.h
diff options
context:
space:
mode:
authorMartin Miller2017-03-24 13:32:25 -0500
committerMartin Miller2017-03-24 13:32:25 -0500
commitbd2b2ae18c2c9416fd57a02b187d42b805969af0 (patch)
tree6d73c31142ddda14413b5f437e3cecd8d57893ce /src/feature.h
parent9458b30126e346c9e0d373c9df5690655bf7f8ab (diff)
downloadrefslam-bd2b2ae18c2c9416fd57a02b187d42b805969af0.zip
refslam-bd2b2ae18c2c9416fd57a02b187d42b805969af0.tar.gz
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.
Diffstat (limited to 'src/feature.h')
-rw-r--r--src/feature.h13
1 files changed, 10 insertions, 3 deletions
diff --git a/src/feature.h b/src/feature.h
index eb3e5bd..fe2afea 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -1,12 +1,17 @@
#ifndef feature_INC
#define feature_INC
#include <Eigen/Dense>
+#include <iostream>
#include "types.h"
using Eigen::Vector3d;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::Quaterniond;
+using std::cout;
+using std::cerr;
+using std::endl;
+
/*
* =====================================================================================
@@ -18,8 +23,8 @@ class Feature
{
public:
/* ==================== LIFECYCLE ======================================= */
- Feature ();
- Feature ( int id, const Vector3d &xib, const Vector3d &xpb, const Vector3d &xbw, const Quaterniond &q ); /* constructor */
+ Feature ( int id, const Vector3d &xs, const Vector3d &xr,
+ const Vector3d &xbw, const Quaterniond &q, double z ); /* constructor */
/* ==================== ACCESSORS ======================================= */
int id();
@@ -29,7 +34,9 @@ class Feature
void motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt);
/* ==================== OPERATORS ======================================= */
- Matrix<double,3,9> Fx();
+ Vector3d findDepth( const Quaterniond &q, double z, const Vector3d &xs,
+ const Vector3d &xr);
+ Matrix<double,3,9> Fx( double dt );
Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang, double dt);
Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q);
Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q);