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

changes proposed in covariance and sqrt upper information matrix

parent 997a9429
No related branches found
No related tags found
1 merge request!207FeatureBase: covariance, information, square root information upper matrices
Pipeline #
......@@ -89,40 +89,69 @@ void FeatureBase::getConstraintList(ConstraintBaseList & _ctr_list)
void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
{
if (_meas_cov.determinant() < Constants::EPS_SMALL)
{
Eigen::MatrixXs eps = Eigen::MatrixXs::Identity(_meas_cov.rows(), _meas_cov.cols()) * 1e-10;
measurement_covariance_ = _meas_cov + eps; // avoid singular covariance
}
else
measurement_covariance_ = _meas_cov;
// Check symmetry (soft)
assert((_meas_cov - _meas_cov.transpose()).cwiseAbs().maxCoeff() < Constants::EPS && "Not symmetric measurement covariance");
  • Developer

    @joanvallve FYI there are a couple helper function in wolf.h :

    • isSymmetric
    • isPositiveSemiDefinite
    • isCovariance

    Together with a macro WOLF_ASSERT_COVARIANCE_MATRIX.

  • Please register or sign in to reply
// set (ensuring strong symmetry)
measurement_covariance_ = _meas_cov.selfadjointView<Eigen::Upper>();
// Avoid singular covariance
avoidSingleCovariance();
measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_cov);
// compute square root information upper matrix
measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse());
}
void FeatureBase::setMeasurementInfo(const Eigen::MatrixXs & _meas_info)
{
assert(_meas_info.determinant() > 0 && "Not positive definite measurement information");
assert(_meas_info.determinant() > Constants::EPS_SMALL && "Not positive definite measurement information");
assert((_meas_info - _meas_info.transpose()).cwiseAbs().maxCoeff() < Constants::EPS && "Not symmetric measurement information");
measurement_covariance_ = _meas_info.inverse();
measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info.inverse());
// set (ensuring strong symmetry)
measurement_covariance_ = _meas_info.inverse().selfadjointView<Eigen::Upper>();
// Avoid singular covariance
avoidSingleCovariance();
// compute square root information upper matrix
measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info);
}
Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _covariance) const
Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) const
{
if (_covariance.determinant() < Constants::EPS_SMALL)
{
// Avoid singular covariances matrix
Eigen::MatrixXs cov = _covariance + 1e-8 * Eigen::MatrixXs::Identity(_covariance.rows(), _covariance.cols());
Eigen::LLT<Eigen::MatrixXs> llt_of_info(cov.inverse());
return llt_of_info.matrixU();
}
else
assert(_info.determinant() > Constants::EPS_SMALL && "Matrix is not positive definite!");
assert((_info - _info.transpose()).cwiseAbs().maxCoeff() < Constants::EPS && "Matrix is not symmetric!");
// impose symmetry
Eigen::MatrixXs info = ((_info + _info.transpose()) / 2);
info = info.selfadjointView<Eigen::Upper>();
// Normal Cholesky factorization
Eigen::LLT<Eigen::MatrixXs> llt_of_info(info);
Eigen::MatrixXs R = llt_of_info.matrixU();
// Good factorization
if ((R.transpose() * R - info).cwiseAbs().maxCoeff() < Constants::EPS)
return R;
// Not good factorization: SelfAdjointEigenSolver
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXs> es(info);
Eigen::VectorXs eval = es.eigenvalues().real().cwiseMax(0);
R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose();
return R;
}
void FeatureBase::avoidSingleCovariance()
{
Scalar eps_scalar = 1e-10;
while (measurement_covariance_.determinant() < Constants::EPS_SMALL && eps_scalar < 1e-3)
{
// Normal operation
Eigen::LLT<Eigen::MatrixXs> llt_of_info(_covariance.inverse());
return llt_of_info.matrixU();
measurement_covariance_ += Eigen::MatrixXs::Identity(measurement_covariance_.rows(), measurement_covariance_.cols()) * eps_scalar; // avoid singular covariance
eps_scalar*=10;
}
assert(measurement_covariance_.determinant() > Constants::EPS_SMALL && "Couldn't avoid singular covariance");
}
} // namespace wolf
......@@ -66,6 +66,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
void setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov);
void setMeasurementInfo(const Eigen::MatrixXs & _meas_info);
const Eigen::MatrixXs& getMeasurementCovariance() const;
Eigen::MatrixXs getMeasurementInformation() const;
const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const;
const Eigen::VectorXs& getExpectation() const;
......@@ -90,6 +91,8 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
private:
Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
void avoidSingleCovariance();
};
}
......@@ -135,6 +138,11 @@ inline const Eigen::MatrixXs& FeatureBase::getMeasurementCovariance() const
return measurement_covariance_;
}
inline Eigen::MatrixXs FeatureBase::getMeasurementInformation() const
{
return measurement_sqrt_information_upper_.transpose() * measurement_sqrt_information_upper_;
}
inline const Eigen::MatrixXs& FeatureBase::getMeasurementSquareRootInformationUpper() const
{
return measurement_sqrt_information_upper_;
......
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