热门标签 | HotTags
当前位置:  开发笔记 > 编程语言 > 正文

gtsam学习笔记2dSLAM(二)

前言:根据README.md里面的说明,对于2DPoseSLAM部分的example学习记录。一、LocalizationExample.cpp比

前言:

根据README.md里面的说明,对于2D Pose SLAM部分的example学习记录。


一、LocalizationExample.cpp

比较简单的一个例子:

在二维平面上有一个移动机器人,有它移动3个点时记录下来的gps数据,和这三个点之间的odometry的数据。然后给定一个错误的初值,最后求得结果和前面设置的gps数据对比。


unaryFactor设置



// Before we begin the example, we must create a custom unary factor to implement a


// "GPS-like" functionality. Because standard GPS measurements provide information


// only on the position, and not on the orientation, we cannot use a simple prior to


// properly model this measurement.


就是说用标准的gps是只有position坐标,没有orientation信息,所以就需要创建一个unaryFactor类。

具体代码如下:

class UnaryFactor: public NoiseModelFactor1 {
// The factor will hold a measurement consisting of an (X,Y) location
// We could this with a Point2 but here we just use two doubles
double mx_, my_;
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr shared_ptr;
// The constructor requires the variable key, the (X, Y) measurement value, and the noise model
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
NoiseModelFactor1(model, j), mx_(x), my_(y) {}
virtual ~UnaryFactor() {}
// Using the NoiseModelFactor1 base class there are two functions that must be overridden.
// The first is the 'evaluateError' function. This function implements the desired measurement
// function, returning a vector of errors when evaluated at the provided variable value. It
// must also calculate the Jacobians for this measurement function, if requested.
Vector evaluateError(const Pose2& q, boost::optional H = boost::none) const
{
// The measurement function for a GPS-like measurement is simple:
// error_x = pose.x - measurement.x
// error_y = pose.y - measurement.y
// Consequently, the Jacobians are:
// [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0]
// [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0]
if (H) (*H) &#61; (Matrix(2,3) <<1.0,0.0,0.0, 0.0,1.0,0.0).finished();
return (Vector(2) < }
// The second is a &#39;clone&#39; function that allows the factor to be copied. Under most
// circumstances, the following code that employs the default copy constructor should
// work fine.
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast(
gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }
// Additionally, we encourage you the use of unit testing your custom factors,
// (as all GTSAM factors are), in which you would need an equals and print, to satisfy the
// GTSAM_CONCEPT_TESTABLE_INST(T) defined in Testable.h, but these are not needed below.
}; // UnaryFactor

比较简单&#xff0c;值得注意的是如果要继承NoiseModelFactor1&#xff0c;需要override两个函数evaluateError和clone。


具体执行过程&#xff1a;

int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise &#61; noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.emplace_shared >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
graph.emplace_shared >(2, 3, Pose2(3.0, 0.0, 0.0), odometryNoise);
// 2b. Add "GPS-like" measurements
// We will use our custom UnaryFactor for this.
noiseModel::Diagonal::shared_ptr unaryNoise &#61; noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
graph.emplace_shared(1, 0.0, 0.0, unaryNoise);
graph.emplace_shared(2, 2.0, 0.0, unaryNoise);
graph.emplace_shared(3, 5.0, 0.0, unaryNoise);
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Optimize using Levenberg-Marquardt optimization. The optimizer
// accepts an optional set of configuration parameters, controlling
// things like convergence criteria, the type of linear system solver
// to use, and the amount of information displayed during optimization.
// Here we will use the default set of parameters. See the
// documentation for the full set of parameters.
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
Values result &#61; optimizer.optimize();
result.print("Final Result:\n");
// 5. Calculate and print marginal covariances for all variables
Marginals marginals(graph, result);
cout <<"x1 covariance:\n" < cout <<"x2 covariance:\n" < cout <<"x3 covariance:\n" < return 0;
}

第一步创建graph&#xff1b;

第二步往graph里面添加odometry factor&#xff0c;添加GPS-like的factor&#xff1b;

第三步设置Values&#xff0c;初值initialEstimate&#xff0c;添加一些数&#xff1b;

第四步选择优化器&#xff0c;优化并输出结果。


结果&#xff1a;

Factor Graph:
size: 5
Factor 0: BetweenFactor(1,2)
measured: (2, 0, 0)
noise model: diagonal sigmas[0.2; 0.2; 0.1];
Factor 1: BetweenFactor(2,3)
measured: (3, 0, 0)
noise model: diagonal sigmas[0.2; 0.2; 0.1];
Factor 2: keys &#61; { 1 }
isotropic dim&#61;2 sigma&#61;0.1
Factor 3: keys &#61; { 2 }
isotropic dim&#61;2 sigma&#61;0.1
Factor 4: keys &#61; { 3 }
isotropic dim&#61;2 sigma&#61;0.1
Initial Estimate:
Values with 3 values:
Value 1: (N5gtsam5Pose2E) (0.5, 0, 0.2)
Value 2: (N5gtsam5Pose2E) (2.3, 0.1, -0.2)
Value 3: (N5gtsam5Pose2E) (4.1, 0.1, 0.1)
Final Result:
Values with 3 values:
Value 1: (N5gtsam5Pose2E) (-1.34728301e-14, 1.16725243e-15, -1.64808921e-16)
Value 2: (N5gtsam5Pose2E) (2, 2.17285499e-16, -7.22571183e-17)
Value 3: (N5gtsam5Pose2E) (5, -2.19538393e-16, -7.00766967e-17)
x1 covariance:
0.00828571429 2.76564799e-19 -7.47383061e-19
2.76564799e-19 0.00928571429 -0.00261904762
-7.47383061e-19 -0.00261904762 0.00706349206
x2 covariance:
0.00714285714 1.61131272e-19 1.17630708e-19
1.61131272e-19 0.00801587302 -0.00134920635
1.17630708e-19 -0.00134920635 0.00468253968
x3 covariance:
0.00828571429 1.35379321e-19 2.35144668e-19
1.35379321e-19 0.00968253968 0.00253968254
2.35144668e-19 0.00253968254 0.0146825397

可以看出&#xff0c;虽然在添加odometry和gps factor中添加了nosiy&#xff0c;但最后输出的结果是符合真实值的。


二、Pose2SLAMExample.cpp

这个内容很简单&#xff0c;有一个2d平面&#xff0c;然后机器人在里面运行&#xff0c;记录了5个点之间的里程计信息。通过优化&#xff0c;求解它们相对于第一个点的坐标信息。

 就是这个图的意思。


第一步&#xff1a;创建graph

// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;

第二步&#xff1a;创建factor

// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise &#61; noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model &#61; noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses
graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model);
graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model);
// 2c. Add the loop closure constraint
// This factor encodes the fact that we have returned to the same pose. In real systems,
// these constraints may be identified in many ways, such as appearance-based techniques
// with camera images. We will use another Between Factor to enforce this constraint:
graph.emplace_shared >(5, 2, Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print

包括创建第一个因子和回环限制因子。


第三步&#xff1a;创建初值&#xff0c;并确定输出的数据类型和初值的格式一样。

// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 ));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 ));
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI ));
initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
initialEstimate.print("\nInitial Estimate:\n"); // print

第四步&#xff1a;优化

// 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
// The optimizer accepts an optional set of configuration parameters,
// controlling things like convergence criteria, the type of linear
// system solver to use, and the amount of information displayed during
// optimization. We will set a few parameters as a demonstration.
GaussNewtonParams parameters;
// Stop iterating once the change in error between steps is less than this value
parameters.relativeErrorTol &#61; 1e-5;
// Do not perform more than N iteration steps
parameters.maxIterations &#61; 100;
// Create the optimizer ...
GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
// ... and optimize
Values result &#61; optimizer.optimize();
result.print("Final Result:\n");

与之前不同的是&#xff0c;多了对于优化器参数的设置部分。


第五步&#xff1a;查看方差

// 5. Calculate and print marginal covariances for all variables
cout.precision(3);
Marginals marginals(graph, result);
cout <<"x1 covariance:\n" < cout <<"x2 covariance:\n" < cout <<"x3 covariance:\n" < cout <<"x4 covariance:\n" < cout <<"x5 covariance:\n" <

简单明了&#xff0c;最容易理解的就是这个。


结果&#xff1a;

Final Result:
Values with 5 values:
Value 1: (N5gtsam5Pose2E) (-1.27455483e-20, 8.10527674e-20, 3.07189987e-20)
Value 2: (N5gtsam5Pose2E) (2, 1.78514217e-19, 4.34262714e-20)
Value 3: (N5gtsam5Pose2E) (4, -3.42173202e-11, 1.57079633)
Value 4: (N5gtsam5Pose2E) (4, 2, 3.14159265)
Value 5: (N5gtsam5Pose2E) (2, 2, -1.57079633)

三、Pose2SLAMExampleExpressions.cpp

这个和二的Pose2SLAMExample.cpp区别不大&#xff0c;主要的区别是使用&#xff1a;

// Create Expressions for unknowns
Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5);

来代替了Pose2SLAMExample.cpp里面的数字1&#xff0c;2&#xff0c;3&#xff0c;4&#xff0c;5。

以及ExpressionFactorGraph来替代了NonlinearFactorGraph。

ExpressionFactorGraph graph;

结果都一样&#xff1a;

Final Result:
Values with 5 values:
Value 1: (N5gtsam5Pose2E) (1.49017327e-20, -2.14141805e-20, -4.58273596e-21)
Value 2: (N5gtsam5Pose2E) (2, -4.0097066e-20, -4.40676513e-21)
Value 3: (N5gtsam5Pose2E) (4, -4.9189295e-12, 1.57079633)
Value 4: (N5gtsam5Pose2E) (4, 2, -3.14159265)
Value 5: (N5gtsam5Pose2E) (2, 2, -1.57079633)

四、Pose2SLAMExample_g2o.cpp

读取g2o文件的内容然后直接输出计算&#xff0c;和前面的类似。

example中的源码部分使用了很多封装的函数比如readG2o&#xff0c;输入g2o文件名和3d标志位&#xff0c;输出已经将factor加入的graph和已经添加了的初值initial指针。

boost::tie(graph, initial) &#61; readG2o(g2oFile,is3D);

/**
* &#64;brief This function parses a g2o file and stores the measurements into a
* NonlinearFactorGraph and the initial guess in a Values structure
* &#64;param filename The name of the g2o file\
* &#64;param is3D indicates if the file describes a 2D or 3D problem
* &#64;param kernelFunctionType whether to wrap the noise model in a robust kernel
* &#64;return graph and initial values
*/
GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D &#61; false,
KernelFunctionType kernelFunctionType &#61; KernelFunctionTypeNONE);

还有writeG2o函数&#xff1a;

/**
* &#64;brief This function writes a g2o file from
* NonlinearFactorGraph and a Values structure
* &#64;param filename The name of the g2o file to write
* &#64;param graph NonlinearFactor graph storing the measurements
* &#64;param estimate Values
*/
GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
const Values& estimate, const std::string& filename);

其他部分和前面的一样&#xff0c;这里只是多了对于txt文件的读取和封装函数readG2o的使用。

完整源码&#xff1a;

#include
#include
#include
#include
#include
using namespace std;
using namespace gtsam;
// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
int main(const int argc, const char *argv[]) {
string kernelType &#61; "none";
int maxIterations &#61; 100; // default
string g2oFile &#61; findExampleDataFile("noisyToyGraph.txt"); // default
// Parse user&#39;s inputs
if (argc > 1){
g2oFile &#61; argv[1]; // input dataset filename
// outputFile &#61; g2oFile &#61; argv[2]; // done later
}
if (argc > 3){
maxIterations &#61; atoi(argv[3]); // user can specify either tukey or huber
}
if (argc > 4){
kernelType &#61; argv[4]; // user can specify either tukey or huber
}
// reading file and creating factor graph
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D &#61; false;
if(kernelType.compare("none") &#61;&#61; 0){
boost::tie(graph, initial) &#61; readG2o(g2oFile,is3D);
}
if(kernelType.compare("huber") &#61;&#61; 0){
std::cout <<"Using robust kernel: huber " < boost::tie(graph, initial) &#61; readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
}
if(kernelType.compare("tukey") &#61;&#61; 0){
std::cout <<"Using robust kernel: tukey " < boost::tie(graph, initial) &#61; readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
}
// Add prior on the pose having index (key) &#61; 0
NonlinearFactorGraph graphWithPrior &#61; *graph;
noiseModel::Diagonal::shared_ptr priorModel &#61; //
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor(0, Pose2(), priorModel));
std::cout <<"Adding prior on pose 0 " < graphWithPrior.print("\nFactor Graph:\n");
GaussNewtonParams params;
params.setVerbosity("TERMINATION");
if (argc > 3) {
params.maxIterations &#61; maxIterations;
std::cout <<"User required to perform maximum " < }
std::cout <<"initial data: " < (*initial).print("\nInitial Estimate:\n");
std::cout <<"Optimizing the factor graph" < GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
Values result &#61; optimizer.optimize();
std::cout <<"Optimization complete" < // std::cout <<"initial error&#61;" <error(*initial)< // std::cout <<"final error&#61;" <error(result)< if (argc <3) {
result.print("result");
} else {
const string outputFile &#61; argv[2];
std::cout <<"Writing results to file: " < NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) &#61; readG2o(g2oFile);
writeG2o(*graphNoKernel, result, outputFile);
std::cout <<"done! " < }
return 0;
}

原始部分没有对于加入graph的输出和初值initial的输出提示&#xff0c;我加了进去。

输出结果&#xff1a;

Factor Graph:
size: 6
Factor 0: BetweenFactor(0,1)
measured: (0.774115, 1.18339, 1.57617)
noise model: unit (3)
Factor 1: BetweenFactor(1,2)
measured: (0.869231, 1.03188, 1.57942)
noise model: unit (3)
Factor 2: BetweenFactor(2,3)
measured: (1.35784, 1.03426, 1.56646)
noise model: unit (3)
Factor 3: BetweenFactor(2,0)
measured: (0.303492, 1.86501, -3.1139)
noise model: unit (3)
Factor 4: BetweenFactor(0,3)
measured: (-0.928526, 0.993695, -1.56354)
noise model: unit (3)
Factor 5: PriorFactor on 0
prior mean: (0, 0, 0)
noise model: diagonal sigmas[0.001; 0.001; 0.0001];
initial data:
Initial Estimate:
Values with 4 values:
Value 0: (N5gtsam5Pose2E) (0, 0, 0)
Value 1: (N5gtsam5Pose2E) (0.774115, 1.183389, 1.576173)
Value 2: (N5gtsam5Pose2E) (-0.26242, 2.047059, -3.127594)
Value 3: (N5gtsam5Pose2E) (-1.605649, 0.993891, -1.561134)
Optimizing the factor graph
Warning: stopping nonlinear iterations because error increased
errorThreshold: 0.0685034665 absoluteDecrease: -2.28853697639e-05 relativeDecrease: -0.000334187727181 iterations: 3 >? 100
Optimization complete
resultValues with 4 values:
Value 0: (N5gtsam5Pose2E) (-7.53875248522e-23, -3.04401700725e-24, 1.41745664019e-24)
Value 1: (N5gtsam5Pose2E) (0.956178940406, 1.14319822891, 1.54150026689)
Value 2: (N5gtsam5Pose2E) (0.126611466178, 1.9817945338, -3.08397402066)
Value 3: (N5gtsam5Pose2E) (-1.05038303244, 0.935131621066, -1.54052801033)

然后&#xff0c;我根据Pose2SLAMExampleExpressions.cpp的内容&#xff0c;使用这个noisyToyGraph.txt文件的内容&#xff0c;写了个类似的&#xff0c;手动输入值。

#include
#include
#include
#include
#include
#include
#include
using namespace std;
using namespace gtsam;
int main(int argc, char** argv)
{
ExpressionFactorGraph graph;
Pose2_ x1(1), x2(2), x3(3), x4(4);
noiseModel::Diagonal::shared_ptr priorNoise &#61; noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
noiseModel::Diagonal::shared_ptr model &#61; noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.addExpressionFactor(between(x1,x2), Pose2(0.774115, 1.183389, 1.576173), model);
graph.addExpressionFactor(between(x2,x3), Pose2(0.869231, 1.031877, 1.579418), model);
graph.addExpressionFactor(between(x3,x4), Pose2(1.357840, 1.034262, 1.566460), model);
graph.addExpressionFactor(between(x3,x1), Pose2(0.303492, 1.865011, -3.113898), model);
graph.addExpressionFactor(between(x1,x4), Pose2(-0.928526, 0.993695, -1.563542), model);
graph.print("\nFactor Graph:\n"); // print
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.0, 0.0, 0.0 ));
initialEstimate.insert(2, Pose2(0.774115, 1.183389, 1.576173 ));
initialEstimate.insert(3, Pose2(-0.262420, 2.047059, -3.127594));
initialEstimate.insert(4, Pose2(-1.605649, 0.993891, -1.561134 ));
initialEstimate.print("\nInitial Estimate:\n"); // print
GaussNewtonParams parameters;
parameters.relativeErrorTol &#61; 1e-5;
parameters.maxIterations &#61; 3;
GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
Values result &#61; optimizer.optimize();
result.print("Final Result:\n");
return 0;
}

发现它们的结果不一样&#xff0c;不知道是哪里出现了问题。

//我自己写的
Values with 4 values:
Value 1: (N5gtsam5Pose2E) (-2.22725181e-16, 4.72511144e-17, 2.13599316e-17)
Value 2: (N5gtsam5Pose2E) (0.986584345, 1.13382604, 1.56093936)
Value 3: (N5gtsam5Pose2E) (0.175794601, 1.96362262, -3.12403007)
Value 4: (N5gtsam5Pose2E) (-1.04609988, 0.949686099, -1.56055604)
//使用Pose2SLAMExample_g2o.cpp输出的
resultValues with 4 values:
Value 0: (N5gtsam5Pose2E) (-7.53875248522e-23, -3.04401700725e-24, 1.41745664019e-24)
Value 1: (N5gtsam5Pose2E) (0.956178940406, 1.14319822891, 1.54150026689)
Value 2: (N5gtsam5Pose2E) (0.126611466178, 1.9817945338, -3.08397402066)
Value 3: (N5gtsam5Pose2E) (-1.05038303244, 0.935131621066, -1.54052801033)

五、Pose2SLAMExample_graph.cpp

主要是读取w100.graph文件内容的example&#xff0c;和前面没有太多的不同&#xff0c;有一个封装函数load2D&#xff0c;

boost::tie(graph, initial) &#61; load2D(graph_file, model);

输入graph_file文件和nosiy model&#xff0c;输出是已经将factor加入的graph和已经加入的初值initial。其它类似前面的。


六、Pose2SLAMExample_graphviz.cpp

这部分的主要内容和Pose2SLAMExample.cpp一模一样&#xff0c;它不同在于介绍了一个函数&#xff1a;

// save factor graph as graphviz dot file
// Render to PDF using "fdp Pose2SLAMExample.dot -Tpdf > graph.pdf"
ofstream os("Pose2SLAMExample.dot");
graph.saveGraph(os, result);

可以将获得的结果result存放在os文件中&#xff0c;并通过命令&#xff1a;

fdp Pose2SLAMExample.dot -Tpdf > graph.pdf

将它转换为PDF文件。

对比initial值&#xff1a;

优化的效果还是很明显的。

我利用这个函数和命令把前面w100.graph的优化结果也显示了一下&#xff1a;


 七、Pose2SLAMStressTest.cpp

这个好像是创造一系列的3D poses&#xff0c;每个pose都加入噪声&#xff0c;然后就优化&#xff1f;



// Create N 3D poses, add relative motion between each consecutive poses. (The


// relative motion is simply a unit translation(1, 0, 0), no rotation). For each


// each pose, add some random noise to the x value of the translation part.


// Use gtsam to create a prior factor for the first pose and N-1 between factors


// and run optimization.


比较简单&#xff0c;对于initial部分的添加偏差值得学习一下&#xff1a;

std::random_device rd;
std::mt19937 e2(rd());
std::uniform_real_distribution<> dist(0, 1);
vector poses;
for (int i &#61; 0; i Matrix4 M;
double r &#61; dist(e2);
r &#61; (r - 0.5) / 10 &#43; i;
M <<1, 0, 0, r, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
poses.push_back(Pose3(M));
}

八、Pose2SLAMwSPCG.cpp

使用了PCG solver来求解&#xff0c;其他内容和Pose2SLAMExample.cpp部分一样。

// 4. Single Step Optimization using Levenberg-Marquardt
LevenbergMarquardtParams parameters;
parameters.verbosity &#61; NonlinearOptimizerParams::ERROR;
parameters.verbosityLM &#61; LevenbergMarquardtParams::LAMBDA;
// LM is still the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.
// In addition, the *type* of the iterativeParams decides on the type of
// iterative solver, in this case the SPCG (subgraph PCG)
parameters.linearSolverType &#61; NonlinearOptimizerParams::Iterative;
parameters.iterativeParams &#61; boost::make_shared();

就是使用了sub PCG的求解&#xff0c;具体为什么使用这个不太清楚。



推荐阅读
  • 探讨ChatGPT在法律和版权方面的潜在风险及影响,分析其作为内容创造工具的合法性和合规性。 ... [详细]
  • 本文详细介绍了Java中org.w3c.dom.Text类的splitText()方法,通过多个代码示例展示了其实际应用。该方法用于将文本节点在指定位置拆分为两个节点,并保持在文档树中。 ... [详细]
  • 基于KVM的SRIOV直通配置及性能测试
    SRIOV介绍、VF直通配置,以及包转发率性能测试小慢哥的原创文章,欢迎转载目录?1.SRIOV介绍?2.环境说明?3.开启SRIOV?4.生成VF?5.VF ... [详细]
  • 本文探讨了领域驱动设计(DDD)的核心概念、应用场景及其实现方式,详细介绍了其在企业级软件开发中的优势和挑战。通过对比事务脚本与领域模型,展示了DDD如何提升系统的可维护性和扩展性。 ... [详细]
  • 探讨了如何解决Ajax请求响应时间过长的问题。本文分析了一个从服务器获取少量数据的Ajax请求,尽管服务器已经对JSON响应进行了缓存,但实际响应时间仍然不稳定。 ... [详细]
  • 解决FCKeditor应用主题后上传问题及优化配置
    本文介绍了在Freetextbox收费后选择FCKeditor作为替代方案时遇到的上传问题及其解决方案。通过调整配置文件和调试工具,最终解决了上传失败的问题,并对相关配置进行了优化。 ... [详细]
  • 反向投影技术主要用于在大型输入图像中定位特定的小型模板图像。通过直方图对比,它能够识别出最匹配的区域或点,从而确定模板图像在输入图像中的位置。 ... [详细]
  • Python处理Word文档的高效技巧
    本文详细介绍了如何使用Python处理Word文档,涵盖从基础操作到高级功能的各种技巧。我们将探讨如何生成文档、定义样式、提取表格数据以及处理超链接和图片等内容。 ... [详细]
  • 采用IKE方式建立IPsec安全隧道
    一、【组网和实验环境】按如上的接口ip先作配置,再作ipsec的相关配置,配置文本见文章最后本文实验采用的交换机是H3C模拟器,下载地址如 ... [详细]
  • 实用正则表达式有哪些
    小编给大家分享一下实用正则表达式有哪些,相信大部分人都还不怎么了解,因此分享这篇文章给大家参考一下,希望大家阅读完这篇文章后大有收获,下 ... [详细]
  • 本文介绍了如何在 Node.js 中使用 `setDefaultEncoding` 方法为可写流设置默认编码,并提供了详细的语法说明和示例代码。 ... [详细]
  • Redux入门指南
    本文介绍Redux的基本概念和工作原理,帮助初学者理解如何使用Redux管理应用程序的状态。Redux是一个用于JavaScript应用的状态管理库,特别适用于React项目。 ... [详细]
  • 本文介绍了在CentOS 6.4系统中安装MySQL 5.5.37时遇到的启动失败和PID文件问题,并提供了详细的解决方案,包括日志分析、权限检查等步骤。 ... [详细]
  • 本文探讨了如何使用ls -lsh命令排除总大小输出,仅显示文件大小的方法,并提供了几种实现这一目标的解决方案。 ... [详细]
  • 在编译BSP包过程中,遇到了一个与 'gets' 函数相关的编译错误。该问题通常发生在较新的编译环境中,由于 'gets' 函数已被弃用并视为安全漏洞。本文将详细介绍如何通过修改源代码和配置文件来解决这一问题。 ... [详细]
author-avatar
素手红裳000_367
这个家伙很懒,什么也没留下!
PHP1.CN | 中国最专业的PHP中文社区 | DevBox开发工具箱 | json解析格式化 |PHP资讯 | PHP教程 | 数据库技术 | 服务器技术 | 前端开发技术 | PHP框架 | 开发工具 | 在线工具
Copyright © 1998 - 2020 PHP1.CN. All Rights Reserved | 京公网安备 11010802041100号 | 京ICP备19059560号-4 | PHP1.CN 第一PHP社区 版权所有