本文整理汇总了C++中SparseOptimizer::setMethod方法的典型用法代码示例。如果您正苦于以下问题：C++ SparseOptimizer::setMethod方法的具体用法？C++ SparseOptimizer::setMethod怎么用？C++ SparseOptimizer::setMethod使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类SparseOptimizer的用法示例。
在下文中一共展示了SparseOptimizer::setMethod方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: main
int main()
{
double euc_noise = 0.01; // noise in position, m
// double outlier_ratio = 0.1;
SparseOptimizer optimizer;
optimizer.setMethod(SparseOptimizer::LevenbergMarquardt);
optimizer.setVerbose(false);
// variable-size block solver
BlockSolverX::LinearSolverType * linearSolver
= new LinearSolverCholmod<g2o
::BlockSolverX::PoseMatrixType>();
BlockSolverX * solver_ptr
= new BlockSolverX(&optimizer,linearSolver);
optimizer.setSolver(solver_ptr);
vector<Vector3d> true_points;
for (size_t i=0;i<1000; ++i)
{
true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
Sample::uniform()-0.5,
Sample::uniform()+10));
}
// set up two poses
int vertex_id = 0;
for (size_t i=0; i<2; ++i)
{
// set up rotation and translation for this node
Vector3d t(0,0,i);
Quaterniond q;
q.setIdentity();
SE3Quat cam(q,t); // camera pose
// set up node
VertexSE3 *vc = new VertexSE3();
vc->estimate() = cam;
vc->setId(vertex_id); // vertex id
cerr << t.transpose() << " | " << q.coeffs().transpose() << endl;
// set first cam pose fixed
if (i==0)
vc->setFixed(true);
// add to optimizer
optimizer.addVertex(vc);
vertex_id++;
}
// set up point matches
for (size_t i=0; i<true_points.size(); ++i)
{
// get two poses
VertexSE3* vp0 =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
VertexSE3* vp1 =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
// calculate the relative 3D position of the point
Vector3d pt0,pt1;
pt0 = vp0->estimate().inverse().map(true_points[i]);
pt1 = vp1->estimate().inverse().map(true_points[i]);
// add in noise
pt0 += Vector3d(Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ));
pt1 += Vector3d(Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ));
// form edge, with normals in varioius positions
Vector3d nm0, nm1;
nm0 << 0, i, 1;
nm1 << 0, i, 1;
nm0.normalize();
nm1.normalize();
Edge_V_V_GICP * e // new edge with correct cohort for caching
= new Edge_V_V_GICP();
e->vertices()[0] // first viewpoint
= dynamic_cast<OptimizableGraph::Vertex*>(vp0);
e->vertices()[1] // second viewpoint
= dynamic_cast<OptimizableGraph::Vertex*>(vp1);
//.........这里部分代码省略.........
示例2: main
int main(int argc, char **argv)
{
int num_points = 0;
// check for arg, # of points to use in projection SBA
if (argc > 1)
num_points = atoi(argv[1]);
double euc_noise = 0.1; // noise in position, m
double pix_noise = 1.0; // pixel noise
// double outlier_ratio = 0.1;
SparseOptimizer optimizer;
optimizer.setMethod(SparseOptimizer::LevenbergMarquardt);
optimizer.setVerbose(false);
// variable-size block solver
BlockSolverX::LinearSolverType * linearSolver
= new LinearSolverCholmod<g2o
::BlockSolverX::PoseMatrixType>();
BlockSolverX * solver_ptr
= new BlockSolverX(&optimizer,linearSolver);
optimizer.setSolver(solver_ptr);
vector<Vector3d> true_points;
for (size_t i=0;i<1000; ++i)
{
true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
Sample::uniform()-0.5,
Sample::uniform()+10));
}
// set up camera params
Vector2d focal_length(500,500); // pixels
Vector2d principal_point(320,240); // 640x480 image
double baseline = 0.075; // 7.5 cm baseline
// set up camera params and projection matrices on vertices
g2o::VertexSCam::setKcam(focal_length[0],focal_length[1],
principal_point[0],principal_point[1],
baseline);
// set up two poses
int vertex_id = 0;
for (size_t i=0; i<2; ++i)
{
// set up rotation and translation for this node
Vector3d t(0,0,i);
Quaterniond q;
q.setIdentity();
SE3Quat cam(q,t); // camera pose
// set up node
VertexSCam *vc = new VertexSCam();
vc->estimate() = cam;
vc->setId(vertex_id); // vertex id
cerr << t.transpose() << " | " << q.coeffs().transpose() << endl;
// set first cam pose fixed
if (i==0)
vc->setFixed(true);
// make sure projection matrices are set
vc->setAll();
// add to optimizer
optimizer.addVertex(vc);
vertex_id++;
}
// set up point matches for GICP
for (size_t i=0; i<true_points.size(); ++i)
{
// get two poses
VertexSE3* vp0 =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
VertexSE3* vp1 =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
// calculate the relative 3D position of the point
Vector3d pt0,pt1;
pt0 = vp0->estimate().inverse().map(true_points[i]);
pt1 = vp1->estimate().inverse().map(true_points[i]);
// add in noise
pt0 += Vector3d(Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ));
//.........这里部分代码省略.........