gtsam 学习十一（ISAM2 实践）

最新推荐文章于 2024-03-31 20:01:49 发布
最新推荐文章于 2024-03-31 20:01:49 发布
阅读量3.4k 收藏 24
点赞数 5
原文链接：https://editor.csdn.net/md?not_checkout=1&articleId=114630593
版权

来自gtam/examples/

  1. ISAM1模板
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <vector>
/**
*@brief 创建8个点
*@return 返回vector<Point3> 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<Pose3>
 */
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;
}
  1. ISAM2 基本使用
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/ISAM2Params.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam/inference/Key.h>
#include <iostream>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <vector>

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;
}
  1. ISAM2模板
#include <gtsam/geometry/Point2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/nonlinear/ISAM2Params.h>
#include <vector>
/**
*@brief 创建8个点
*@return 返回vector<Point3> 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<Pose3>
 */
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;
}
  1. IMU预计分与相机代矫正位姿
//IMU 尝试使用IMU预计分与3d相机量联合优化位姿，用于处理无特征的帧
//假设 相机与IMU之间的坐标系转换是已知的
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

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.998727, 0.035207,
             0.045417, -0.033553, -0.998404,
             0.998315, 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.0942015, 0.0);
    Vector3 gyro_bias(-0.00527483, -0.00757152, -0.00469968);
    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<Pose3> 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;
};
磊磊哈哈
关注
  • 5
    点赞
  • 踩
  • 24
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
专栏目录
iSAM2 Incremental Smoothing and Mapping Using the Bayes Tree.pdf
05-12
iSAM2 Incremental Smoothing and Mapping Using the Bayes Tree; 因子图大佬论文，可以看看
gtsam 学习十（ISAM2 理论）
weixin_37781153的博客
03-08 6612
翻译自：iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree 摘要 提出了一种新型的贝斯树处理稀疏矩阵，在转化为因子图，可以更好的求解平方根信息和映射问题。 文中提出了三个概念 使用概率密度项进行矩阵分解 介绍如何将矩阵分解转换为贝叶斯数和条件概率密度 基于贝叶斯树提供了一种新的系数非线性优化：ISAM2（进行重新排序和重新线性化，避免了批量更新） 问题描述 文章聚焦与如何用增量实时解决非线性问题， 利用增量即新的测量值实时更新估
gtsam::Pose3的compose()函数作用
最新发布
m0_60346726的博客
03-31 273
gtsam::Pose3的compose()函数作用
ISAM2运行流程
人无远虑，必有近忧
01-28 1683
1. 算法流程 2. update函数运行流程 updateDelta: Update delta if we need it to check relinearization later update.pushBackFactors: Add any new factors 为每一个新的factor产生一个索引，把新factor装入nonlinearFactors中 把需要移除的factor从nonlinearFactors和linearFactors中同时移除，把从nonlinea.
【GTSAM】GTSAM学习资源汇总
热门推荐
王博（Kings）的博客
08-05 1万+
GTSAM 是一个在机器人领域和计算机视觉领域用于平滑（smoothing）和建图（mapping）的C++库。它与g2og2o不同的是，g2og2o采用稀疏矩阵的方式求解一个非线性优化问题，而GTSAM是采用因子图（factor graphs）和贝叶斯网络（Bayes networks）的方式最大化后验概率。 泡泡机器人的公开课： 【泡泡机器人公开课】第五十六课：gtsam解析 by 董...
slam_backend：用于2D姿势图SLAM的基于iSAM2的后端接口
02-04
slam_backend：用于2D姿势图SLAM的基于iSAM2的后端接口
SLAM-using-GTSAM：无人机的同时映射和本地化框架
02-03
SLAM-using-GTSAM：无人机的同时映射和本地化框架
ISAM.rar_isam
09-23
c++高手学习必备,c++学习者的有利帮手,快下哦,很好的
Isam.rar_isam
09-21
WEB页面上显示包含图象的数据库信息编程代码
iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree 阅读笔记
ziliwangmoe的博客
01-24 2237
优化这样一个方程，其实就是解这个最小二乘问题，其实就是求A的逆。 这样一个问题包括几个可研究的部分 怎么把各种观测值和变量之间的关系，变成A矩阵 为啥解决一个概率的问题，但都是在处理矩阵的问题呢？ 因为我们都是解决的高斯分布的随机变量。高斯分布用一个协方差矩阵和均值向量就能完全表示了。 当我们使用矩阵变换手段，把一个矩阵变成另外一个矩阵，其实也是把一个随机变量变成另外一个随机变量了...
gtsam的简介
02-11
slam中的后端优化，效率较高的一种优化方式，在VIO或者VI-SLAM中很常见
iSAM2阅读笔记
Pancheng1的博客
07-11 3191
因子图-gtsam
余辉亮的学习笔记
02-12 3679
闲着写下关于gtsam的内容 更新中。。。。。。
iSAM2 部分状态更新算法 (I - 原理解读)
Robotics_Notes
03-21 924
Title: iSAM2 部分状态更新算法 (I-原理解读) 文章目录I. 前言II. 部分状态的更新 (Partial State Update)III. 因子图的线性化 (Linearization of Factor Grahps)1. 简单实例的设定2. 一个线性化计算3. 其他线性化计算4. 状态更新量说明IV. 部分 QR 分解实现变量消元 (Eliminating Variables Using Partial QR)1. 首变量的消元2. 中间变量的消元3. 最末变量的消元V. 状态更新的
GTSAM中ISAM2和IncrementalFixedLagSmoother说明
人无远虑，必有近忧
06-15 1099
GTSAM
因子图优化原理（iSAM、iSAM2论文解析）
weixin_45112559的博客
08-04 1万+
因子图优化原理（iSAM、iSAM2论文解析）
GTSAM:使用gtsam的C++
qq_23418081的博客
06-15 2879
slam问题可以看做是一个贝叶斯网络的问题。 根据各个观测值估计机器人的位置、姿态和和路标点的位置。 第一个问题，这个观测值是如何转化为概率的，还是观测值和概率之间的关系问题。 使用gtsam的C++例子 1.build factor graph 2.give initial values(this is a little bit tricky and highly application-related, design your strategybased on your application!) 3
从零开始搭二维激光SLAM --- 基于gtsam的后端优化的代码实现
李想的博客
11-22 3679
上一篇文章我们分析了如何使用ceres进行位姿图的优化. 1 gtsam简介 2 基于gtsam的后端优化的代码讲解 3 运行 3.1 依赖 这篇文章的代码是需要依赖 4.0.2 版本的gtsam库, 如果没装gtsam的需要先安装一下. 我将gtsam库的安装包放在了 工程Creating-2D-laser-slam-from-scratch/TrirdParty文件加内, 可以直接解压安装. 安装的方法可以看一下 install_dependence.sh 脚本中的安装指令, 也可以直接执行这个脚本进行
arcgis找不到可安装的isam
06-01
这可能是由于您安装的 Microsoft Access 数据库引擎缺失或不正确引起的。您可以尝试重新安装 Microsoft Access 数据库引擎，或者安装最新版本的 Microsoft Office，这将包含必要的驱动程序。另外，您还可以尝试将您的数据文件转换为另一种格式，例如 CSV 或 Excel，然后尝试在 ArcGIS 中导入这些文件。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值