Eigen 的简单使用 与 轨迹拟合代码的理解

工作中遇到一个问题，发到hmi的车辆引导线为斜的，有一说一，仔细看下这段代码，发现用到了Eigen库用来多项式曲线拟合，线性回归，矩阵向量计算等。

#include <iostream>
#include <vector>
#include <eigen3/Eigen/Dense>

int main()
{
  Eigen::MatrixXd matrix_a;
  matrix_a.resize(2,2);
  Eigen::IOFormat fmt;
  fmt.rowPrefix='[';
  fmt.rowSuffix=']';
  fmt.coeffSeparator=',';
  matrix_a(0,0)=1;
  matrix_a(0,1)=2;
  matrix_a(1,0)=3;
  matrix_a(1,1)=4;
  std::cout<<matrix_a.format(fmt)<<std::endl;
  std::cout<<"矩阵转置"<<std::endl;
  std::cout<<matrix_a.transpose().format(fmt)<<std::endl;
  std::cout<<"逆矩阵"<<std::endl;
  std::cout<<matrix_a.inverse().format(fmt)<<std::endl;

  return 0;
}

/*输出
[1,2]
[3,4]
矩阵转置
[1,3]
[2,4]
逆矩阵
[  -2,   1]
[ 1.5,-0.5]

如下图，通过一系列点拟合一元三次方程，表示轨迹线：

 

 利用 Eigen库完成计算，开始想着直接调用 inverse 接口不就行了？试着改了下代码，直接报错了，看提示是 Assertion `rows() == cols()' failed.

哦，忘记方阵才有逆矩阵了，既然拟合的点数量与方程阶数不一定对应，所以不能直接求 X 的逆矩阵，借用转置矩阵来计算。

（设拟合点数量为n）

下面的函数用于求得 Y=XC中Y（n阶向量）与X（n行3列矩阵）调用的时候 order 给3。

void XYTrajectoryPoly(const std::vector<Point>& traj, unsigned int order, Eigen::MatrixXd& Xs,Eigen::VectorXd& Ys) {

  Xs.resize(traj.size(), order + 1);
  Ys.resize(traj.size());
  for (uint32_t i = 0; i < traj.size(); ++i) {
    for (uint32_t j = 0; j < order + 1; ++j) {
      Xs(i, j) = std::pow(traj[i].x, j);
    }
    Ys(i) = traj[i].y;
  }
}

下面的函数，接收Y与X，返回由方程的系数组成的向量，transpose返回转置矩阵，inverse返回逆矩阵 

void LinearRegression(const Eigen::MatrixXd& Xs,const Eigen::VectorXd& Ys, 
Eigen::VectorXd& Ck) {
  Eigen::MatrixXd Pk;
  Pk = (Xs.transpose() * Xs).inverse();
  Ck = Pk * Xs.transpose() * Ys;
}

Eigen::VectorXd 的读取，下列代码输出-15

  Eigen::VectorXd Ck;
  Ck.resize(3);
  Ck(0)=10;
  Ck(1)=15;
  Ck(2)=3;
  std::cout<<-Ck(1)<<std::endl;