//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);
}
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;
}
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;
}