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/camera.cpp | 53 ------------------------------------------ src/camera.h | 6 +++-- src/feature.cpp | 72 +++++++++++++++++++++++++++++++++++++++++++++++++++++---- src/feature.h | 13 ++++++++--- src/state.cpp | 58 +++++++++++++++++++++++++++++++++++++--------- src/state.h | 6 +++++ 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 Rbc4; - Rbc4 = Rc2b4().transpose(); - - // Projection matrices - Matrix P1,P2,P; - P1 = K4()*Rbc4; - - // Rotation from body to world - Matrix Rbw; - Matrix Rbw4,Rwb4; - Rbw = qbw.toRotationMatrix(); - Rbw4 = Matrix::Identity(); - Rbw4.block<3,3>(0,0) = Rbw; - Rwb4 = Rbw4.transpose(); - - - Matrix J; // Reflection matrix - J = Matrix::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 +#include #include #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 P1,P2; + P1 = Matrix::Identity(); + + // Rotation from body to world + Matrix Rbw; + Matrix Rbw4,Rwb4; + Rbw = qbw.toRotationMatrix(); + Rbw4 = Matrix::Identity(); + Rbw4.block<3,3>(0,0) = Rbw; + Rwb4 = Rbw4.transpose(); + + Matrix J; // Reflection matrix + J = Matrix::Identity(); + J(2,2) = -1; + J(2,3) = -2*z; + + P2 = Rwb4*J*Rbw4; + + Matrix 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 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 -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 +#include #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 Fx(); + Vector3d findDepth( const Quaterniond &q, double z, const Vector3d &xs, + const Vector3d &xr); + Matrix Fx( double dt ); Matrix Fy( const Vector3d &vel, const Vector3d &ang, double dt); Matrix Hx( const Vector3d &pos, const Quaterniond &q); Matrix 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 &z ) } void -State::feature_update ( const std::vector &z, const Quaterniond &q ) +State::feature_update ( const std::vector &z, const Quaterniond &q) { std::vector 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 &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 Pi; @@ -87,7 +97,7 @@ State::feature_update ( const std::vector &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 &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(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 &Pi ) void State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt) { - Matrix F,Q; + Matrix 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 #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 &F, const Quaterniond &q); /* ==================== DATA MEMBERS ======================================= */ Body *body; -- cgit v1.1