Commit 355ece05 authored by Anthony Mallet's avatar Anthony Mallet
Browse files

[localization/pom-genom3] Upstream patch fixing build with eigen 3.0 series

Bump PKGREVISION
parent a411d6b2
......@@ -2,7 +2,7 @@
# Created: Anthony Mallet on Mon, 21 Mar 2016
#
PKGREVISION= 1
PKGREVISION= 2
DISTNAME= pom-genom3-2.3
CATEGORIES= localization
......
SHA1 (pom-genom3-2.3.tar.gz) = 07736c2bf27fee361dca57502a2edada8ab7840a
RMD160 (pom-genom3-2.3.tar.gz) = b59d8612bd998835e14ed4e0f90be553f317c2f5
Size (pom-genom3-2.3.tar.gz) = 391346 bytes
SHA1 (patch-aa) = df596b3152afd5bc48a7e30446ece9e0a1ad8869
SHA1 (patch-aa) = 415629e651b13206e030afca16c60f1f0fc6265a
Fix an issue with cwiseMax() and eigen3 < 3.2.4
Display how bad the covariance matrix was when it becomes not positive definite.
diff --git codels/ukf_filter.cc codels/ukf_filter.cc
index 9b0747c..3cb3239 100644
index 9b0747c..3bde6c2 100644
--- codels/ukf_filter.cc
+++ codels/ukf_filter.cc
@@ -83,9 +83,10 @@ class filter_s::impl_s
@@ -16,6 +16,9 @@
*/
#include <err.h>
+#include <sstream>
+#include <string>
+
#include "ukf.h"
#include <Eigen/Geometry>
@@ -83,9 +86,16 @@ class filter_s::impl_s
warnx("state covariance is not positive definite ... fixing");
Eigen::EigenSolver<state_s::dcov_s> e(cov);
......@@ -12,7 +23,13 @@ index 9b0747c..3cb3239 100644
+ state_s::dcov_s d = e.eigenvalues().real().asDiagonal();
state_s::dcov_s v = e.eigenvectors().real();
+ d.diagonal() = d.diagonal().array().max(1e-10);
+ {
+ std::ostringstream msg;
+ msg << "diagonal was [" << d.diagonal().transpose() << "]'";
+ warnx("%s", msg.str().c_str());
+ }
+ d.diagonal() =
+ d.diagonal().cwiseMax(state_s::delta_s::Constant(1e-10));
cov = v * d * v.inverse();
llt.compute((state_s::dof + lambda()) * cov);
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment