//Second cam is fixed, therefore the transformation from the second to the first cam will be computed std::pair<g2o::VertexSE3*, g2o::VertexSE3*> sensorVerticesSetup(g2o::SparseOptimizer& optimizer, Eigen::Matrix4f& tf_estimate){ g2o::VertexSE3 *vc2 = new VertexSE3(); {// set up rotation and translation for the newer node as identity Eigen::Quaterniond q(1,0,0,0); Eigen::Vector3d t(0,0,0); q.setIdentity(); g2o::SE3Quat cam2(q,t); vc2->setEstimate(cam2); vc2->setId(1); vc2->setFixed(true); optimizer.addVertex(vc2); } g2o::VertexSE3 *vc1 = new VertexSE3(); { Eigen::Quaterniond q(tf_estimate.topLeftCorner<3,3>().cast<double>());//initialize rotation from estimate Eigen::Vector3d t(tf_estimate.topRightCorner<3,1>().cast<double>()); //initialize translation from estimate g2o::SE3Quat cam1(q,t); vc1->setEstimate(cam1); vc1->setId(0); optimizer.addVertex(vc1); } // add to optimizer return std::make_pair(vc1, vc2); }
//!Set parameters for icp optimizer void optimizerSetup(g2o::SparseOptimizer& optimizer){ //optimizer.setMethod(g2o::SparseOptimizer::LevenbergMarquardt); //g2o::Optimizato optimizer.setVerbose(false); // variable-size block solver g2o::BlockSolverX::LinearSolverType * linearSolver = new g2o::LinearSolverCholmod<g2o ::BlockSolverX::PoseMatrixType>(); g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); optimizer.setAlgorithm(solver); //optimizer.setSolver(solver_ptr); g2o::ParameterCamera* cameraParams = new g2o::ParameterCamera(); //FIXME From Parameter server or cam calibration cameraParams->setKcam(521,521,319.5,239.5); g2o::SE3Quat offset; // identity cameraParams->setOffset(offset); cameraParams->setId(0); optimizer.addParameter(cameraParams); }
g2o::VertexSE3* addFrameVertex(const ros::Time& timestamp) { g2o::VertexSE3* frame_vertex = new g2o::VertexSE3(); frame_vertex->setId(max_vertex_id_++); frame_vertex->setUserData(new Timestamped(timestamp)); if(!graph_.addVertex(frame_vertex)) { throw std::runtime_error("failed to add vertex to g2o graph!"); } return frame_vertex; }
LocalMapImpl(const dvo::core::RgbdImagePyramid::Ptr& keyframe, const dvo::core::AffineTransformd& keyframe_pose) : keyframe_(keyframe), keyframe_vertex_(0), previous_vertex_(0), current_vertex_(0), max_vertex_id_(1), max_edge_id_(1) { // g2o setup graph_.setAlgorithm( new g2o::OptimizationAlgorithmLevenberg( new BlockSolver( new LinearSolver() ) ) ); graph_.setVerbose(false); keyframe_vertex_ = addFrameVertex(ros::Time(keyframe->timestamp())); keyframe_vertex_->setFixed(true); keyframe_vertex_->setEstimate(toIsometry(keyframe_pose)); }
g2o::EdgeSE3* addTransformationEdge(g2o::VertexSE3 *from, g2o::VertexSE3 *to, const dvo::core::AffineTransformd& transform, const dvo::core::Matrix6d& information) { assert(from != 0 && to != 0); g2o::EdgeSE3* edge = new g2o::EdgeSE3(); edge->setId(max_edge_id_++); edge->resize(2); edge->setVertex(0, from); edge->setVertex(1, to); edge->setMeasurement(toIsometry(transform)); edge->setInformation(information); graph_.addEdge(edge); return edge; }
void test() { Isometry3d T = Eigen::Isometry3d::Identity(); T(0,0)=1; T(0,1)=0; T(0,2)=0; T(1,0)=0; T(1,1)=0.707; T(1,2)=-0.707; T(2,0)=0; T(2,1)=0.707; T(2,2)=0.707; T(0,3)=0; T(1,3)=0; T(2,3)=0.5; cout<<T.matrix()<<endl; for (int i = 2; i <=8; i++) { g2o::EdgeSE3* edge = new g2o::EdgeSE3(); g2o::VertexSE3 *v = new g2o::VertexSE3(); v->setId(i); v->setEstimate( Eigen::Isometry3d::Identity() ); m_globalOptimizer.addVertex(v); edge->vertices() [0] = m_globalOptimizer.vertex( i-1 ); edge->vertices() [1] = m_globalOptimizer.vertex( i ); Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity(); information(0,0) = information(1,1) = information(2,2) = 100; information(3,3) = information(4,4) = information(5,5) = 100; edge->setInformation( information ); edge->setMeasurement( T ); m_globalOptimizer.addEdge(edge); } #if 0 T = Eigen::Isometry3d::Identity(); //T(2,3)=1; //cout<<T.matrix()<<endl; g2o::EdgeSE3* edge = new g2o::EdgeSE3(); g2o::VertexSE3 *v = new g2o::VertexSE3(); v->setId(8); v->setEstimate( Eigen::Isometry3d::Identity() ); m_globalOptimizer.addVertex(v); edge->vertices() [0] = m_globalOptimizer.vertex( 1 ); edge->vertices() [1] = m_globalOptimizer.vertex( 8 ); Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity(); information(0,0) = information(1,1) = information(2,2) = 100; information(3,3) = information(4,4) = information(5,5) = 100; edge->setInformation( information ); edge->setMeasurement( T ); m_globalOptimizer.addEdge(edge); #endif m_globalOptimizer.save("pa.g2o"); m_globalOptimizer.initializeOptimization(); //m_globalOptimizer.optimize(50); m_globalOptimizer.save("pa2.g2o"); cout<<"f**k"<<endl; VertexSE3* vertex = dynamic_cast<VertexSE3*>(m_globalOptimizer.vertex(2)); Isometry3d pose = vertex -> estimate(); //cout << "matrix:" << endl; cout << pose.matrix() << endl; }