Skip to content
Snippets Groups Projects
Commit 9f45eb28 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Imposing SDP+sym input instead of fixing it

parent 1a95699e
No related branches found
No related tags found
1 merge request!207FeatureBase: covariance, information, square root information upper matrices
Pipeline #
...@@ -89,8 +89,7 @@ void FeatureBase::getConstraintList(ConstraintBaseList & _ctr_list) ...@@ -89,8 +89,7 @@ void FeatureBase::getConstraintList(ConstraintBaseList & _ctr_list)
void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov) void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
{ {
// Check symmetry (soft) WOLF_ASSERT_COVARIANCE_MATRIX(_meas_cov);
assert((_meas_cov - _meas_cov.transpose()).cwiseAbs().maxCoeff() < Constants::EPS && "Not symmetric measurement covariance");
// set (ensuring strong symmetry) // set (ensuring strong symmetry)
measurement_covariance_ = _meas_cov.selfadjointView<Eigen::Upper>(); measurement_covariance_ = _meas_cov.selfadjointView<Eigen::Upper>();
...@@ -104,11 +103,11 @@ void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov) ...@@ -104,11 +103,11 @@ void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
void FeatureBase::setMeasurementInformation(const Eigen::MatrixXs & _meas_info) void FeatureBase::setMeasurementInformation(const Eigen::MatrixXs & _meas_info)
{ {
assert(_meas_info.determinant() > Constants::EPS_SMALL && "Not positive definite measurement information"); WOLF_ASSERT_INFORMATION_MATRIX(_meas_info);
assert((_meas_info - _meas_info.transpose()).cwiseAbs().maxCoeff() < Constants::EPS && "Not symmetric measurement information");
// set (ensuring strong symmetry) // set (ensuring strong symmetry)
measurement_covariance_ = _meas_info.inverse().selfadjointView<Eigen::Upper>(); measurement_covariance_ = _meas_info.inverse().selfadjointView<Eigen::Upper>();
WOLF_ASSERT_COVARIANCE_MATRIX(measurement_covariance_);
// Avoid singular covariance // Avoid singular covariance
avoidSingularCovariance(); avoidSingularCovariance();
...@@ -128,12 +127,12 @@ Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) con ...@@ -128,12 +127,12 @@ Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) con
Eigen::MatrixXs R = llt_of_info.matrixU(); Eigen::MatrixXs R = llt_of_info.matrixU();
// Good factorization // Good factorization
if ((R.transpose() * R - info).cwiseAbs().maxCoeff() < Constants::EPS) if (info.isApprox(R.transpose() * R, Constants::EPS))
return R; return R;
// Not good factorization: SelfAdjointEigenSolver // Not good factorization: SelfAdjointEigenSolver
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXs> es(info); Eigen::SelfAdjointEigenSolver<Eigen::MatrixXs> es(info);
Eigen::VectorXs eval = es.eigenvalues().real().cwiseMax(0); Eigen::VectorXs eval = es.eigenvalues().real().cwiseMax(Constants::EPS);
R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose(); R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose();
...@@ -150,7 +149,7 @@ void FeatureBase::avoidSingularCovariance() ...@@ -150,7 +149,7 @@ void FeatureBase::avoidSingularCovariance()
//std::cout << "pre\n" << measurement_covariance_ << std::endl; //std::cout << "pre\n" << measurement_covariance_ << std::endl;
if ((eigensolver.eigenvalues().array() < Constants::EPS).all()) if ((eigensolver.eigenvalues().array() < Constants::EPS).all())
measurement_covariance_= eigensolver.eigenvectors() * measurement_covariance_= eigensolver.eigenvectors() *
(eigensolver.eigenvalues().array() > Constants::EPS).select(eigensolver.eigenvalues(), Constants::EPS).asDiagonal() * eigensolver.eigenvalues().cwiseMax(Constants::EPS).asDiagonal() *
eigensolver.eigenvectors().transpose(); eigensolver.eigenvectors().transpose();
//std::cout << "post\n" << measurement_covariance_ << std::endl; //std::cout << "post\n" << measurement_covariance_ << std::endl;
} }
......
...@@ -303,28 +303,32 @@ bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M, ...@@ -303,28 +303,32 @@ bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M,
} }
template <typename T, int N, int RC> template <typename T, int N, int RC>
bool isPositiveSemiDefinite(const Eigen::Matrix<T, N, N, RC>& M) bool isPositiveSemiDefinite(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
{ {
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M); Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M);
if (eigensolver.info() == Eigen::Success) if (eigensolver.info() == Eigen::Success)
{ {
// All eigenvalues must be >= 0: // All eigenvalues must be >= 0:
return (eigensolver.eigenvalues().array() >= T(0)).all(); return (eigensolver.eigenvalues().array() >= T(eps)).all();
} }
return false; return false;
} }
template <typename T, int N, int RC> template <typename T, int N, int RC>
bool isCovariance(const Eigen::Matrix<T, N, N, RC>& M) bool isCovariance(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
{ {
return isSymmetric(M) && isPositiveSemiDefinite(M); return isSymmetric(M) && isPositiveSemiDefinite(M, eps);
} }
#define WOLF_ASSERT_COVARIANCE_MATRIX(x) \ #define WOLF_ASSERT_COVARIANCE_MATRIX(x) \
assert(x.determinant() > 0 && "Not positive definite measurement covariance"); \ assert(x.determinant() > 0 && "Not positive definite measurement covariance"); \
assert(isCovariance(x) && "Not a covariance"); assert(isCovariance(x, Constants::EPS_SMALL) && "Not a covariance");
#define WOLF_ASSERT_INFORMATION_MATRIX(x) \
assert(x.determinant() > 0 && "Not positive definite measurement covariance"); \
assert(isCovariance(x, 0.) && "Not an information matrix");
//=================================================== //===================================================
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment