Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Update the new version of G2O interface. #110

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Update the new version of G2O interface. #110

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8e7dcaf
Update new version of G2O
zhkmxx9302013 Jun 2, 2018
a442111
Update new verison of G2O
zhkmxx9302013 Jun 2, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 14 additions & 6 deletions ch6/g2o_curve_fitting/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,12 +73,20 @@ int main( int argc, char** argv )

// 构建图优化，先设定g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3，误差值维度为1
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
Block* solver_ptr = new Block( linearSolver ); // 矩阵块求解器

//-----修改以下注释到下行代码-----------//
// Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverDense<Block::PoseMatrixType>());

//-----修改以下注释到下行代码-----------//
//Block* solver_ptr = new Block( linearSolver ); // 矩阵块求解器
std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver)));

// 梯度下降方法，从GN, LM, DogLeg 中选
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
//-----修改以下注释到下行代码-----------//
// g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));

g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm( solver ); // 设置求解器
optimizer.setVerbose( true ); // 打开调试输出
Expand Down Expand Up @@ -114,4 +122,4 @@ int main( int argc, char** argv )
cout<<"estimated model: "<<abc_estimate.transpose()<<endl;

return 0;
}
}
19 changes: 12 additions & 7 deletions ch7/pose_estimation_3d2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -146,27 +146,32 @@ void bundleAdjustment (
Mat& R, Mat& t )
{
// 初始化g2o
// 初始化部分需要改成unique_ptr的， 2018年06月01日17:54:25
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block; // pose 维度为 6, landmark 维度为 3
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
Block* solver_ptr = new Block ( linearSolver ); // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
std::unique_ptr<Block::LinearSolverType> linearSolver (new g2o::LinearSolverCSparse<Block::PoseMatrixType>()); // 线性方程求解器
std::unique_ptr<Block> solver_ptr( new Block(std::move(linearSolver))); // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr) );
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );

// vertex
// vertex(se(3), R, t
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
Eigen::Matrix3d R_mat;
R_mat <<
R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );

pose->setId ( 0 );
pose->setEstimate ( g2o::SE3Quat (
pose->setEstimate ( g2o::SE3Quat ( //si yuan shu {R}, t
R_mat,
Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
) );
)
);

optimizer.addVertex ( pose );

// vertex (Pos in world axes)
int index = 1;
for ( const Point3f p:points_3d ) // landmarks
{
Expand Down