diff options
| author | Martin Miller | 2017-04-07 11:21:22 -0500 |
|---|---|---|
| committer | Martin Miller | 2017-04-07 11:21:22 -0500 |
| commit | c1ca399fcb2516da1ebe8e6a92f3eb4fff44f224 (patch) | |
| tree | 27ad93c9b20bc03b76f5216a7702401ba346410c /src/state.cpp | |
| parent | 50f7b829a9848aae7236f69a90f7db5d9af19f22 (diff) | |
| download | refslam-c1ca399fcb2516da1ebe8e6a92f3eb4fff44f224.zip refslam-c1ca399fcb2516da1ebe8e6a92f3eb4fff44f224.tar.gz | |
changes
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 4 |
1 files changed, 4 insertions, 0 deletions
diff --git a/src/state.cpp b/src/state.cpp index 0bae8e7..937a653 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -204,6 +204,10 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S, // Get the update Matrix<double,Dynamic,1> dx; dx = K*y; + if (dx.segment<3>(3).norm()>0.04) { + cerr << "dx: " << dx.head<STATESIZE>().transpose() << endl; + return K; + } assert (dx.rows()==STATESIZE+3*features.size()); body->dx(dx.segment<STATESIZE>(0)); { |