gtsam 学习十一（ISAM2 实践）

来自gtam/examples/

1. ISAM1模板

#include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     / *@brief 创建8个点 *@return 返回vector 
   
     points 长度为8 */ 
    std::vector<gtsam::Point3> createPoints() { 
    // Create the set of ground-truth landmarks std::vector<gtsam::Point3> points; points.push_back(gtsam::Point3(10.0,10.0,10.0)); points.push_back(gtsam::Point3(-10.0,10.0,10.0)); points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); points.push_back(gtsam::Point3(10.0,-10.0,10.0)); points.push_back(gtsam::Point3(10.0,10.0,-10.0)); points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); return points; } / *@brief *@param init 初始化pose，队列的第一个值，之后每一个都会在pose之上旋转delta(Pose3) *@param delta 旋转角度 *@param steps 默认为8 返回Pose3的长度 *@return vector 
   
     */ 
    std::vector<gtsam::Pose3> createPoses( const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)), const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))), int steps = 8) { 
    // Create the set of ground-truth poses // Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center std::vector<gtsam::Pose3> poses; int i = 1; poses.push_back(init); for(; i < steps; ++i) { 
    poses.push_back(poses[i-1].compose(delta)); } return poses; }; using namespace std; using namespace gtsam; int main(int argc, const char** argv) { 
    //相机内参 Cal3_S2::shared_ptr K(new Cal3_S2(50.0,50.0,0.0,50.0,50.0)); //相机观测噪声 noiseModel::Isotropic::shared_ptr noise=noiseModel::Isotropic::Sigma(2,1.0); //创建路标点，位姿 vector<Pose3> poses=createPoses(); vector<Point3> points=createPoints(); //创建ISAM：周期性重新平滑线性化和排序 int relinearizeInterval=3;//新添加多少个之后更新， NonlinearISAM isam(relinearizeInterval); //创建图模型 NonlinearFactorGraph graph; Values initialEstimate; //添加观则量 for(int i=0;i<poses.size();i++){ 
    for(int j=0;j<points.size();j++){ 
    SimpleCamera camera(poses[i],*K); Point2 measurement=camera.project(points[j]); graph.emplace_shared<GenericProjectionFactor<Pose3,Point3,Cal3_S2> >(measurement,noise,Symbol('x',i),Symbol('l',j),K); } //初始化 路边真实点 Pose3 noise(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); Pose3 initial_xi=poses[i].compose(noise);//初始旋转值 initialEstimate.insert(symbol('x',i),initial_xi); //设置起始值同时设置当前帧的坐标系，并在第一个路标点上设置尺度，ISAM使用增量求解，至少需要两个值 if(i==0){ 
    // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noiseModel::Diagonal::shared_ptr poseNoise=noiseModel::Diagonal::Sigmas(Vector6(0.3,0.3,0.3,0.1,0.1,0.1)); graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x',0),poses[0],poseNoise); //添加prior l0 noiseModel::Isotropic::shared_ptr pointnoise=noiseModel::Isotropic::Sigma(3,0.1); graph.emplace_shared<PriorFactor<Point3> >(Symbol('l',0),points[0],pointnoise); //初始化噪声 Point3 noise(-0.25, 0.20, 0.15); //插入值 for(int j=0;j<points.size();j++){ 
    Point3 initial_lj=points[j]+noise; initialEstimate.insert(Symbol('l',j),initial_lj); } } else{ 
    isam.update(graph,initialEstimate); Values currentEsimate=isam.estimate(); cout << "" << endl; cout << "Frame " << i << ": " << endl; currentEsimate.print("Current estimate: "); graph.resize(0); initialEstimate.clear(); } } return 0; } 

2. ISAM2 基本使用

#include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     using namespace std; using namespace gtsam; using symbol_shorthand::X; using symbol_shorthand::P; //Cal3_S2 相机内参结构体 typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor; //SmartProjectionFactor 专门用来实现相机位姿优化的一类 // 三角化 适用于单目相机，同时矫正标定相机和位姿 // 如果相机已经标定使用：SmartProjectionPoseFactor ：只矫正位姿 int main(int argc,char **argv){ 
    //初始化相机内参:fx_(1), fy_(1), s_(0), u0_(0), v0_(0) Cal3_S2::shared_ptr K(new Cal3_S2(50.0,50.0,0,50,50)); auto measurementNoise=noiseModel::Isotropic::Sigma(2,1.0);//1单位像素的误差值 Vector6 sigmas; sigmas<<0.3,0.3,0.3,0.1,0.1,0.1; auto noise=noiseModel::Diagonal::Sigmas(sigmas); ISAM2Params parameters; parameters.relinearizeThreshold=0.01; //差值大于0.1需要重新线性化 parameters.relinearizeSkip=1; //每当有1个值需要重新线性化时，对贝叶斯树进行更新 parameters.enableRelinearization=true; //是否可以重新线性化任意变量 parameters.evaluateNonlinearError=false; //是否计算线性化误差默认false parameters.cacheLinearizedFactors = false; //default: true 是否保存保存线性化结果，可以优化性能，但是当线性化容易计算时会造成相反效果 parameters.factorization=ISAM2Params::Factorization::QR;//默认为QR分级，还可以选用CHOESKY分解，但CHOESKY求解数值不稳定 parameters.keyFormatter=DefaultKeyFormatter;// debug时key值形式默认 parameters.enableDetailedResults=true; //是否计算返回ISAM2Result::detailedResults，会增加计算量 parameters.enablePartialRelinearizationCheck=false; //是否只进行部分更新功能 parameters.findUnusedFactorSlots=false;//当要移除许多因子时，比如ISAM2做固定平滑时，启用此选项第一值进行插入时 //避免出现NULL值，但是会造成每当有新值插入时必须查找 ISAM2 isam(parameters);//贝叶斯树 NonlinearFactorGraph graph; Values initialEstimate; Point3 point(0.0,0.0,1.0); //Rodrigues 将RPY转为悬装向量罗德里格旋转公式 Pose3 delta(Rot3::Rodrigues(0.0,0.0,0.0),Point3(0.05,-0.10,0.20)); Pose3 pose1(Rot3(), Point3(0.0, 0.0, 0.0)); Pose3 pose2(Rot3(), Point3(0.0, 0.2, 0.0)); Pose3 pose3(Rot3(), Point3(0.0, 0.4, 0.0)); Pose3 pose4(Rot3(), Point3(0.0, 0.5, 0.0)); Pose3 pose5(Rot3(), Point3(0.0, 0.6, 0.0)); vector<Pose3> poses = { 
   pose1, pose2, pose3, pose4, pose5}; //构造图 //起始点 graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], noise); initialEstimate.insert(X(0), poses[0].compose(delta)); SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K)); smartFactor->add(PinholePose<Cal3_S2>(poses[0], K).project(point), X(0)); graph.push_back(smartFactor); for(size_t i=1;i<5;i++){ 
    cout << "" << endl; cout << "i = " << i << endl; graph.emplace_shared<PriorFactor<Pose3>>(X(i), poses[i], noise); initialEstimate.insert(X(i), poses[i].compose(delta));//SE compose 偏移小量 //添加噪声模型 PinholePose<Cal3_S2> camera(poses[i],K); //仿真相机 位姿+内参 Point2 measurement=camera.project(point); //point在5个位置投影 cout << "Measurement " << i << "" << measurement << endl; //添加测量值 smartFactor->add(measurement,X(i)); //更新 ISAM2Result result=isam.update(graph,initialEstimate); result.print(); cout << "Detailed results:" << endl; //遍历键值信息 for(auto keyedStatus:result.detail->variableStatus){ 
    const auto& status=keyedStatus.second; PrintKey(keyedStatus.first); cout << " {" << endl; cout<<"变量是否被被重新限制（重新线性化、添加新值、或者在被更新的根目录路径上）:"<<status.isReeliminated<<endl; cout<<"是否超过阈值(在平滑线性时超过是否阈值):"<<status.isAboveRelinThreshold<<endl; cout<<"是否被设计被重新线性化:"<<status.isRelinearized<<endl; cout<<"是否被观测到（仅仅与添加的新元素相关):"<< status.isObserved << endl; cout<<"新值:"<<status.isNew<<endl; cout<<"是否为根团："<<status.inRootClique<<endl; cout << " }" << endl; } Values currentEstimate=isam.calculateBestEstimate();//calculateBestEstimate使用所有值进行回带 currentEstimate.print("Current estimate:"); boost::optional<Point3> pointEstimate=smartFactor->point(currentEstimate); if (pointEstimate) { 
    cout << *pointEstimate << endl; } graph.resize(0); initialEstimate.clear(); } return 0; } 

2. ISAM2模板

#include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     / *@brief 创建8个点 *@return 返回vector 
   
     points 长度为8 */ 
    std::vector<gtsam::Point3> createPoints() { 
    // Create the set of ground-truth landmarks std::vector<gtsam::Point3> points; points.push_back(gtsam::Point3(10.0,10.0,10.0)); points.push_back(gtsam::Point3(-10.0,10.0,10.0)); points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); points.push_back(gtsam::Point3(10.0,-10.0,10.0)); points.push_back(gtsam::Point3(10.0,10.0,-10.0)); points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); return points; } / *@brief *@param init 初始化pose，队列的第一个值，之后每一个都会在pose之上旋转delta(Pose3) *@param delta 旋转角度 *@param steps 默认为8 返回Pose3的长度 *@return vector 
   
     */ 
    std::vector<gtsam::Pose3> createPoses( const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)), const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))), int steps = 8) { 
    // Create the set of ground-truth poses // Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center std::vector<gtsam::Pose3> poses; int i = 1; poses.push_back(init); for(; i < steps; ++i) { 
    poses.push_back(poses[i-1].compose(delta)); } return poses; }; using namespace std; using namespace gtsam; int main(int argc, const char** argv) { 
    //相机内参K，无畸变 Cal3_S2::shared_ptr K(new Cal3_S2(50.0,50.0,0,50.0,50.0)); //模拟数据 auto measurementnoise=noiseModel::Isotropic::Sigma(2,1.0); vector<Point3> points=createPoints(); vector<Pose3> poses=createPoses(); //与ISAM1不同，ISAM1进行周期性批量处理，ISAM2进行部分平滑线性 ISAM2Params parameters; parameters.relinearizeThreshold=0.01; //重新线性化平滑值 parameters.relinearizeSkip=1; ISAM2 isam(parameters); NonlinearFactorGraph graph; Values initialEstimate; for(size_t i=0;i<poses.size();i++){ 
    //测量值 for(size_t j=0;j<points.size();j++){ 
    SimpleCamera camera(poses[i],*K); Point2 measurement=camera.project(points[j]); graph.emplace_shared<GenericProjectionFactor<Pose3,Point3,Cal3_S2> >(measurement,measurementnoise,Symbol('x',i),Symbol('l',j),K); } Pose3 kDeltaPose(Rot3::Rodrigues(-0.1,0.2,0.25),Point3(0.05,-0.10,0.20)); initialEstimate.insert(symbol('x',i),poses[i]*kDeltaPose); if(i==0){ 
    auto kPosePrior=noiseModel::Diagonal::Sigmas(Vector6(0.3,0.3,0.3,0.1,0.1,0.1)); graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x',0),poses[0],kPosePrior); noiseModel::Isotropic::shared_ptr kPointPrior=noiseModel::Isotropic::Sigma(3,0.1); graph.emplace_shared<PriorFactor<Point3> >(Symbol('l',0),points[0],kPointPrior); Point3 kDeltaPoints(-0.25,0.20,0.15); for(size_t j=0;j<points.size();j++){ 
    initialEstimate.insert<Point3>(Symbol('l',j),points[j]+kDeltaPoints); } } else{ 
    //每次update 都会进行一次非线性迭代求解，如果需要更加准确的数据，可以调用多个，但会消耗额外的时间 isam.update(graph,initialEstimate); isam.update(); Values currentEstimate=isam.calculateBestEstimate(); cout << "" << endl; cout << "Frame " << i << ": " << endl; currentEstimate.print("Current estimate: "); graph.resize(0); initialEstimate.clear(); } } return 0; } 

4. IMU预计分与相机代矫正位姿

//IMU 尝试使用IMU预计分与3d相机量联合优化位姿，用于处理无特征的帧 //假设 相机与IMU之间的坐标系转换是已知的 #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     #include  
     using namespace std; using namespace gtsam; using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; struct IMUhelper{ 
    IMUhelper(){ 
    { 
    auto gaussian=noiseModel::Diagonal::Sigmas(Vector6(5.0e-2,5.0e-2,5.0e-2,5.0e-3,5.0e-3,5.0e-3)); auto huber=noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345),gaussian); biasNoiseModel=huber; } { 
    auto gassian=noiseModel::Isotropic::Sigma(3,0.01); auto huber=noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345),gassian); velocityNoiseModel = huber; } //定义初始位置重力常量 auto p = boost::make_shared<PreintegratedCombinedMeasurements::Params>( Vector3(0.0, 9.8, 0.0)); //连续加速度白噪声 p->accelerometerCovariance=I_3x3 *pow(0.0565,2.0); //积分连续不确定性 p->integrationCovariance=I_3x3*1e-9; //陀螺仪连续白噪声 p->gyroscopeCovariance=I_3x3*pow(4.0e-5,2.0); //加速度残差 连续白噪声 p->biasAccCovariance=I_3x3*pow(0.00002,2.0); //陀螺仪残差 连续白噪声 p->biasAccOmegaInt=Matrix::Identity(6,6)*1e-5; //Imu imu自身坐标与导航坐标系的旋转 Rot3 iRb(0.036129, -0., 0.035207, 0.045417, -0.033553, -0., 0., 0.037670, 0.044147); //Imu imu自身坐标与导航坐标系的位置误差 Point3 iTb(0.03,-0.025,-0.06); //左边相机与imu的位姿变换 p->body_P_sensor = Pose3(iRb, iTb); //初始位姿 Rot3 prior_rotation = Rot3(I_3x3); Pose3 prior_pose(prior_rotation, Point3(0, 0, 0)); //相机坐标系下 imu矫正的残差 Vector3 acc_bias(0.0, -0.0, 0.0); Vector3 gyro_bias(-0.00, -0.00, -0.00); priorImuBias = imuBias::ConstantBias(acc_bias, gyro_bias); //初始化导航向量 prevState=NavState(prior_pose,Vector3::Zero()); propState=prevState; preintegrated=new PreintegratedCombinedMeasurements(p,priorImuBias); } imuBias::ConstantBias priorImuBias; //初始误差 noiseModel::Robust::shared_ptr velocityNoiseModel; //速度噪声 noiseModel::Robust::shared_ptr biasNoiseModel; //残差噪声模型 NavState prevState; //之前的导航状态 NavState propState; //当前导航状态 imuBias::ConstantBias prevBias; //之前的残差 PreintegratedCombinedMeasurements* preintegrated; //之前的预积分 }; int main(int argc, const char** argv) { 
    string file="/home/n1/notes/gtsam/ISAM_IMU_CAMERA/ISAM2_SmartFactorStereo_IMU.txt"; ifstream in(file); //相机内参 double fx = 822.37; double fy = 822.37; double cx = 538.73; double cy = 579.10; double baseline = 0.372; //初始化相机 Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline)); ISAM2Params parameters; parameters.relinearizeThreshold=0.1; ISAM2 isam(parameters); //创建因子图 std::map<size_t,SmartStereoProjectionPoseFactor::shared_ptr>smartFactors;//构造容器 NonlinearFactorGraph graph; Values InitialEstimate; IMUhelper imu; //初始值处理 //Pose初始值 auto priorPoseNoise=noiseModel::Diagonal::Sigmas( Vector6(0.1,0.1,0.1,0.1,0.1,0.1)); graph.emplace_shared<PriorFactor<Pose3> >(X(1), Pose3::identity(),priorPoseNoise); //从0开始 InitialEstimate.insert(X(0),Pose3::identity()); //imu初始值和噪声模型 graph.add(PriorFactor<imuBias::ConstantBias>(B(1),imu.priorImuBias,imu.biasNoiseModel)); InitialEstimate.insert(B(0),imu.priorImuBias); //加速度 假设为恒定的 graph.add(PriorFactor<Vector3>(V(1),Vector3(0,0,0),imu.velocityNoiseModel)); InitialEstimate.insert(V(0),Vector3(0, 0, 0)); int lastframe=1; int frame; while(1){ 
    char line[1024]; in.getline(line,sizeof(line)); stringstream ss(line); char type; ss>> type >> frame; if(frame !=lastframe||in.eof()){ 
    cout<<"当前帧:"<<lastframe<<endl; InitialEstimate.insert(X(lastframe),Pose3::identity()); InitialEstimate.insert(V(lastframe),Vector3(0,0,0)); InitialEstimate.insert(B(lastframe),imu.priorImuBias); CombinedImuFactor imuFactor(X(lastframe-1),V(lastframe - 1), X(lastframe),V(lastframe),B(lastframe-1), B(lastframe),*imu.preintegrated); graph.add(imuFactor);//添加新的的变量 isam.update(graph,InitialEstimate);//更新 Values currentEstimate=isam.calculateEstimate();//当前值 //获得当前估计值 predict（当前状态，当前残差） imu.propState=imu.preintegrated->predict(imu.prevState,imu.prevBias);// //更新状态 imu.prevState=NavState(currentEstimate.at<Pose3>(X(lastframe)),currentEstimate.at<Vector3>(V(lastframe))); //更新残差 imu.prevBias=currentEstimate.at<imuBias::ConstantBias>(B(lastframe)); //更新当前预计分值 imu.preintegrated->resetIntegrationAndSetBias(imu.prevBias); //重塑大小，删除所有已存在的。 //当新的size小于原始size，先删除新的值，大于添加null graph.resize(0); InitialEstimate.print(); InitialEstimate.clear(); if(in.eof()){ 
    break; } } if(type=='i'){ 
   //Imu数据 double ax,ay,az; double gx,gy,gz; double dt=1/800.0;//Imu 800Hz ss>>ax>>ay>>az; ss>>gx>>gy>>gz; Vector3 acc(ax,ay,az); Vector3 gyro(gx,gy,gz); //测量值 imu.preintegrated->integrateMeasurement(acc,gyro,dt); } else if(type=='s'){ 
   //双目相机 int landmark; double xl,xr,y; ss>>landmark>>xl>>xr>>y; if (smartFactors.count(landmark) == 0) { 
    auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0); //SmartProjectionParams 双目相机的smartFactor // 该类实现了相机类贝叶斯树的线性化和degeneracy // 默认参数： // LinearizationMode linMode = HESSIAN, HESSIAN：2阶海森矩阵线性化 // DegeneracyMode degMode = IGNORE_DEGENERACY,：degeneracy（退化）模式 // IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY // bool throwCheirality = false, 如果为真重新抛出Cheirality（与景深相关）异常 // bool verboseCheirality = false, 输出Cheirality异常 // double retriangulationTh = 1e-5 重新三角化范围 SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY); //SmartStereoProjectionPoseFactor // 假设每一个相机内参都有自己独立参数且已经被矫正 //参数： // const SharedNoiseModel& sharedNoiseModel：噪声模型 // const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),参数 // const boost::optional 
   
     body_P_sensor = boost::none 初始位姿 
    smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr( new SmartStereoProjectionPoseFactor(gaussian, params)); graph.push_back(smartFactors[landmark]); } //xl 左边相机的像素x //xr 右边相机对应像素的x //y 矫正后的y smartFactors[landmark]->add(StereoPoint2(xl,xr,y),X(frame),K); } else { 
    throw runtime_error("读取错误: " + string(1, type)); } lastframe = frame; } return 0; };