來自gtam/examples/

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 landmarksstd::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 centerstd::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,yawnoiseModel::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 l0noiseModel::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; }

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; //是否計算線性化誤差默認falseparameters.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; }

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 landmarksstd::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 centerstd::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; }

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，先刪除新的值，大于添加nullgraph.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 800Hzss>>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 矯正后的ysmartFactors[landmark]->add(StereoPoint2(xl,xr,y),X(frame),K);}else {throw runtime_error("讀取錯誤: " + string(1, type));}lastframe = frame;}return 0; };

總結

以上是生活随笔為你收集整理的gtsam 学习十一（ISAM2 实践）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。