SFM with G2O Issue

[Smithangshu]

Hi sir,

First of all, thank you for your effort to make such a great library. I am facing an issue while I am trying to use G2O to Bundle Adjust SFM solution. Without the Bundle Adjustment performed we get the result which is almost very close to reality. But after Bundle Adjustment the result we get is completely wrong especially the camera poses, for the 3d points they don't differ much.

I am using following code for Bundle Adjustment using G2O.

#define OPENCV_TRAITS_ENABLE_DEPRECATED
#define G2O_USE_VENDORED_CERES
#include <opencv2/opencv.hpp>
#include <opencv2/viz/vizcore.hpp>
#include <g2o/config.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/icp/types_icp.h>
#include <g2o/solvers/structure_only/structure_only_solver.h>
#include <g2o/stuff/sampler.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_sba.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/sim3/types_seven_dof_expmap.h>

....

	optimizer.setVerbose(false);
	std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver = g2o::make_unique<g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>>();
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));
	optimizer.setAlgorithm(solver);
	double focal_length = K.at<double>(0, 0);
	Eigen::Vector2d principal_point(K.at<double>(0, 2), K.at<double>(1, 2));
	g2o::CameraParameters* cam_params = new g2o::CameraParameters(focal_length, principal_point, 0.);
	cam_params->setId(0);
	if (!optimizer.addParameter(cam_params)) {
		assert(false);
	}
	for (int i = 0; i < SFM.img_pose.size(); i++) {
		SFM_Helper::ImagePose campose = SFM.img_pose[i];
		Mat R = campose.T(Rect(0, 0, 3, 3));
		Mat t = campose.T(Rect(3, 0, 1, 3));
		g2o::Matrix3 Rei(R.t().operator cv::Mat().ptr<double>());
		g2o::Vector3 trans(t.at<double>(0), t.at<double>(1), t.at<double>(2));
		g2o::SE3Quat pose = g2o::SE3Quat(Rei, trans);
		g2o::VertexSE3Expmap* v_se3 = new g2o::VertexSE3Expmap();
		v_se3->setId(i);
		if (i < 1)
			v_se3->setFixed(true);
		v_se3->setEstimate(pose);
		optimizer.addVertex(v_se3);
	}
	for (int s = 0; s < SFM.landmark.size(); s++) {
		SFM_Helper::Landmark mapPoint = SFM.landmark[s];
		if (mapPoint.seen >= MIN_LANDMARK_SEEN)
		{
			g2o::VertexPointXYZ* v_p = new g2o::VertexPointXYZ();
			v_p->setId(s + SFM.img_pose.size());
			v_p->setMarginalized(true);
			v_p->setEstimate(g2o::Vector3(mapPoint.pt.x, mapPoint.pt.y, mapPoint.pt.z));
			optimizer.addVertex(v_p);
			for (int f = 0; f < SFM.img_pose.size(); f++) {
				if (mapPoint.lm_exists.count(f) > 0)
				{
					size_t kp_idx = mapPoint.lm_exists[f];
					KeyPoint kp = SFM.img_pose[f].kp[kp_idx];
					g2o::EdgeProjectXYZ2UV* e = new g2o::EdgeProjectXYZ2UV();
					e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p));
					e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(f)));

					e->setMeasurement(g2o::Vector2(kp.pt.x, kp.pt.y));
					e->information() = Eigen::Matrix2d::Identity();
					g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
					e->setRobustKernel(rk);
					e->setParameterId(0, 0);
					optimizer.addEdge(e);
				}
			}
		}
	}
	optimizer.initializeOptimization();
	optimizer.setVerbose(true);
	optimizer.optimize(10);
	for (int p = 0; p < SFM.img_pose.size(); p++)
	{
		SFM_Helper::ImagePose& pose = SFM.img_pose[p];
		auto& est = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(p))->estimate();
		Eigen::Matrix3d Re = est.rotation().matrix();
		g2o::Vector3 Te = est.translation();
		Mat R, t;
		R = Mat(int(Re.rows()), int(Re.cols()), CV_64F, Re.data());
		t = Mat(int(Te.rows()), int(Te.cols()), CV_64F, Te.data());
		Mat T = Mat::eye(4, 4, CV_64F);
		R.t().operator cv::Mat().copyTo(T(Rect(0, 0, 3, 3)));
		t.copyTo(T(Rect(3, 0, 1, 3)));
		pose.T = T.clone().inv();
	}
	for (int p = 0; p < SFM.landmark.size(); p++) {
		if (SFM.landmark[p].seen >= MIN_LANDMARK_SEEN)
		{
			SFM_Helper::Landmark& l = SFM.landmark[p];
			auto est = dynamic_cast<g2o::VertexPointXYZ*>(optimizer.vertex(p + SFM.img_pose.size()))->estimate();
			l.pt = Point3f(est(0), est(1), est(2));
		}
	}

What I think is we are doing something wrong. Can you please help us to fix this?

[Smithangshu]

Without Bundle Adjustment I am getting the result like following But after doing Bundle Adjustment I am getting result like following The 5th pose goes completely wrong. I am completely clueless about what is wrong here. I tried to remove the 5th image and run it with only 4 images to verify if I have any wrong code regarding the last pose frame or pose. But with only 4 images result is correct no problem with the 4th image/pose is obsereved.