cartographer源码分析(41)-kalman_filter-gaussian_distribution.h

源码可在https://github.com/learnmoreonce/SLAM 下载

文件:kalman_filter/gaussian_distribution.h#ifndef CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_#define CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_#include "Eigen/Cholesky"#include "Eigen/Core"#include "Eigen/Geometry"namespace cartographer {namespace kalman_filter {/*高斯分布类构造函数是N*1的均值矩阵和N*N的协方差矩阵*/template <typename T, int N>class GaussianDistribution { public:  GaussianDistribution(const Eigen::Matrix<T, N, 1>& mean,                       const Eigen::Matrix<T, N, N>& covariance)      : mean_(mean), covariance_(covariance) {}  const Eigen::Matrix<T, N, 1>& GetMean() const { return mean_; }  const Eigen::Matrix<T, N, N>& GetCovariance() const { return covariance_; } private:  Eigen::Matrix<T, N, 1> mean_;       //N*1，均值  Eigen::Matrix<T, N, N> covariance_; //N*N。协方差};/*重载+加号操作符,高斯+高斯=对应均值+均值,对应协方差+协方差返回值：新高斯对象高斯分布性质:https://zh.wikipedia.org/wiki/%E6%AD%A3%E6%80%81%E5%88%86%E5%B8%83*/template <typename T, int N>GaussianDistribution<T, N> operator+(const GaussianDistribution<T, N>& lhs,                                     const GaussianDistribution<T, N>& rhs) {  return GaussianDistribution<T, N>(lhs.GetMean() + rhs.GetMean(),                                    lhs.GetCovariance() + rhs.GetCovariance());}/*重载乘法运算符1,矩阵N*M2,高斯分布M*M返回值:高斯分布：N*N*/template <typename T, int N, int M>GaussianDistribution<T, N> operator*(const Eigen::Matrix<T, N, M>& lhs,                                     const GaussianDistribution<T, M>& rhs) {  return GaussianDistribution<T, N>(      lhs * rhs.GetMean(),                          // N*M || M*1 -> N*1      lhs * rhs.GetCovariance() * lhs.transpose()); // N*M ||M*M || M*N ->  N*N}}  // namespace kalman_filter}  // namespace cartographer#endif  // CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_

测试代码:kalman_filter/gaussian_distribution_test.cc#include "cartographer/kalman_filter/gaussian_distribution.h"#include "gtest/gtest.h"namespace cartographer {namespace kalman_filter {namespace {TEST(GaussianDistributionTest, testConstructor) {  Eigen::Matrix2d covariance;//2*2  covariance << 1., 2., 3., 4.;  GaussianDistribution<double, 2> distribution(Eigen::Vector2d(0., 1.),  //均值是0,1                                               covariance);  EXPECT_NEAR(0., distribution.GetMean()[0], 1e-9);//0列的的均值是0  EXPECT_NEAR(1., distribution.GetMean()[1], 1e-9);  EXPECT_NEAR(2., distribution.GetCovariance()(0, 1), 1e-9);//0行1列的方差是2}}  // namespace}  // namespace kalman_filter}  // namespace cartographer

本文发于：
* http://www.jianshu.com/u/9e38d2febec1
* https://zhuanlan.zhihu.com/learnmoreonce
* http://blog.csdn.net/learnmoreonce
* slam源码分析微信公众号:slamcode