GTSAM 库学习三（因⼦图进⾏最⼩乘优化）
：为路标点(GPS、imu等)
：位置的实际3D坐标
：测量值（观测值）
#include  <gtsam/base/Value.h>
//基础类均在gtsam/geometry/下
#include  <gtsam/geometry/Pose2.h>
eyeq#include  <gtsam/inference/Key.h>
#include  <gtsam/slam/BetweenFactor.h>
#include  <gtsam/nonlinear/Values.h>
#include  <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include  <gtsam/nonlinear/Marginals.h>
#include  <iostream>
#include  <gtsam/nonlinear/NonlinearFactor.h>
#include  <gtsam/nonlinear/NonlinearFactorGraph.h>
#include  <gtsam/slam/BetweenFactor.h>
#include  <gtsam/nonlinear/NonlinearFactorGraph.h>
using  namespace  gtsam ;
using  namespace  std ;
//必须实现计算误差
class  UnaryFactor : public  NoiseModelFactor1<Pose2> {锐志
//NoiseModelFactor1 gtsam 可以实现单边NoiseModelFactor
/
/                  使⽤时需要实现evaluateError()
//                  模板为VALUE，可以⽀持vector,Rot3,Pose3，包括gtsam 内的李
double  mx_,my_;
//mx_,my_：测量值
public :
typedef  boost ::shared_ptr <UnaryFactor > Ptr ;
//构造函数
UnaryFactor (Key i ,double  x ,double  y ,const  SharedNoiseModel & model ):
NoiseModelFactor1<Pose2>(model , i ), mx_(x ), my_(y ) {;}
//typedef noiseModel::Base::shared_ptr SharedNoiseModel;
//noiseModel::Base ⼀个虚基类
virtual  ~UnaryFactor (){}
l ,l 12x ,x 12u ,u 12
//计算误差并且当值⽆效时，返回⽆效的向量，有时还需要计算雅克⽐矩阵
Vector evaluateError(const Pose2& q,boost::optional<Matrix&> H=boost::none)const{
if(H)(*H)=(Matrix(2,3)<<1.0,0.0,0.0,0.0,1.0,0.0).finished();
return(Vector(2)<<q.x()-mx_,q.y()-my_).finished();
}
//克隆函数
virtual gtsam::NonlinearFactor::shared_ptr clone()const{
return boost::static_pointer_cast<gtsam::NonlinearFactor>(gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); ;}
};
int main(int argc,char const*argv[])
{
特斯拉明年开始生产人形机器人//步骤1：创建图
NonlinearFactorGraph graph;
//步骤2：添加odometry 因⼦，构造有连接因⼦之间的关系
noiseModel::Diagonal::shared_ptr odometriyNose=noiseModel::Diagonal::Sigmas(Vector3(0.2,0.2,0.1));
//步骤2：添加观测值：“GPS”信息 vector(x,y)
noiseModel::Diagonal::shared_ptr unaryNosie=noiseModel::Diagonal::Sigmas((Vector2(0.1,0.1)));
//步骤3：根据数据结构插⼊数据，为了有效验证这些值均有误差
Values InitialEstimate;
InitialEstimate.insert(1,Pose2(0.5,0.0,0.2));
雅阁2020款InitialEstimate.insert(2,Pose2(2.3,0.1,-0.2));
InitialEstimate.insert(3,Pose2(4.1,0.1,0.1));
InitialEstimate.print("\nInitial Estimate:\n");
深圳宝骏汽车//步骤4：选择⽅法进⾏迭代计算，可以设置收敛条件，如果不设置则使⽤默认参数
LevenbergMarquardtOptimizer optimizer(graph,InitialEstimate);
Values result=optimizer.optimize();
result.print("Final Result:\n");
cout<<"result 1:"<<result.at<Pose2>(1).matrix()<<endl;
cout<<"result 2:"<<result.at<Pose2>(2).matrix()<<endl;
cout<<"result 3:"<<result.at<Pose2>(3).matrix()<<endl;
// result.at<Pose2>(1).print();
// cout<<"result0:"<<Pose2(-1.5424e-14,1.34169e-15,-1.38879e-16).matrix()<<endl;
//这⾥最后结果表⽰最优值
//计算边缘概率密度，表⽰不确定度
Marginals marginals(graph, result);
cout <<"x1 covariance:\n"<< marginals.marginalCovariance(1)<< endl;
cout <<"x2 covariance:\n"<< marginals.marginalCovariance(2)<< endl;
cout <<"x3 covariance:\n"<< marginals.marginalCovariance(3)<< endl;
北京汽车魔方
return0;
}