summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/camera.cpp53
-rw-r--r--src/camera.h6
-rw-r--r--src/feature.cpp72
-rw-r--r--src/feature.h13
-rw-r--r--src/state.cpp58
-rw-r--r--src/state.h6
6 files changed, 134 insertions, 74 deletions
diff --git a/src/camera.cpp b/src/camera.cpp
index 9b7c134..d522cd6 100644
--- a/src/camera.cpp
+++ b/src/camera.cpp
@@ -134,56 +134,3 @@ Camera::img2body ( Vector3d &xi )
return xb;
} /* ----- end of method Camera::img2body ----- */
-/*
- *--------------------------------------------------------------------------------------
- * Class: Camera
- * Method: Camera :: ref2body
- * Description: Determines the feature position with respect to the body from a
- * source reflection correspondence xs & sr given the body rotation and height.
- *--------------------------------------------------------------------------------------
- */
-Vector3d
-Camera::ref2body ( double z, const Quaterniond &qbw, const Vector3d &xs,
- const Vector3d &xr )
-{
- // The output vector
- Vector3d xb;
- Vector4d xb4;
-
- // Rotation from body to camera
- Matrix<double,4,4> Rbc4;
- Rbc4 = Rc2b4().transpose();
-
- // Projection matrices
- Matrix<double,4,4> P1,P2,P;
- P1 = K4()*Rbc4;
-
- // Rotation from body to world
- Matrix<double,3,3> Rbw;
- Matrix<double,4,4> Rbw4,Rwb4;
- Rbw = qbw.toRotationMatrix();
- Rbw4 = Matrix<double,4,4>::Identity();
- Rbw4.block<3,3>(0,0) = Rbw;
- Rwb4 = Rbw4.transpose();
-
-
- Matrix<double,4,4> J; // Reflection matrix
- J = Matrix<double,4,4>::Identity();
- J(2,2) = -1;
- J(2,3) = -2*z;
-
- P2 = K4()*Rbc4*Rwb4*J*Rbw4;
- P.block<3,4>(0,0) = P1.block<3,4>(0,0);
- P.block<1,4>(3,0) = P2.block<1,4>(1,0);
-
- Vector4d b;
- b.segment<3>(0) = xs;
- b[3] = xr[1];
-
- xb4 = P.inverse()*b;
- xb4 /= xb4[3];
- xb = xb4.segment<3>(0);
-
- return xb ;
-} /* ----- end of method Camera::ref2body ----- */
-
diff --git a/src/camera.h b/src/camera.h
index be0bd54..bee8246 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -1,6 +1,7 @@
#ifndef camera_INC
#define camera_INC
#include <Eigen/Dense>
+#include <iostream>
#include <yaml-cpp/yaml.h>
#define BINNING 0.5 // set the binning factor
@@ -8,6 +9,9 @@ using Eigen::Matrix;
using Eigen::Vector4d;
using Eigen::Vector3d;
using Eigen::Quaterniond;
+using std::cout;
+using std::cerr;
+using std::endl;
/*
* =====================================================================================
@@ -32,8 +36,6 @@ class Camera
/* ==================== OPERATORS ======================================= */
Vector3d img2body(Vector3d &xi);
- Vector3d ref2body(double z, const Quaterniond &qbw, const Vector3d &xs,
- const Vector3d &xr);
protected:
/* ==================== METHODS ======================================= */
diff --git a/src/feature.cpp b/src/feature.cpp
index fd4a85f..aa0490e 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -25,18 +25,79 @@
* Description: constructor
*--------------------------------------------------------------------------------------
*/
-Feature::Feature ( int id, const Vector3d &xib, const Vector3d &xpb,
- const Vector3d &xbw, const Quaterniond &q ) /* constructor */
+Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr,
+ const Vector3d &xbw, const Quaterniond &q, double z ) /* constructor */
{
_id = id;
+ Vector3d xib;
+ xib = findDepth(q,z,xs,xr);
X = x2p(xib);
- X[2] = 1./20.;
xb0w = xbw;
q0 = q;
} /* ----- end of method Feature::Feature (constructor) ----- */
-Feature::Feature () {;}
+/*
+ *--------------------------------------------------------------------------------------
+ * Class: Feature
+ * Method: Feature :: findDepth
+ * Description: Compute the feature depth given the camera pose and the source
+ * and reflection measurements. The measurements xs and xr should be given in
+ * the body frame (i.e. transformed from the image frame into the camera frame
+ * according to K and rotated from the camera frame into the body frame with
+ * Rcb. The feature is then returned in body coordinates.
+ *--------------------------------------------------------------------------------------
+ */
+Vector3d
+Feature::findDepth ( const Quaterniond &qbw, double z, const Vector3d &xs,
+ const Vector3d &xr )
+{
+ // The output vector
+ Vector3d xb;
+ Vector4d xb4;
+
+ // Projection matrices
+ Matrix<double,4,4> P1,P2;
+ P1 = Matrix<double,4,4>::Identity();
+
+ // Rotation from body to world
+ Matrix<double,3,3> Rbw;
+ Matrix<double,4,4> Rbw4,Rwb4;
+ Rbw = qbw.toRotationMatrix();
+ Rbw4 = Matrix<double,4,4>::Identity();
+ Rbw4.block<3,3>(0,0) = Rbw;
+ Rwb4 = Rbw4.transpose();
+
+ Matrix<double,4,4> J; // Reflection matrix
+ J = Matrix<double,4,4>::Identity();
+ J(2,2) = -1;
+ J(2,3) = -2*z;
+
+ P2 = Rwb4*J*Rbw4;
+
+ Matrix<double,6,4> P;
+ P.block<3,4>(0,0) = P1.block<3,4>(0,0);
+ P.block<3,4>(3,0) = P2.block<3,4>(0,0);
+
+ //Vector4d b;
+ Matrix<double,6,1> b;
+ b.segment<3>(0) = xs;
+ //b[3] = 1;
+ //b.segment<3>(4) = xr;
+ //b[7] = 1;
+ b[3] = xr[0];
+ b[4] = xr[1];
+ b[5] = xr[2];
+
+
+ xb4 = P.colPivHouseholderQr().solve(b);
+ //xb4 = P.inverse()*b;
+ xb4 /= xb4[3];
+ xb = xb4.segment<3>(0);
+ cout << xb.transpose() << endl;
+
+ return xb ;
+} /* ----- end of method Feature::findDepth ----- */
/*
*--------------------------------------------------------------------------------------
@@ -208,7 +269,7 @@ Feature::x2p ( const Vector3d &x )
*--------------------------------------------------------------------------------------
*/
Matrix<double,3,9>
-Feature::Fx ( )
+Feature::Fx ( double dt )
{
double y0,y1,y2;
y0 = X[0];
@@ -218,6 +279,7 @@ Feature::Fx ( )
F << 0., 0., 0., y0*y2,-y2, 0.,0,0,0,
0.,0.,0., y1*y2, 0.,-y2, 0,0,0,
0,0,0, y2*y2, 0, 0, 0,0,0;
+ F *= dt;
return F;
} /* ----- end of method Feature::Fx ----- */
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);
diff --git a/src/state.cpp b/src/state.cpp
index 9a2a6b3..f2344f0 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -60,26 +60,36 @@ State::update ( const Matrix<double,1,1> &z )
}
void
-State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &q )
+State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &q)
{
std::vector<measurement_t> featuresToAdd;
for (auto i=z.begin(); i!=z.end(); ++i) {
if (exists(i->id)) {
- printf("Exists!\n");
+ ;
} else {
featuresToAdd.push_back(*i);
}
}
+ addFeatures( featuresToAdd, q);
- // Expand P matrix
- expandP(featuresToAdd.size());
+ return ;
+} /* ----- end of method State::feature_update ----- */
+void
+State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
+{
+ // Expand P matrix all at once
+ expandP(F.size());
+ //
// Add new features
- for (auto i=featuresToAdd.begin(); i!=featuresToAdd.end(); ++i) {
+ for (auto i=F.begin(); i!=F.end(); ++i) {
+ // Estimate initial depth
+ Vector3d xib;
+
// Create feature
- Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q);
+ Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, -0.44);
features.push_back(f);
-
+
// Initialize P values
int j=features.size();
Matrix<double,3,3> Pi;
@@ -87,7 +97,7 @@ State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &
initializePi(j,Pi);
}
return ;
-} /* ----- end of method State::feature_update ----- */
+} /* ----- end of method State::addFeatures ----- */
/*
*--------------------------------------------------------------------------------------
@@ -109,6 +119,30 @@ State::initializePi ( int i, const Matrix<double,3,3> &Pi )
return ;
} /* ----- end of method State::initializePi ----- */
+MatrixXd
+State::F ( const Quaterniond &q, const Vector3d &w, double dt )
+{
+ Vector3d v;
+ v = body->vel();
+ // Allocate matrix F
+ MatrixXd f;
+ int rows = 9 + 3*features.size();
+ f = MatrixXd::Zero(rows,rows);
+
+ // Set body F
+ f.topLeftCorner<9,9>() = body->F(w,q,dt);
+ // Set Fxi Fyi
+ { // limit i's scope
+ auto i = features.begin();
+ for (int row=9; row<rows; row+=3,++i) {
+ f.block<3,9>(row,0) = (*i)->Fx(dt);
+ f.block<3,3>(row,row) = (*i)->Fy(v,w,dt);
+ }
+ }
+
+ return f ;
+} /* ----- end of method State::F ----- */
+
/*
*--------------------------------------------------------------------------------------
* Class: State
@@ -119,10 +153,12 @@ State::initializePi ( int i, const Matrix<double,3,3> &Pi )
void
State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt)
{
- Matrix<double,9,9> F,Q;
+ Matrix<double,9,9> Q;
Q = body->Q(dt);
- F = body->F(ang,q,dt);
- P = F*P*F.transpose()+Q;
+ MatrixXd f;
+ f = F ( q, ang, dt );
+ //cout << f << endl;
+ P = f*P*f.transpose()+Q;
// Enforce symmetry
P = 0.5*(P+P.transpose());
return ;
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;