GTSAM在windows下安装，配置，简单调试

  • BOOST安装配置
  • GTSAM安装配置
    • 安装
    • 配置
  • GTSAM简单调试
    • 代码分析1
    • 代码分析2
    • 模型分析和结果展示

BOOST安装配置

链接: boost官网下载.
以boost_1_74_0为例
在命令行中无参数运行：

.\bootstrap.bat

在命令行中无参数运行:

.\b2.exe

编译完成后会在命令行显示boost头文件和库文件的搜索路径:

The Boost C++ Libraries were successfully built!The following directory should be added to compiler include paths:D:\Learning\GTSAM_win\3rd_party\boost_1_74_0\boost_1_74_0The following directory should be added to linker library paths:D:\Learning\GTSAM_win\3rd_party\boost_1_74_0\boost_1_74_0\stage\libPS D:\Learning\GTSAM_win\3rd_party\boost_1_74_0\boost_1_74_0>

打开文件：

\boost_1_74_0\boost\serialization\optional.hpp

注释掉第97~100行，否则构建gtsam过程中会报告错误。

template<class T>
struct version<boost::optional<T> > {BOOST_STATIC_CONSTANT(int, value = 1);
};

GTSAM安装配置

安装

下载gtsam

git clone -b 4.0.3 https://github.com/borglab/gtsam.git

修改gtsam\CMakeLists.txt文件，在167行之上增加下面两行代码，添加boost头文件和库文件的搜索路径，否则cmake找不到boost：

//add two lines
SET(CMAKE_INCLUDE_PATH ${CMAKE_INCLUDE_PATH} "D:/Learning/GTSAM_win/3rd_party/boost_1_74_0/boost_1_74_0")
SET(CMAKE_LIBRARY_PATH ${CMAKE_LIBRARY_PATH} "D:/Learning/GTSAM_win/3rd_party/boost_1_74_0/boost_1_74_0/stage/lib")
//
find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS})

在gtsam目录下新建build文件夹，打开CMake进行配置和生成

tips：选x64而非默认的win32，否则会出现eigen的对齐问题，可能和gtsam自带的3rd-party里的eigen的版本有关。
编译all_build和install，VS就会将GTSAM安装到C:\Program Files\GTSAM目录下：

配置

以官方example中Pose3Localization为例：
设置工作目录至data文件夹下：

GTSAM简单调试

代码分析1

int main(const int argc, const char* argv[]) {// Read graph from filestring g2oFile;if (argc < 2)g2oFile = findExampleDataFile("pose3Localizationexample.txt");elseg2oFile = argv[1];NonlinearFactorGraph::shared_ptr graph;Values::shared_ptr initial;bool is3D = true;//graph的赋值详见“代码分析2”boost::tie(graph, initial) = readG2o(g2oFile, is3D);// Add prior on the first key//加入初始位置（引入以第一个pose为基准的绝对坐标系）auto priorModel = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());Key firstKey = 0;for (const Values::ConstKeyValuePair& key_value : *initial) {std::cout << "Adding prior to g2o file " << std::endl;firstKey = key_value.key;graph->addPrior(firstKey, Pose3(), priorModel);break;}
//采用高斯-牛顿法求解std::cout << "Optimizing the factor graph" << std::endl;GaussNewtonParams params;params.setVerbosity("TERMINATION");  // show info about stopping conditionsGaussNewtonOptimizer optimizer(*graph, *initial, params);Values result = optimizer.optimize();std::cout << "Optimization complete" << std::endl;std::cout << "initial error=" << graph->error(*initial) << std::endl;std::cout << "final error=" << graph->error(result) << std::endl;if (argc < 3) {result.print("result");} else {const string outputFile = argv[2];std::cout << "Writing results to file: " << outputFile << std::endl;NonlinearFactorGraph::shared_ptr graphNoKernel;Values::shared_ptr initial2;boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);writeG2o(*graphNoKernel, result, outputFile);std::cout << "done! " << std::endl;}// Calculate and print marginal covariances for all variablesMarginals marginals(*graph, result);for (const auto& key_value : result) {auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);if (!p) continue;std::cout << marginals.marginalCovariance(key_value.key) << endl;}getchar();return 0;
}

输入数据：pose3Localizationexample.txt，数据格式详见代码分析2

VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024
VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488
EDGE_SE3:QUAT 0 1   1.001367 0.015390 0.004948   0.190253 0.283162 -0.392318 0.854230   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
EDGE_SE3:QUAT 1 2   0.523923 0.776654 0.326659   0.311512 0.656877 -0.678505 0.105373   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
EDGE_SE3:QUAT 2 3   0.910927 0.055169 -0.411761   0.595795 -0.561677 0.079353 0.568551   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
EDGE_SE3:QUAT 3 4   0.775288 0.228798 -0.596923   -0.592077 0.303380 -0.513226 0.542221   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000

代码分析2

factor的输入格式，输入数据为四元数的格式表示（EDGE_SE3:QUAT）：

  std::vector<BetweenFactor<Pose3>::shared_ptr> factors;while (!is.eof()) {char buf[LINESIZE];is.getline(buf, LINESIZE);istringstream ls(buf);string tag;ls >> tag;if (tag == "EDGE3") {Key id1, id2;double x, y, z, roll, pitch, yaw;ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;Matrix m(6, 6);for (size_t i = 0; i < 6; i++)for (size_t j = i; j < 6; j++) ls >> m(i, j);SharedNoiseModel model = noiseModel::Gaussian::Information(m);factors.emplace_back(new BetweenFactor<Pose3>(id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model));}if (tag == "EDGE_SE3:QUAT") {Key id1, id2;double x, y, z, qx, qy, qz, qw;ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;Matrix m(6, 6);for (size_t i = 0; i < 6; i++) {for (size_t j = i; j < 6; j++) {double mij;ls >> mij;m(i, j) = mij;m(j, i) = mij;}}Matrix mgtsam(6, 6);mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3);  // cov rotationmgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0);  // cov translationmgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3);  // off diagonalmgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonalSharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);//将四元数标准化为3*3的旋转矩阵auto R12 = NormalizedRot3(qw, qx, qy, qz);if (sampler) {R12 = R12.retract(sampler->sample());}//加上1*3的位移矩阵为RT矩阵factors.emplace_back(new BetweenFactor<Pose3>(id1, id2, Pose3(R12, {x, y, z}), model));}}

pose的输入格式，输入数据为四元数的格式表示（VERTEX_SE3:QUAT）：

while (!is.eof()) {char buf[LINESIZE];is.getline(buf, LINESIZE);istringstream ls(buf);string tag;ls >> tag;if (tag == "VERTEX3") {Key id;double x, y, z, roll, pitch, yaw;ls >> id >> x >> y >> z >> roll >> pitch >> yaw;poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}));}if (tag == "VERTEX_SE3:QUAT") {Key id;double x, y, z, qx, qy, qz, qw;ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;poses.emplace(id, Pose3(NormalizedRot3(qw, qx, qy, qz), {x, y, z}));}}

模型分析和结果展示


x为pose，输入的为初值；f为betweenfactor，定义如下。
主要看如何定义需要最小化的残差evaluateError。

    /** vector of errors */Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =boost::none, boost::optional<Matrix&> H2 = boost::none) const {T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)// manifold equivalent of h(x)-z -> log(z,h(x))
#ifdef SLOW_BUT_CORRECT_BETWEENFACTORtypename traits<T>::ChartJacobian::Jacobian Hlocal;Vector rval = traits<T>::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0);if (H1) *H1 = Hlocal * (*H1);if (H2) *H2 = Hlocal * (*H2);return rval;
#elsereturn traits<T>::Local(measured_, hx);
#endif}

运行结果如下，5个位置优化后的结果以及errors优化前后的值；

Adding prior to g2o file
Optimizing the factor graph
converged
errorThreshold: 5.0187e-20 <? 0
absoluteDecrease: 9.73604311581e-08 <? 1e-05
relativeDecrease: 0.999999999999 <? 1e-05
iterations: 1 >? 100
Optimization complete
initial error=9.73604311582e-08
final error=5.01870103033e-20
resultValues with 5 values:
Value 0: (class gtsam::Pose3) R:
[1,  2.28287480605e-24,  2.15832506594e-23;-2.28287480605e-24,                  1,  1.24171131055e-23;-2.15832506594e-23, -1.24171131055e-23,                  1
]
[-5.76827616676e-22, -5.18767533685e-22, -7.13512232555e-22]';Value 1: (class gtsam::Pose3) R:
[0.531811375952,  0.778005050656,  0.334491855747;-0.562515204414,   0.61978047231, -0.547219161714;-0.633050791968,  0.102860620728,  0.767245975872
]
[1.001367, 0.01539, 0.004948]';Value 2: (class gtsam::Pose3) R:
[-0.39734090331, -0.0718430546753,  -0.914854514145;0.91294942536,  0.0700534823995,   -0.40201474642;0.0929707120121,  -0.994952805463,  0.0377539615052
]
[1.99350072124, 0.023276265353, 0.00379304568313]';Value 3: (class gtsam::Pose3) R:
[-0.770836879634,   -0.256388817292,    0.583159737433;-0.00992299502881,   -0.910487908304,   -0.413416621584;0.636955288199,   -0.324463469746,    0.699293513223
]
[2.00429006433, 1.02430532112, 0.0180464171948]';Value 4: (class gtsam::Pose3) R:
[0.174491373111,  -0.64969410537, -0.739898864817;0.715731876593,   0.59973837325, -0.357829239276;0.676225289041, -0.467131087704,  0.569655953504
]

errors为所有factor的error的累加，一个factor的error的定义如下；
这里用的统计指标为马氏距离，而残差由evaluateError定义。

double NoiseModelFactor::error(const Values& c) const {if (active(c)) {//计算残差const Vector b = unwhitenedError(c);check(noiseModel_, b.size());if (noiseModel_)return noiseModel_->loss(noiseModel_->squaredMahalanobisDistance(b));elsereturn 0.5 * b.squaredNorm();} else {return 0.0;}
}

virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {if(this->active(x)) {const X1& x1 = x.at<X1>(keys_[0]);const X2& x2 = x.at<X2>(keys_[1]);if(H) {return evaluateError(x1, x2, (*H)[0], (*H)[1]);} else {return evaluateError(x1, x2);}} else {return Vector::Zero(this->dim());}}

GTSAM在windows下安装，配置，简单调试相关推荐

  1. python开发工具及环境配置_python_在windows下安装配置python开发环境及Ulipad开发工具...

    最近开始学习Python,在网上寻找一下比较好的IDE.因为以前用C#做开发的,用Visual Studio作为IDE,鉴于用惯了VS这么强大的IDE,所以对IDE有一定的依赖性. Python的ID ...

  2. 在windows下安装配置Ulipad

    在windows下安装配置Ulipad 今天推荐一款轻便的文本编辑器Ulipad,用来写一些小的Python脚本非常方便. Ulipad下载地址: https://github.com/limodou ...

  3. Windows下安装配置SubVersion的简明步骤

    Windows下安装配置SubVersion的简明步骤 [使用的安装程序和文档说明] svn-1.4.0-setup.exe:Subversion服务端1.4.0安装程序: SubService.ra ...

  4. Windows下安装配置ant

    1.ant安装 请从官网下载ant的*.zip格式的安装包, Windows建议下载*.zip版本, Linux建议下载*.gz版本. 2.配置环境变量 解压之后,在Windows中配置环境变量, 在 ...

  5. tflearn教程_环境部署——Windows下安装配置tensorflow和tflearn

    原标题:环境部署--Windows下安装配置tensorflow和tflearn 本例依赖Windows 7 x64位系统,由于Tensorflow依赖于Python,以及大量的相关依赖包,为了免除大 ...

  6. 在 windows 下安装和简单配置 trac 0.12

    赖勇浩(http://laiyonghao.com) Trac是一个基于Web的,轻量级的项目管理工具,它使用python语言编写,官网:http://trac.edgewall.org/.它集成了增 ...

  7. OpenEye | Windows下安装配置OpenEye

    OpenEye简介 OpenEye是一款用于药物先导发现和改造的专业工具,它不仅考虑了严格的科学意义.还兼顾计算速度.可拓展性以及平台可用性.OpenEye药物设计软件聚焦于先导物识别与优化,尤其擅长 ...

  8. windows下安装配置cwrsync

    CwRsync是基于cygwin平台的rsync软件包,支持windows对windows.windows对Linux.Linux对 windows高效文件同步.由于CwRsync已经集成了cygwi ...

  9. RabbitMq学习笔记001---RabbitMq在Windows下安装配置

    rabbitMQ是一个在AMQP协议标准基础上完整的,可服用的企业消息系统.它遵循Mozilla Public License开源协议,采用 Erlang 实现的工业级的消息队列(MQ)服务器,Rab ...

最新文章

  1. 取代Python？Rust凭什么
  2. Qtcreator配置Ros环境
  3. 虚拟机卡在登录界面卡住了_手机都10核了 为何还是照样卡？
  4. Ubuntu安装kaldi
  5. 离散数学及其应用第八版纠正（p29）：关于n皇后问题的SAT表示
  6. SKU与SPU的区别与联系
  7. 计算机无法创建新文件夹,使用win10系统时，发现无法新建文件夹怎么办，学会这招即可修复...
  8. PHP实现时间轴函数（个性化时间）
  9. Rayson API 框架分析系列之3：RSON序列化格式
  10. js摇号程序_车管所怎么摇号流程及查询
  11. Kruskal重构树 学习笔记
  12. C++寻找数组最大值和最小值
  13. SAP汇率损益处理方法
  14. windows文本转语音调用
  15. 解释#define的用法
  16. windows找不到gpedit.msc请确定文件名
  17. java 对Outlook保存的.Msg格式文件解析
  18. VC++6.0 用gSoap客户端访问WebService
  19. 方舟服务器维护公告11月19日,《方舟指令》11月29日维护公告
  20. KMP算法DNA的病毒检测

热门文章

  1. 意外收到快钱送的现金了
  2. 第二周 Turtle库的使用
  3. 海康视频VTM流监控浏览器实时播放调试总结
  4. 测试-答对5道题的人是天才，答对4道的是帅才，答对3道的是将才，答对2道的是奇才，答对1道的是人才...
  5. BAT的“说散就散”
  6. 基于python fitz的pdf文件处理器--已开源
  7. PTA题目集python判断题
  8. el-checkbox为甚点击一个其他的全部都选中了，或者为什么选中的绑定的获取的不是label而是true或者false呢?获取label但是不显示
  9. 区块链技术1---密码学基础
  10. auto.js B0021 图片查找 订阅功能 2021-10-07