Gtsam 학습 노트

Gtsam 학습 노트
글 목록

Gtsam 학습 노트

cmake 도입

인자 인자 인자 인자 인자

미리 정 의 된 factor

생 성 인자

초기 값 정의

소음 정의

최적화 방법

GaussNewton 법

LevenbergMarquardt 법

가장자리 화 marginal

g2o 파일 읽 기

iSAM 업데이트 과정

LevenbergMarquardtOptimizer

cmake 도입

#       ，        ，         
find_package(Boost COMPONENTS thread filesystem date_time system REQUIRED)
FIND_PACKAGE(GTSAM REQUIRED)

#            ，        
INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${GTSAM_INCLUDE_DIR})
INCLUDE_DIRECTORIES("/usr/include/eigen3")

add_executable(gtsam_test main.cpp)
#     
target_link_libraries(gtsam_test ${Boost_LIBRARIES} -lgtsam -ltbb)
install(TARGETS gtsam_test RUNTIME DESTINATION bin)

clion인식 이 안 되면 다시 시작 하 세 요.새로 고침 이 안 된 것 같 습 니 다.
인자 인자
미리 정 의 된 factor

   
#include  //    ，    ，    
#include  //    ，    
//     
NonlinearFactorGraph graph;
//          ：
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
//           
//       
graph.emplace_shared >(1, 2, Pose2(2, 0, 0     ), model);
			//     ^ ：      ^ 		key 1、2  ^    			^     
//       
graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise);

생 성 인자

  Rot2 prior = Rot2::fromAngle(30 * degree);
  prior.print("goal angle"); 
  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
  Symbol key('x',1); //   key    label
  PriorFactor factor(key, prior, model);

초기 값 정의
그림 에서 모든 변수 에 초기 값 을 부여 해 야 합 니 다.

  Values initialEstimate; //       
  initialEstimate.insert(1, Pose2(0.5, 0.0,  0.2   )); //       arg1：      arg2:      

잡음 정의
gtsam 의 소음 은 여러 가지 유형 이 있 습 니 다.namespace NM = gtsam::noiseModel;

// Set Noise parameters
Vector priorSigmas = Vector3(1,1,M_PI);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
const NM::Base::shared_ptr //    
  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior      
  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust       
  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust 

최적화 방법
가우스 뉴턴 법
도입#include

GaussNewtonParams parameters;//    
parameters.relativeErrorTol = 1e-5; //              
parameters.maxIterations = 100; //      
//            arg：		      				    
GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
//              
Values result = optimizer.optimize();

LevenbergMarquardt 법
도입#include

LevenbergMarquardtOptimizer optimizer(graph,initialEstimate);
Values result=optimizer.optimize(); 

가장자리 화 marginal

    Marginals marginals(graph,result);
    for (int j = 1; j < 4; ++j) {
        boost::format fmt("
 x%1% covariance: 
 %2% 
");
        fmt%j%marginals.marginalCovariance(j); //          
        cout<

g2o 파일 읽 기

#include  //     
// 
NonlinearFactorGraph::shared_ptr graph1;
Values::shared_ptr initialEstamate1;
string g2ofile=findExampleDataFile("noisyToyGraph.txt");
bool is3D =false;
boost::tie(graph1,initialEstamate1)=readG2o(g2ofile,is3D);
//  g2o  
writeG2o(graph,result,"graph2g2o.g2o"); //args:     ，    ，     

iSAM 업데이트 과정
파일 ISAM2.cpp 의 update 함수 에서

새로운 인자 추가Impl::AddFactorsStep1F a c t o r s:=F a c t o r F a c t o r′Factors:=Factor Factor:=Factor Factor′

   //    
   ISAM2Result result;
   isam.update(graph, initialEstimate);
   ISAM2Params params_; //   ISAM2          。
   	//  parameters.relinearizeThreshold = 0.01;
     	// parameters.relinearizeSkip = 1;
   Impl //Internal implementation functions       
   useUnusedSlots //slot   

새로운 변 수 를 초기 화 합 니 다Impl::AddVariablesΘ : = Θ ∪ Θ n e w . \Theta:=\Theta\cup\Theta_{new}. Θ:=Θ∪Θnew​.

태그 선형 업데이트

표기 변 화 량 이 한도 값 보다 많은 keyβ : J = { Δ j ∈ Δ ∣ Δ j ≥ β } . \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. β:J={Δj​∈Δ∣Δj​≥β}.

변수 와 관련 된 모든 작은 단체 clique 와 그들의 모든 부모 노드

를 표시 합 니 다.

선형 화 점 이 표 시 된 변 수 를 업데이트 합 니 다.Θ J : = Θ J + Δ J . \Theta_{J}:=\Theta_{J}+\Delta_{J}. ΘJ​:=ΘJ​+ΔJ​.

선형 화 된 새로운 인자

베 이 루스 수의 나무 꼭대기 재 정리

LevenbergMarquardtOptimizer
Levenberg Marquardt Optimizer 계승NonlinearOptimizer의optimize->defaultOptimizeiterate();(이 함 수 는 하위 클래스 에서 이 루어 진 것 이지 부모 클래스 에서 이 루어 진 것 이 아니다)

Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
virtual const Values& optimize() { defaultOptimize(); return values(); }

iterate 에서:KaTeX parse error:Undefined control sequence:\\f at position 2:\\̲f̲ ̲Ax-b \approx h(…

1、
GaussianFactorGraph::shared_ptr linear = linearize();
             ：NonlinearFactor：：linearize（）
2、
whitenedError