gtsam学习笔记(一)
前言：
根据README.md里面的说明，对于Basic Examples部分的学习记录。
一、SimpleRotation
这个例子将对单个因子的单个变量执行一个简单的优化，用来说明gtsam的流程。
头文件部分：
#include <gtsam/geometry/Rot2.h>用来引入本例中使用的数据格式是Ro2。
#include <gtsam/inference/Symbol.h>Symbol是gtsam的一个特点，它类似于stl里面的map中key和value。每一个优化的变量都有一个独特的symbol。
#include <gtsam/slam/PriorFactor.h>gtsam中测量函数用因子表示。
#include <gtsam/nonlinear/NonlinearFactorGraph.h>创建了factor后，就要加入到factor graph里面，使用非线性的factor就要加入到非线性的factor graph里面。
#include <gtsam/nonlinear/Values.h>gtsam中的非线性求解器是迭代求解器，这意味着它们将围绕初始线性化点的非线性函数，然后求解线性系统更新线性化点。 这种情况反复发生，直到求解器收敛到一组一致的变量值。 这需要我们指定一个初始猜测对于每个变量，保存在一个值容器中。
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>选择lm优化方法。
下面根据源码内容来展示整个流程
第一步：创建因子factor
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);
PriorFactor<Rot2> factor(key, prior, model);prior是Rot2格式的变量，它来自某个传感器的测量值，因为是传感器的测量值，所以会有noise，符合高斯分布；
model就表示了噪声；
key就是前面提起的用来表示类似map中key-value中的key。它的value在本例中应该是prior。
factor就是因子，数据格式为Rot2，第一个参数是key，第二个是prior，第三个就是model，偏差或噪声。
第二步：创建因子图graph
在开始优化前，需要把所有创建的factor因子加入的graph中去，这个图就是提供了整个系统最高级约束。本例中只有一个factor，有些系统里面会有很多的factor。
NonlinearFactorGraph graph;
graph.push_back(factor);
graph.print("full graph");graph就是图，然后将factor加入里面。
第三步：设定初始估计值
对于待优化问题来说，给定初始值是必要的。本例中的变量是RotValue数据格式，所以初值应该也是对应的结构。
Values initial;
initial.insert(key, Rot2::fromAngle(20 * degree));
initial.print("initial estimate");第四步：开始优化
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
result.print("final result");将graph和initial加入优化器LevenbergMarquardtOptimizer中，结果给Value，优化完成。
完整源码：
#include <gtsam/geometry/Rot2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace std;
using namespace gtsam;
const double degree = M_PI / 180;
int main() {
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);
PriorFactor<Rot2> factor(key, prior, model);
NonlinearFactorGraph graph;
graph.push_back(factor);
graph.print("full graph");
Values initial;
initial.insert(key, Rot2::fromAngle(20 * degree));
initial.print("initial estimate");
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
result.print("final result");
return 0;
}
cmake_minimum_required(VERSION 2.8)
project(SimpleRotation)
find_package(GTSAM 4.0.2 REQUIRED QUIET)
include_directories(${GTSAM_INCLUDE_DIR})
include_directories("/usr/include/eigen3")
add_executable(${PROJECT_NAME} "SimpleRotation.cpp")
target_link_libraries(${PROJECT_NAME} gtsam)
输出：
goal angle: 0.523599
full graphsize: 1
Factor 0: PriorFactor on x1
prior mean: : 0.523599
isotropic dim=1 sigma=0.0174533
initial estimateValues with 1 values:
Value x1: (N5gtsam4Rot2E) : 0.349066
final resultValues with 1 values:
Value x1: (N5gtsam4Rot2E) : 0.523599二、CameraResectioning
相机重投影的问题，已知一些点，对于相机进行重投影。
头文件：
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <boost/make_shared.hpp>基本类似，不同是使用了gtsam中简易的相机模型和智能指针。
引用命名空间：
using namespace gtsam;
using namespace gtsam::noiseModel;
using symbol_shorthand::X;gtsam和gtsam::noiseModel不解释，而X就表示了gtsam中重要的key。
ResectioningFactor类：
/**
* Unary factor on the unknown pose, resulting from meauring the projection of
* a known 3D point in the image
*/
class ResectioningFactor: public NoiseModelFactor1<Pose3> {
typedef NoiseModelFactor1<Pose3> Base;
Cal3_S2::shared_ptr K_; ///< camera's intrinsic parameters
Point3 P_; ///< 3D point on the calibration rig
Point2 p_; ///< 2D measurement of the 3D point
public:
/// Construct factor given known point P and its projection p
ResectioningFactor(const SharedNoiseModel& model, const Key& key,
const Cal3_S2::shared_ptr& calib, const Point2& p, const Point3& P) :
Base(model, key), K_(calib), P_(P), p_(p) {
}
/// evaluate the error
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const {
SimpleCamera camera(pose, *K_);
return camera.project(P_, H, boost::none, boost::none) - p_;
}
};这个类就是表示要使用gtsam求解的未知量。
未知姿态的一元因子，由测量图像中已知 3D 点的投影产生。
使用Base作为NoiseModelFactor1<Pose3>的重命名；
K_为相机内参；
P_为3D点；
p_为2D点，它来源于3D点的投影；
之后就是构造函数，最后是计算误差，它是使用gtsam中设计的SimpleCamera生成的对象camera的投影值减去观测到的投影值。
main函数：
int main(int argc, char* argv[]) {
/* read camera intrinsic parameters */
Cal3_S2::shared_ptr calib(new Cal3_S2(1, 1, 0, 50, 50));
/* 1. create graph */
NonlinearFactorGraph graph;
/* 2. add factors to the graph */
// add measurement factors
SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5));
boost::shared_ptr<ResectioningFactor> factor;
graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 45), Point3(10, 10, 0));
graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 45), Point3(-10, 10, 0));
graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 55), Point3(-10, -10, 0));
graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 55), Point3(10, -10, 0));
/* 3. Create an initial estimate for the camera pose */
Values initial;
initial.insert(X(1),
Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 1)));
/* 4. Optimize the graph using Levenberg-Marquardt*/
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
result.print("Final result:\n");
return 0;
}和前面的流程类似，
第一步创建一个graph；
第二步将factor加入进去；
第三步创建初值；
第四步优化；
最后结果：
Final result:
Values with 1 values:
Value x1: (N5gtsam5Pose3E) R:
[
1, 0, 0;
0, -1, 0;
0, 0, -1
]
[0, 0, 2]';三、SFMExample.cpp
使用模拟数据求解SFM问题，SFM没有怎么接触过，来学习一下使用gtsam的流程。
前言：
通过SFMdata.h来创建一些pose和point的数据，给SFMExample.cpp提供ground truth。
// As this is a full 3D problem, we will use Pose3 variables to represent the camera
// positions and Point3 variables (x, y, z) to represent the landmark coordinates.
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
// We will also need a camera object to hold calibration information and perform projections.#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
// We will also need a camera object to hold calibration information and perform projections.
#include <gtsam/geometry/SimpleCamera.h>
/* ************************************************************************* */
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;
}
/* ************************************************************************* */
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;
}内容很简单，就是创建了两个函数createPoints和createPoses用来生成point和pose。
值的注意的是poses中的compose函数它的作用是加法，就是不能使用常规的加法，比如四元数的加法，旋转的的加法等。
具体代码：
// For loading the data, see the comments therein for scenario (camera rotates around cube)
#include "SFMdata.h"
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
#include <gtsam/geometry/Point2.h>
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/inference/Symbol.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Projection factors to model the camera's landmark observations.
// Also, we will initialize the robot at some location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Finally, once all of the factors have been added to our factor graph, we will want to
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
// trust-region method known as Powell's Degleg
#include <gtsam/nonlinear/DoglegOptimizer.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
#include <vector>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
std::cout << 1;
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks
vector<Point3> points = createPoints();
// Create the set of ground-truth poses
vector<Pose3> poses = createPoses();
// Create a factor graph
NonlinearFactorGraph graph;
// Add a prior on pose x1. This indirectly specifies where the origin is.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
SimpleCamera camera(poses[i], *K);
for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
}
}
// Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
// Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
// between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
graph.print("Factor Graph:\n");
// Create the data structure to hold the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth
Values initialEstimate;
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
initialEstimate.print("Initial Estimates:\n");
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
result.print("Final results:\n");
cout << "initial error = " << graph.error(initialEstimate) << endl;
cout << "final error = " << graph.error(result) << endl;
return 0;
}注释写的很明白了，最后输出结果：
1Factor Graph:
size: 66
Factor 0: PriorFactor on x0
prior mean: R:
[
6.12323e-17 -6.12323e-17 -1;
1 3.7494e-33 6.12323e-17;
-0 -1 6.12323e-17
]
[30, 0, 0]'; noise model: diagonal sigmas[0.3; 0.3; 0.3; 0.1; 0.1; 0.1];
Factor 1: GenericProjectionFactor, z = (75, 25)
keys = { x0 l0 }
noise model: unit (2)
Factor 2: GenericProjectionFactor, z = (62.5, 37.5)
keys = { x0 l1 }
noise model: unit (2)
Factor 3: GenericProjectionFactor, z = (37.5, 37.5)
keys = { x0 l2 }
noise model: unit (2)
Factor 4: GenericProjectionFactor, z = (25, 25)
keys = { x0 l3 }
noise model: unit (2)
Factor 5: GenericProjectionFactor, z = (75, 75)
keys = { x0 l4 }
noise model: unit (2)
Factor 6: GenericProjectionFactor, z = (62.5, 62.5)
keys = { x0 l5 }
noise model: unit (2)
Factor 7: GenericProjectionFactor, z = (37.5, 62.5)
keys = { x0 l6 }
noise model: unit (2)
Factor 8: GenericProjectionFactor, z = (25, 75)
keys = { x0 l7 }
noise model: unit (2)
Factor 9: GenericProjectionFactor, z = (50, 18.4699031)
keys = { x1 l0 }
noise model: unit (2)
Factor 10: GenericProjectionFactor, z = (73.570226, 33.3333333)
keys = { x1 l1 }
noise model: unit (2)
Factor 11: GenericProjectionFactor, z = (50, 38.672954)
keys = { x1 l2 }
noise model: unit (2)
Factor 12: GenericProjectionFactor, z = (26.429774, 33.3333333)
keys = { x1 l3 }
noise model: unit (2)
Factor 13: GenericProjectionFactor, z = (50, 81.5300969)
keys = { x1 l4 }
noise model: unit (2)
Factor 14: GenericProjectionFactor, z = (73.570226, 66.6666667)
keys = { x1 l5 }
noise model: unit (2)
Factor 15: GenericProjectionFactor, z = (50, 61.327046)
keys = { x1 l6 }
noise model: unit (2)
Factor 16: GenericProjectionFactor, z = (26.429774, 66.6666667)
keys = { x1 l7 }
noise model: unit (2)
Factor 17: GenericProjectionFactor, z = (25, 25)
keys = { x2 l0 }
noise model: unit (2)
Factor 18: GenericProjectionFactor, z = (75, 25)
keys = { x2 l1 }
noise model: unit (2)
Factor 19: GenericProjectionFactor, z = (62.5, 37.5)
keys = { x2 l2 }
noise model: unit (2)
Factor 20: GenericProjectionFactor, z = (37.5, 37.5)
keys = { x2 l3 }
noise model: unit (2)
Factor 21: GenericProjectionFactor, z = (25, 75)
keys = { x2 l4 }
noise model: unit (2)
Factor 22: GenericProjectionFactor, z = (75, 75)
keys = { x2 l5 }
noise model: unit (2)
Factor 23: GenericProjectionFactor, z = (62.5, 62.5)
keys = { x2 l6 }
noise model: unit (2)
Factor 24: GenericProjectionFactor, z = (37.5, 62.5)
keys = { x2 l7 }
noise model: unit (2)
Factor 25: GenericProjectionFactor, z = (26.429774, 33.3333333)
keys = { x3 l0 }
noise model: unit (2)
Factor 26: GenericProjectionFactor, z = (50, 18.4699031)
keys = { x3 l1 }
noise model: unit (2)
Factor 27: GenericProjectionFactor, z = (73.570226, 33.3333333)
keys = { x3 l2 }
noise model: unit (2)
Factor 28: GenericProjectionFactor, z = (50, 38.672954)
keys = { x3 l3 }
noise model: unit (2)
Factor 29: GenericProjectionFactor, z = (26.429774, 66.6666667)
keys = { x3 l4 }
noise model: unit (2)
Factor 30: GenericProjectionFactor, z = (50, 81.5300969)
keys = { x3 l5 }
noise model: unit (2)
Factor 31: GenericProjectionFactor, z = (73.570226, 66.6666667)
keys = { x3 l6 }
noise model: unit (2)
Factor 32: GenericProjectionFactor, z = (50, 61.327046)
keys = { x3 l7 }
noise model: unit (2)
Factor 33: GenericProjectionFactor, z = (37.5, 37.5)
keys = { x4 l0 }
noise model: unit (2)
Factor 34: GenericProjectionFactor, z = (25, 25)
keys = { x4 l1 }
noise model: unit (2)
Factor 35: GenericProjectionFactor, z = (75, 25)
keys = { x4 l2 }
noise model: unit (2)
Factor 36: GenericProjectionFactor, z = (62.5, 37.5)
keys = { x4 l3 }
noise model: unit (2)
Factor 37: GenericProjectionFactor, z = (37.5, 62.5)
keys = { x4 l4 }
noise model: unit (2)
Factor 38: GenericProjectionFactor, z = (25, 75)
keys = { x4 l5 }
noise model: unit (2)
Factor 39: GenericProjectionFactor, z = (75, 75)
keys = { x4 l6 }
noise model: unit (2)
Factor 40: GenericProjectionFactor, z = (62.5, 62.5)
keys = { x4 l7 }
noise model: unit (2)
Factor 41: GenericProjectionFactor, z = (50, 38.672954)
keys = { x5 l0 }
noise model: unit (2)
Factor 42: GenericProjectionFactor, z = (26.429774, 33.3333333)
keys = { x5 l1 }
noise model: unit (2)
Factor 43: GenericProjectionFactor, z = (50, 18.4699031)
keys = { x5 l2 }
noise model: unit (2)
Factor 44: GenericProjectionFactor, z = (73.570226, 33.3333333)
keys = { x5 l3 }
noise model: unit (2)
Factor 45: GenericProjectionFactor, z = (50, 61.327046)
keys = { x5 l4 }
noise model: unit (2)
Factor 46: GenericProjectionFactor, z = (26.429774, 66.6666667)
keys = { x5 l5 }
noise model: unit (2)
Factor 47: GenericProjectionFactor, z = (50, 81.5300969)
keys = { x5 l6 }
noise model: unit (2)
Factor 48: GenericProjectionFactor, z = (73.570226, 66.6666667)
keys = { x5 l7 }
noise model: unit (2)
Factor 49: GenericProjectionFactor, z = (62.5, 37.5)
keys = { x6 l0 }
noise model: unit (2)
Factor 50: GenericProjectionFactor, z = (37.5, 37.5)
keys = { x6 l1 }
noise model: unit (2)
Factor 51: GenericProjectionFactor, z = (25, 25)
keys = { x6 l2 }
noise model: unit (2)
Factor 52: GenericProjectionFactor, z = (75, 25)
keys = { x6 l3 }
noise model: unit (2)
Factor 53: GenericProjectionFactor, z = (62.5, 62.5)
keys = { x6 l4 }
noise model: unit (2)
Factor 54: GenericProjectionFactor, z = (37.5, 62.5)
keys = { x6 l5 }
noise model: unit (2)
Factor 55: GenericProjectionFactor, z = (25, 75)
keys = { x6 l6 }
noise model: unit (2)
Factor 56: GenericProjectionFactor, z = (75, 75)
keys = { x6 l7 }
noise model: unit (2)
Factor 57: GenericProjectionFactor, z = (73.570226, 33.3333333)
keys = { x7 l0 }
noise model: unit (2)
Factor 58: GenericProjectionFactor, z = (50, 38.672954)
keys = { x7 l1 }
noise model: unit (2)
Factor 59: GenericProjectionFactor, z = (26.429774, 33.3333333)
keys = { x7 l2 }
noise model: unit (2)
Factor 60: GenericProjectionFactor, z = (50, 18.4699031)
keys = { x7 l3 }
noise model: unit (2)
Factor 61: GenericProjectionFactor, z = (73.570226, 66.6666667)
keys = { x7 l4 }
noise model: unit (2)
Factor 62: GenericProjectionFactor, z = (50, 61.327046)
keys = { x7 l5 }
noise model: unit (2)
Factor 63: GenericProjectionFactor, z = (26.429774, 66.6666667)
keys = { x7 l6 }
noise model: unit (2)
Factor 64: GenericProjectionFactor, z = (50, 81.5300969)
keys = { x7 l7 }
noise model: unit (2)
Factor 65: PriorFactor on l0
prior mean: [10, 10, 10]'
isotropic dim=3 sigma=0.1
Initial Estimates:
Values with 16 values:
Value l0: (N5gtsam6Point3E) [9.75, 10.2, 10.15]'
Value l1: (N5gtsam6Point3E) [-10.25, 10.2, 10.15]'
Value l2: (N5gtsam6Point3E) [-10.25, -9.8, 10.15]'
Value l3: (N5gtsam6Point3E) [9.75, -9.8, 10.15]'
Value l4: (N5gtsam6Point3E) [9.75, 10.2, -9.85]'
Value l5: (N5gtsam6Point3E) [-10.25, 10.2, -9.85]'
Value l6: (N5gtsam6Point3E) [-10.25, -9.8, -9.85]'
Value l7: (N5gtsam6Point3E) [9.75, -9.8, -9.85]'
Value x0: (N5gtsam5Pose3E) R:
[
0.208654288 0.0733690165 -0.975233498;
0.949228671 -0.255245398 0.183887786;
-0.235432196 -0.964088572 -0.122902021
]
[29.8, 0.05, 0.1]';
Value x1: (N5gtsam5Pose3E) R:
[
-0.523665168 0.232365481 -0.81962252;
0.818746892 -0.128606022 -0.559565919;
-0.235432196 -0.964088572 -0.122902021
]
[21.0364267, 21.1071374, 0.1]';
Value x2: (N5gtsam5Pose3E) R:
[
-0.949228671 0.255245398 -0.183887786;
0.208654288 0.0733690165 -0.975233498;
-0.235432196 -0.964088572 -0.122902021
]
[-0.05, 29.8, 0.1]';
Value x3: (N5gtsam5Pose3E) R:
[
-0.818746892 0.128606022 0.559565919;
-0.523665168 0.232365481 -0.81962252;
-0.235432196 -0.964088572 -0.122902021
]
[-21.1071374, 21.0364267, 0.1]';
Value x4: (N5gtsam5Pose3E) R:
[
-0.208654288 -0.0733690165 0.975233498;
-0.949228671 0.255245398 -0.183887786;
-0.235432196 -0.964088572 -0.122902021
]
[-29.8, -0.05, 0.1]';
Value x5: (N5gtsam5Pose3E) R:
[
0.523665168 -0.232365481 0.81962252;
-0.818746892 0.128606022 0.559565919;
-0.235432196 -0.964088572 -0.122902021
]
[-21.0364267, -21.1071374, 0.1]';
Value x6: (N5gtsam5Pose3E) R:
[
0.949228671 -0.255245398 0.183887786;
-0.208654288 -0.0733690165 0.975233498;
-0.235432196 -0.964088572 -0.122902021
]
[0.05, -29.8, 0.1]';
Value x7: (N5gtsam5Pose3E) R:
[
0.818746892 -0.128606022 -0.559565919;
0.523665168 -0.232365481 0.81962252;
-0.235432196 -0.964088572 -0.122902021
]
[21.1071374, -21.0364267, 0.1]';
Final results:
Values with 16 values:
Value l0: (N5gtsam6Point3E) [10, 10, 10]'
Value l1: (N5gtsam6Point3E) [-10, 10, 10]'
Value l2: (N5gtsam6Point3E) [-10, -10, 10]'
Value l3: (N5gtsam6Point3E) [10, -10, 10]'
Value l4: (N5gtsam6Point3E) [10, 10, -10]'
Value l5: (N5gtsam6Point3E) [-10, 10, -10]'
Value l6: (N5gtsam6Point3E) [-10, -10, -10]'
Value l7: (N5gtsam6Point3E) [10, -10, -10]'
Value x0: (N5gtsam5Pose3E) R:
[
-5.37770093e-17 8.24800664e-18 -1;
1 2.27644488e-17 -2.68488853e-17;
1.16450654e-16 -1 3.11294338e-17
]
[30, -4.68114623e-19, -2.15189638e-19]';
Value x1: (N5gtsam5Pose3E) R:
[
-0.707106781 3.75312306e-18 -0.707106781;
0.707106781 5.86729548e-18 -0.707106781;
1.52433155e-16 -1 6.99808929e-17
]
[21.2132034, 21.2132034, -1.4796896e-15]';
Value x2: (N5gtsam5Pose3E) R:
[
-1 4.20609106e-17 6.82119776e-17;
-1.12269844e-16 1.27050957e-16 -1;
1.19325226e-16 -1 -1.31789384e-16
]
[-1.25485088e-14, 30, 5.67930507e-15]';
Value x3: (N5gtsam5Pose3E) R:
[
-0.707106781 -1.96868593e-16 0.707106781;
-0.707106781 8.63325543e-17 -0.707106781;
5.99990949e-18 -1 -2.09425395e-16
]
[-21.2132034, 21.2132034, 1.04414843e-14]';
Value x4: (N5gtsam5Pose3E) R:
[
-1.47329142e-16 -2.15068053e-16 1;
-1 1.96941095e-16 -1.50757739e-16;
-5.25285195e-17 -1 -1.80308612e-16
]
[-30, -9.76967476e-15, 7.47967009e-15]';
Value x5: (N5gtsam5Pose3E) R:
[
0.707106781 -3.18356096e-16 0.707106781;
-0.707106781 1.29930462e-16 0.707106781;
-2.05181533e-16 -1 -1.11734276e-16
]
[-21.2132034, -21.2132034, 6.54741874e-15]';
Value x6: (N5gtsam5Pose3E) R:
[
1 -1.45776682e-16 3.6965474e-16;
-2.70530379e-16 7.28784253e-17 1;
3.51855522e-17 -1 5.46050822e-17
]
[-4.72469291e-15, -30, 8.56613512e-16]';
Value x7: (N5gtsam5Pose3E) R:
[
0.707106781 -3.66430096e-16 -0.707106781;
0.707106781 2.43145056e-16 0.707106781;
-3.68683948e-17 -1 4.52437243e-16
]
[21.2132034, -21.2132034, -1.5304036e-14]';
initial error = 7484.19025
final error = 2.53707987e-27
不是很理解SFM问题，看不太明白这段代码求的结果是什么意思。但具体过程就是把前面生成的pose和point一个一个的设置好key-value加入进去，最后选择求解的方法求解。可以看结果是final result是points基本还原了，pose不太好比较。
这部分的前面的比较，变化大的是没有直接创建factor，而是直接加入graph里面了。
gtsam还是很强大的。
版权声明：本文为weixin_45572737原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。