From f21a53aa32f9a005258b129c1e0ef5c7d71890df Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Fri, 31 Mar 2017 15:54:02 -0500 Subject: Add 1-Point RANSAC. 1-Pt RANSAC is a method for detecting outlier measurements in the EKF framework. This algorithm is as described in Civera 2010. --- src/state.cpp | 229 ++++++++++++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 199 insertions(+), 30 deletions(-) (limited to 'src/state.cpp') diff --git a/src/state.cpp b/src/state.cpp index 5121e18..a9a55ae 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -59,13 +59,13 @@ State::H ( const Quaterniond &q, const std::vector &z ) int rows = Hrows(z); h = MatrixXd::Zero(rows,cols); for (int i=0,row=0; i &z ) return r ; } /* ----- end of method State::R ----- */ -/* - *-------------------------------------------------------------------------------------- - * Class: State - * Method: State :: kalmanUpdate - * Description: Performs the kalman update. - *-------------------------------------------------------------------------------------- - */ -void -State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector &z, const Quaterniond &q) +MatrixXd +State::partialUpdate( const MatrixXd &h, const MatrixXd &S, + const std::vector &z, const Quaterniond &q ) { MatrixXd K; // P^T is implied here since P is symmetric K = S.fullPivLu().solve(h*P).transpose(); - // Compute P_k|k - P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P; - P = 0.5*(P+P.transpose()); - // Compute the innovation or error Matrix y; - Eigen::ArrayXd ya; y = innovation(z,q); - ya = y; - - if ( (ya < -0.3).any() ) return; - if ( (ya > 0.3).any() ) return; - + // Get the update Matrix dx; dx = K*y; + assert (dx.rows()==9+3*features.size()); body->dx(dx.segment<9>(0)); { int row=9; @@ -211,6 +196,27 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, (*i)->dx(dx.segment<3>(row)); } } + return K; +} + +/* + *-------------------------------------------------------------------------------------- + * Class: State + * Method: State :: kalmanUpdate + * Description: Performs the kalman update. + *-------------------------------------------------------------------------------------- + */ +void +State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, + const std::vector &z, const Quaterniond &q) +{ + MatrixXd K; + K = partialUpdate(h,S,z,q); + + + // Compute P_k|k + P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P; + P = 0.5*(P+P.transpose()); return ; } /* ----- end of method State::kalmanUpdate ----- */ @@ -257,6 +263,7 @@ State::innovation( const std::vector &z, const Quaterniond &q) row += 1; } else { fprintf(stderr, "Unknown measurement type\n"); + exit(1); } } @@ -281,7 +288,12 @@ State::handle_measurements ( const std::vector &z, const Quaterni featuresToUpdate.push_back(*i); zmeas = i->height; } else if (exists(i->id)) { - featuresToUpdate.push_back(*i); + Feature *ft; + ft = featureById(i->id); + if (ft->isInlier(*i, Pxx(), Pxy(i->id), Pyy(i->id), body->ned(), q, INLIER_THRESHOLD)) + { + featuresToUpdate.push_back(*i); + } } else { featuresToAdd.push_back(*i); } @@ -291,9 +303,10 @@ State::handle_measurements ( const std::vector &z, const Quaterni int feati=0; while (i!=features.end()) { bool measured=false; - for (auto j=z.begin(); j!=z.end(); ++j) { - if (j->z_type!=REFLECTION) continue; - if (j->id==(*i)->id()) { + for (auto j=featuresToUpdate.begin(); j!=featuresToUpdate.end(); ++j) { + if (j->z_type==HEIGHT) { + continue; + } else if (j->id==(*i)->id()) { measured=true; break; } @@ -305,8 +318,9 @@ State::handle_measurements ( const std::vector &z, const Quaterni i=removeFeature(i,feati); } } - if (featuresToUpdate.size()>1) { + ransacUpdate(featuresToUpdate,q); + } else if (featuresToUpdate.size()>1) { MatrixXd h; h = H(q,featuresToUpdate); MatrixXd S; @@ -387,6 +401,7 @@ State::addFeatures ( std::vector &F, const Quaterniond &q, double Vector3d pos = body->ned(); for (auto i=F.begin(); i!=F.end(); ++i) { if (features.size()>MAXFEATURES) break; + if (std::find(blacklist.begin(),blacklist.end(), i->id)!=blacklist.end()) continue; // Create feature Feature *f; if (i->z_type==REFLECTION) { @@ -544,7 +559,7 @@ State::motionModel ( const Vector3d &acc, const Vector3d &ang, ++i; ++j; } else { - i=removeFeature(i,j); + i=removeFeature(i,false); } } } @@ -556,16 +571,55 @@ State::motionModel ( const Vector3d &acc, const Vector3d &ang, /* *-------------------------------------------------------------------------------------- * Class: State + * Method: State :: iById + * Description: Returns the the feature position in the features vector. + * Returns -1 if not found. + *-------------------------------------------------------------------------------------- + */ +int +State::iById ( int id ) +{ + int i=0; + for (auto f=features.begin(); f!=features.end(); ++f, ++i) { + if ((*f)->id()==id) return i; + } + return -1; +} /* ----- end of method State::iById ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: State * Method: State :: removeFeature * Description: Remove the Feature i and Pj in a clean fashion. *-------------------------------------------------------------------------------------- */ std::list::iterator -State::removeFeature ( std::list::iterator &i, int j ) +State::removeFeature ( std::list::iterator &i, bool bl ) { // It is important to shrink P first, because it depends on knowing the // current size of the feature vector. + int j = iById((*i)->id()); + assert (j!=-1); shrinkP(j); + if (bl) { // add to blacklist + blacklist.push_back((*i)->id()); + } + delete *i; + i = features.erase(i); + return i; +} /* ----- end of method State::removeFeature ----- */ + +std::list::iterator +State::removeFeature ( int id, bool bl ) +{ + int j = iById(id); + assert (j!=-1); + shrinkP(j); + if (bl) { + blacklist.push_back(id); + } + Feature *f = featureById(id); + auto i = std::find(features.begin(), features.end(), f); delete *i; i = features.erase(i); return i; @@ -707,3 +761,118 @@ State::shrinkP ( int i ) return ; } /* ----- end of method State::shrinkP ----- */ + void +State::ransacUpdate ( std::vector &z, const Quaterniond &q ) +{ + Matrix mu_old; // x_k|k-1 + mu_old = asVector(); + // Randomize the measurements + std::random_shuffle ( z.begin(), z.end() ); + + int N0 = z.size(); + int N = z.size(); + std::vector inliers; + for (int i=0; i Z; + Z.push_back(zi); + MatrixXd h; + h = H(q,Z); + MatrixXd S; + S = h*P*h.transpose() + R(Z); + partialUpdate(h,S,Z,q); + + for (auto j=z.begin(); j!=z.end(); ++j) { + if (j->id==zi.id) continue; + if (j->z_type==HEIGHT) { + Z.push_back(*j); + continue; + } + Feature *ft = featureById(j->id); + if (ft->isRansacInlier(*j, body->ned(), q)) { + Z.push_back(*j); + } + } + if (Z.size()>inliers.size()) { + inliers.clear(); + for (auto j=Z.begin(); j!=Z.end(); ++j) { + inliers.push_back(*j); + } + double eps = 1. - (double) inliers.size() / (double) z.size(); + //N = ceil(log(0.01)/log(eps)); + } + // Restore original state before trying again. + asVector(mu_old); + } + + // Perform a full update using the inliers. + MatrixXd h; + h = H(q,inliers); + MatrixXd S; + S = h*P*h.transpose() + R(inliers); + kalmanUpdate(h, S, inliers, q); + + // High innovation update + std::vector hi_inliers; + for (auto i=z.begin(); i!=z.end(); ++i) { + bool found = false; + for (auto j=inliers.begin(); j!=inliers.end(); ++j) { + if (i->id==j->id) { + found = true; + break; + } + } + if (!found) { + Feature *ft = featureById(i->id); + if (ft->isInlier(*i, Pxx(), Pxy(i->id), Pyy(i->id), body->ned(), q, RANSAC_HI_THRESHOLD)) { + hi_inliers.push_back(*i); + } else { + removeFeature(i->id, true); + } + } + } + + h = H(q,hi_inliers); + S = h*P*h.transpose() + R(hi_inliers); + kalmanUpdate(h, S, hi_inliers, q); + + /* + cout << "Z: " << z.size() + << " LI: " << inliers.size() + << " HI: " << hi_inliers.size() << endl; + */ + + return ; +} /* ----- end of method State::ransacUpdate ----- */ + +Matrix +State::asVector ( ) +{ + Matrix m; + int rows = 9 + 3*features.size(); + m = Matrix::Zero(rows,1); + m.head<9>() = body->asVector(); + int row = 9; + for (auto i=features.begin(); i!=features.end(); ++i) { + m.segment<3>(row) = (*i)->asVector(); + row += 3; + } + + return m ; +} /* ----- end of method State::asVector ----- */ + +void +State::asVector ( const Matrix &m ) +{ + int rows = 9 + 3*features.size(); + assert (rows==m.rows()); + body->asVector(m.head<9>()); + + int row = 9; + for (auto i=features.begin(); i!=features.end(); ++i) { + (*i)->asVector(m.segment<3>(row)); + row += 3; + } + return ; +} /* ----- end of method State::asVector ----- */ + -- cgit v1.1