// Transform a point from the camera plane to the image plane
Vec2 cam2ima(const Vec2& p) const
{
return focal() * p + principal_point();
}
// Transform a point from the image plane to the camera plane
Vec2 ima2cam(const Vec2& p) const
{
return ( p - principal_point() ) / focal();
}
void keypoint_map::optimize() {
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(true);
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverCholmod<
g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver =
new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
double focal_length = intrinsics[0];
Eigen::Vector2d principal_point(intrinsics[2], intrinsics[3]);
g2o::CameraParameters * cam_params = new g2o::CameraParameters(focal_length,
principal_point, 0.);
cam_params->setId(0);
if (!optimizer.addParameter(cam_params)) {
assert(false);
}
std::cerr << camera_positions.size() << " " << keypoints3d.size() << " "
<< observations.size() << std::endl;
int vertex_id = 0, point_id = 0;
for (size_t i = 0; i < camera_positions.size(); i++) {
Eigen::Affine3f cam_world = camera_positions[i].inverse();
Eigen::Vector3d trans(cam_world.translation().cast<double>());
Eigen::Quaterniond q(cam_world.rotation().cast<double>());
g2o::SE3Quat pose(q, trans);
g2o::VertexSE3Expmap * v_se3 = new g2o::VertexSE3Expmap();
v_se3->setId(vertex_id);
if (i < 1) {
v_se3->setFixed(true);
}
v_se3->setEstimate(pose);
optimizer.addVertex(v_se3);
vertex_id++;
}
for (size_t i = 0; i < keypoints3d.size(); i++) {
g2o::VertexSBAPointXYZ * v_p = new g2o::VertexSBAPointXYZ();
v_p->setId(vertex_id + point_id);
v_p->setMarginalized(true);
v_p->setEstimate(keypoints3d[i].getVector3fMap().cast<double>());
optimizer.addVertex(v_p);
point_id++;
}
for (size_t i = 0; i < observations.size(); i++) {
g2o::EdgeProjectXYZ2UV * e = new g2o::EdgeProjectXYZ2UV();
e->setVertex(0,
dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertices().find(
vertex_id + observations[i].point_id)->second));
e->setVertex(1,
dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertices().find(
observations[i].cam_id)->second));
e->setMeasurement(observations[i].coord.cast<double>());
e->information() = Eigen::Matrix2d::Identity();
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
e->setParameterId(0, 0);
optimizer.addEdge(e);
}
//optimizer.save("debug.txt");
optimizer.initializeOptimization();
optimizer.setVerbose(true);
std::cout << std::endl;
std::cout << "Performing full BA:" << std::endl;
optimizer.optimize(1);
std::cout << std::endl;
for (int i = 0; i < vertex_id; i++) {
g2o::HyperGraph::VertexIDMap::iterator v_it = optimizer.vertices().find(
i);
if (v_it == optimizer.vertices().end()) {
std::cerr << "Vertex " << i << " not in graph!" << std::endl;
exit(-1);
}
g2o::VertexSE3Expmap * v_c =
dynamic_cast<g2o::VertexSE3Expmap *>(v_it->second);
if (v_c == 0) {
std::cerr << "Vertex " << i << "is not a VertexSE3Expmap!"
<< std::endl;
exit(-1);
}
Eigen::Affine3f pos;
pos.fromPositionOrientationScale(
v_c->estimate().translation().cast<float>(),
v_c->estimate().rotation().cast<float>(),
Eigen::Vector3f(1, 1, 1));
camera_positions[i] = pos.inverse();
}
for (int i = 0; i < point_id; i++) {
g2o::HyperGraph::VertexIDMap::iterator v_it = optimizer.vertices().find(
vertex_id + i);
if (v_it == optimizer.vertices().end()) {
std::cerr << "Vertex " << vertex_id + i << " not in graph!"
<< std::endl;
exit(-1);
}
g2o::VertexSBAPointXYZ * v_p =
dynamic_cast<g2o::VertexSBAPointXYZ *>(v_it->second);
if (v_p == 0) {
std::cerr << "Vertex " << vertex_id + i
<< "is not a VertexSE3Expmap!" << std::endl;
exit(-1);
}
keypoints3d[i].getVector3fMap() = v_p->estimate().cast<float>();
}
}
