SLAM中非线性优化(Eigen实现 最小2乘法)

安装matplotlibcpp.h 画散点图

1. 下载：git clone https://github.com/lava/matplotlib-cpp
2. 例子使用：

   #include "matplotlibcpp.h"
   namespace plt = matplotlibcpp;
   int main() {
       plt::plot({1,3,2,4});
       plt::show();
   }
   

   编译：
   g++ minimal.cpp -std=c++11 -I/usr/include/python2.7 -lpython2.7
   会报错如下：
   在matplotlibcpp.h中（315行），注释如下部分即可。
   
3. 在Cmakelists，使用matplotlibcpp.h

 add_compile_options(-std=c++11)
 #指定库路径
file(GLOB_RECURSE Opencv2.7_LIB "/usr/lib/python2.7/config-x86_64-linux-gnu/*.so")
#指定头文件路径
set(Opencv2.7_INLCUDE_DIRS "/usr/include/python2.7")
#添加头文件到工程
include_directories(include ${Opencv2.7_INLCUDE_DIRS})
#编译时添加链接库
target_link_libraries(my_exe ${Opencv2.7_LIB})

2维散点，拟合一次线性函数y=Ax

1. $x=(A^{T}A{)}^{-1}{A}^{T}b$

#include </home/n1/notes/matplotlib-cpp/matplotlibcpp.h>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/SVD>
using namespace std;
using namespace Eigen;
namespace plt = matplotlibcpp;
//将矩阵转为<vector>
void copy(std::vector<float> &a,MatrixXf Mat,int len)
{
	Matrix<float,Dynamic,Dynamic,RowMajor> Mcopy(Mat);
	for(int i=0;i<len;i++)
	{
		a.push_back(Mcopy.transpose().data()[i]);
	}
}
int main(int argc,char **argv) 
{	
	//拟合y=Ax
	vector<float> x,y,z;
	MatrixXf A=MatrixXf::Random(50,2);//随机50×2,组数据，第一列为x坐标，第2列为y坐标
	MatrixXf x_matrix=A.col(0);//取x值
	MatrixXf y_matrix=A.col(1);//取y值
	cout<<"x"<<x_matrix<<endl;//查看
	cout<<"y"<<y_matrix<<endl;//查看
	VectorXf b = VectorXf::Random(50,1);
	cout<<"b"<<b<<endl;
	copy(x,x_matrix,x_matrix.size());//将MatrixXf 类型转化为vector<float>,方便画散点图
	copy(y,y_matrix,y_matrix.size());//将MatrixXf 类型转化为vector<float>,方便画散点图
	//求解线性最小2乘 x=(AA^T)^{-1}*A^T
	MatrixXf Out=((x_matrix.transpose()*x_matrix).inverse()*(x_matrix.transpose())*y_matrix);
	cout<<"Out"<<Out<<endl;//输出系数矩阵
	//Out 为系数矩阵
	
	//SVD 分解
	MatrixXf SVD=();
	MatrixXf=
	for(int j=0;j<x.size();j++)
	{
		z.push_back(x[j]*Out.data()[0]+Out.data()[1]);//计算矫正后的曲线值，并显示
	}
	plt::scatter(x,y);//画出待拟合的散点
	plt::plot(x,z,"r--");//显示拟合的曲线
	plt::show();
}

2. $LU$`

	MatrixXf LU=(ATA.lu()).solve(AT*y_matrix);
	cout<<"LU"<<LU<<endl;//输出系数矩阵

3. $SVD$

	//SVD分解
	MatrixXf SVD=x_matrix.bdcSvd(ComputeThinU|ComputeThinV).solve(y_matrix);
	cout<<"SVD"<<SVD<<endl;//输出系数矩阵

4. $cholesky$

	//cholesky 分解
	MatrixXf Cholesky=ATA.llt().solve(AT*y_matrix);
	cout<<"Cholesky"<<Cholesky<<endl;//输出系数矩阵

效果图如下：