void ScanMatcher::getPose(geometry_msgs::PoseWithCovarianceStamped& pose) const {
pose.header.stamp = transform_.stamp_;
pose.header.frame_id = transform_.frame_id_;
getPose(pose.pose.pose);
if (covariance_valid_)
std::copy(&(covariance_(0,0)), &(covariance_(5,5)) + 1, pose.pose.covariance.begin());
else
pose.pose.covariance.assign(0.0);
}
/**
* \return Gaussian second centered moment
*
* Computes the covariance from other representation of not available
*
* \throws GaussianUninitializedException if the Gaussian is of dynamic-size
* and has not been initialized using SetStandard(dimension).
* \throws InvalidGaussianRepresentationException if non-of theember function)
* representation can be used as a source
*/
virtual const SecondMoment& covariance() const
{
if (dimension() == 0)
{
fl_throw(GaussianUninitializedException());
}
if (is_dirty(CovarianceMatrix) && is_dirty(DiagonalCovarianceMatrix))
{
switch (select_first_representation<4>({{DiagonalSquareRootMatrix,
DiagonalPrecisionMatrix,
SquareRootMatrix,
PrecisionMatrix}}))
{
case SquareRootMatrix:
covariance_ = square_root_ * square_root_.transpose();
break;
case PrecisionMatrix:
covariance_ = precision_.inverse();
break;
case DiagonalSquareRootMatrix:
covariance_.setZero(dimension(), dimension());
for (int i = 0; i < square_root_.diagonalSize(); ++i)
{
covariance_(i, i) = square_root_(i, i) * square_root_(i, i);
}
break;
case DiagonalPrecisionMatrix:
covariance_.setZero(dimension(), dimension());
for (int i = 0; i < precision_.diagonalSize(); ++i)
{
covariance_(i, i) = 1./precision_(i, i);
}
break;
default:
fl_throw(InvalidGaussianRepresentationException());
break;
}
updated_internally(CovarianceMatrix);
}
return covariance_;
}
void gaussian_process::evaluate( const Eigen::MatrixXd& domains, Eigen::VectorXd& means, Eigen::VectorXd& variances ) const
{
if( domains.cols() != domains_.cols() ) { COMMA_THROW( comma::exception, "expected " << domains_.cols() << " column(s) in domains, got " << domains.cols() << std::endl ); }
Eigen::MatrixXd Kxsx = Eigen::MatrixXd::Zero( domains.rows(), domains_.rows() );
for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
{
const Eigen::VectorXd& row = domains.row( r );
for( std::size_t c = 0; c < std::size_t( domains_.rows() ); ++c )
{
Kxsx( r, c ) = covariance_( row, domains_.row( c ) );
}
}
means = Kxsx * alpha_;
means.array() += offset_;
Eigen::MatrixXd Kxxs = Kxsx.transpose();
L_.matrixL().solveInPlace( Kxxs );
Eigen::MatrixXd& variance = Kxxs;
variance = variance.array() * variance.array();
variances = variance.colwise().sum();
// for each diagonal variance, set v(r) = -v(r,r) + Kxsxs
for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
{
variances( r ) = -variances( r ) + self_covariance_;
}
}
gaussian_process::gaussian_process( const Eigen::MatrixXd& domains
, const Eigen::VectorXd& targets
, const gaussian_process::covariance& covariance
, double self_covariance )
: domains_( domains )
, targets_( targets )
, covariance_( covariance )
, self_covariance_( self_covariance )
, offset_( targets.sum() / targets.rows() )
, K_( domains.rows(), domains.rows() )
{
if( domains.rows() != targets.rows() ) { COMMA_THROW( comma::exception, "expected " << domains.rows() << " row(s) in targets, got " << targets.rows() << " row(s)" ); }
targets_.array() -= offset_; // normalise
//use m_K as Kxx + variance*I, then invert it
//fill Kxx with values from covariance function
//for elements r,c in upper triangle
for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
{
K_( r, r ) = self_covariance_;
const Eigen::VectorXd& row = domains.row( r );
for( std::size_t c = r + 1; c < std::size_t( domains.rows() ); ++c )
{
K_( c, r ) = K_( r, c ) = covariance_( row, domains.row( c ) );
}
}
L_.compute( K_ ); // invert Kxx + variance * I to become (by definition) B
alpha_ = L_.solve( targets_ );
}