gtsam学习笔记3d SLAM(三)

前言：

将example中所有的3d SLAM部分的源码刷一遍。

一、Pose3SLAMExample_g2o.cpp

和2d slam中的类似，也是读取文件，然后放入gtsam生成的graph，最后优化。

读取g2o文件：

  // Read graph from file
  string g2oFile;
  if (argc < 2)
    g2oFile = findExampleDataFile("pose3example.txt");
  else
    g2oFile = argv[1];

  NonlinearFactorGraph::shared_ptr graph;
  Values::shared_ptr initial;
  bool is3D = true;
  boost::tie(graph, initial) = readG2o(g2oFile, is3D);

按照运行指令的数目来判断是使用输入的第二个参数还是直接在可执行文件层寻找。

设置graph和initial指针，用来接受readg2o函数处理的结果，注意，graph是已经将factor加入到graph后的指针了。

设置第一个prior：

  // Add prior on the first key
  NonlinearFactorGraph graphWithPrior = *graph;
  noiseModel::Diagonal::shared_ptr 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;
    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
    break;
  }

将前面一步获得的graph赋值给graphWithPrior，之后将第一个点添加空的Pose3()和nosiy在放置到graphWithPrior中。注意for循环里使用了break，说明只循环了一次。

开始优化：

  std::cout << "Optimizing the factor graph" << std::endl;
  GaussNewtonParams params;
  params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
  GaussNewtonOptimizer optimizer(graphWithPrior, *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;
    writeG2o(*graph, result, outputFile);
    std::cout << "done! " << std::endl;
  }

使用了writeG2o函数，将结果输出到outputFile厘米。

结果：

对比原始：

 可视化：

 

 平面好像看不出来差别。还是得像ceres那样使用python可视化图像。

 python可视化代码：

from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plot
import numpy
import sys
from optparse import OptionParser

def set_axes_equal(axes):
    ''' Sets the axes of a 3D plot to have equal scale. '''
    x_limits = axes.get_xlim3d()
    y_limits = axes.get_ylim3d()
    z_limits = axes.get_zlim3d()

    x_range = abs(x_limits[1] - x_limits[0])
    x_middle = numpy.mean(x_limits)
    y_range = abs(y_limits[1] - y_limits[0])
    y_middle = numpy.mean(y_limits)
    z_range = abs(z_limits[1] - z_limits[0])
    z_middle = numpy.mean(z_limits)

    length = 0.5 * max([x_range, y_range, z_range])

    axes.set_xlim3d([x_middle - length, x_middle + length])
    axes.set_ylim3d([y_middle - length, y_middle + length])
    axes.set_zlim3d([z_middle - length, z_middle + length])

parser = OptionParser()
parser.add_option("--initial_poses", dest="initial_poses",
                  default="initial.txt", help="The filename that contains the original poses.")
parser.add_option("--optimized_poses", dest="optimized_poses",
                  default="result.txt", help="The filename that contains the optimized poses.")
parser.add_option("-e", "--axes_equal", action="store_true", dest="axes_equal",
                  default="", help="Make the plot axes equal.")
(options, args) = parser.parse_args()

poses_original = None
if options.initial_poses != '':
  poses_original = numpy.genfromtxt(options.initial_poses,
                                    usecols = (2, 3, 4))

poses_optimized = None
if options.optimized_poses != '':
  poses_optimized = numpy.genfromtxt(options.optimized_poses,
                                     usecols = (2, 3, 4))

figure = plot.figure()

if poses_original is not None:
  axes = plot.subplot(1, 2, 1, projection='3d')
  plot.plot(poses_original[:, 0], poses_original[:, 1], poses_original[:, 2],
            '-', alpha=0.5, color="green")
  plot.title('Original')
  if options.axes_equal:
    axes.set_aspect('equal')
    set_axes_equal(axes)


if poses_optimized is not None:
  axes = plot.subplot(1, 2, 2, projection='3d')
  plot.plot(poses_optimized[:, 0], poses_optimized[:, 1], poses_optimized[:, 2],
            '-', alpha=0.5, color="blue")
  plot.title('Optimized')
  if options.axes_equal:
    axes.set_aspect('equal')
    set_axes_equal(plot.gca())

plot.show()

随便修改了下，只能显示一个3维。

二、Pose3SLAMExample_changeKeys.cpp

和上面的没啥大的变化，主要是使用了不同的key。

读取文件：

  // Read graph from file
  string g2oFile;
  if (argc < 2)
    g2oFile = findExampleDataFile("pose3example.txt");
  else
    g2oFile = argv[1];

  NonlinearFactorGraph::shared_ptr graph;
  Values::shared_ptr initial;
  bool is3D = true;
  boost::tie(graph, initial) = readG2o(g2oFile, is3D);

读取g2o文件，然后使用函数readG2o文件并将factor都加入graph。

自定义key值：

  bool add = false;
  Key firstKey = 8646911284551352320;

  std::cout << "Using reference key: " <<  firstKey  << std::endl;
  if(add)
    std::cout << "adding key "  << std::endl;
  else
    std::cout << "subtracting key "  << std::endl;

选定key为8646911284551352320。

并设置是key加还是key减。

  if (argc < 3) {
    std::cout << "Please provide output file to write "   << std::endl;
  } else {
    const string inputFileRewritten = argv[2];
    std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
    // Additional: rewrite input with simplified keys 0,1,...
    Values simpleInitial;
    for(const Values::ConstKeyValuePair& key_value: *initial) {
      Key key;
      if(add)
        key = key_value.key + firstKey;
      else
        key = key_value.key - firstKey;

      simpleInitial.insert(key, initial->at(key_value.key));
    }

    NonlinearFactorGraph simpleGraph;
    for(const boost::shared_ptr<NonlinearFactor>& factor: *graph) {
      boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
          boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
      if (pose3Between){
        Key key1, key2;
        if(add){
          key1 = pose3Between->key1() + firstKey;
          key2 = pose3Between->key2() + firstKey;
        }else{
          key1 = pose3Between->key1() - firstKey;
          key2 = pose3Between->key2() - firstKey;
        }
        NonlinearFactor::shared_ptr simpleFactor(
            new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->noiseModel()));
        simpleGraph.add(simpleFactor);
      }
    }
    writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
  }

进行initial中key的修改操作；

生成一个新的simpleGraph，然后将修改key后的factor加入。

结果：

 三、Pose3SLAMExample_initializePose3Chordal.cpp

 初始化读入的g2o文件，不太清楚作用是什么，记录一下流程。

读入文件：

  // Read graph from file
  string g2oFile;
  if (argc < 2)
    g2oFile = findExampleDataFile("pose3example.txt");
  else
    g2oFile = argv[1];

  NonlinearFactorGraph::shared_ptr graph;
  Values::shared_ptr initial;
  bool is3D = true;
  boost::tie(graph, initial) = readG2o(g2oFile, is3D);

设置第一个key：

  // Add prior on the first key
  NonlinearFactorGraph graphWithPrior = *graph;
  noiseModel::Diagonal::shared_ptr 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;
    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
    break;
  }

设置初始化：

  std::cout << "Initializing Pose3 - chordal relaxation" << std::endl;
  Values initialization = InitializePose3::initialize(graphWithPrior);
  std::cout << "done!" << std::endl;

输出：

  if (argc < 3) {
    initialization.print("initialization");
  } else {
    const string outputFile = argv[2];
    std::cout << "Writing results to file: " << outputFile  << std::endl;
    writeG2o(*graph, initialization, outputFile);
    std::cout << "done! " << std::endl;
  }

四、Pose3SLAMExample_initializePose3Gradient.cpp

和三唯一的不同是使用了useGradient初始化。

  std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
  bool useGradient = true;
  Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
  std::cout << "done!" << std::endl;