void EssentialMatrixSolver::refine(const std::vector<Vector2d>& y1s, const std::vector<Vector2d>& y2s,
std::vector<bool> *inliers, Pose *P) const
{
assert(is_configured);
assert(y1s.size() == y2s.size());
const Parameters& p = parameters;
// Assume P1 = [I|0]
Pose& P2 = *P;
size_t n = y1s.size();
assert(y2s.size() == n);
// Triangulate inliers
MidpointTriangulator mpt(Pose(), P2);
vector<Vector3d> triangulations;
vector<Vector2d> observations1, observations2;
triangulations.reserve(n);
observations1.reserve(n);
observations2.reserve(n);
for (size_t i = 0; i < n; i++) {
//if (!(*inliers)[i]) continue;
// Triangulate
const Vector2d& y1 = y1s[i];
const Vector2d& y2 = y2s[i];
Vector3d X = mpt.triangulate(y1, y2);
// Validate
if (std::isnan(X(0)) || std::isnan(X(1)) || std::isnan(X(2)) ||
std::isinf(X(0)) || std::isinf(X(1)) || std::isinf(X(2))||(X(2)<0))
{
(*inliers)[i] = false;
continue;
}
double e1 = (y1 - X.hnormalized()).norm();
double e2 = (y2 - (P2 * X).hnormalized()).norm();
if (e1 + e2 > 2 * p.max_reproj_error) {
(*inliers)[i] = false;
continue;
}
// Store
observations1.push_back(y1s[i]);
observations2.push_back(y2s[i]);
triangulations.push_back(X);
(*inliers)[i] = true;
}
n = triangulations.size();
// Prepare solver
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_SCHUR;
options.minimizer_progress_to_stdout = false;
options.max_num_iterations = 15;
options.function_tolerance = 1e-8; // default 1e-6
options.gradient_tolerance = 1e-10; //default 1e-4*function_tolerance
//ceres::LossFunction* loss = nullptr;
ceres::LossFunction* loss = new ceres::HuberLoss(p.max_reproj_error);
// P1 is [I|0]
double *P2_q = P2.getRRef();
double *P2_t = P2.getTRef();
for (size_t i = 0; i < n; i++) {
Vector2d& y1 = observations1[i];
Vector2d& y2 = observations2[i];
Vector3d& X = triangulations[i];
ceres::CostFunction* cost_fn = PointReprojectionError::Create(y1, y2);
problem.AddResidualBlock(cost_fn, loss, P2_q, P2_t, &X(0));
}
problem.AddResidualBlock(UnitLengthError<3>::Create(),nullptr,P2_t);
ceres::ParameterBlockOrdering *ordering = new ceres::ParameterBlockOrdering();
ordering->AddElementToGroup(P2_q, 1);
ordering->AddElementToGroup(P2_t, 2);
for (size_t i = 0; i < n; i++) {
Vector3d& X = triangulations[i];
ordering->AddElementToGroup(&X(0), 0);
}
options.linear_solver_ordering.reset(ordering);
problem.SetParameterization(P2_q, new ceres::QuaternionParameterization());
// Optimize
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
//cout << summary.FullReport() << endl;
// P2 already has the output since Ceres modifies it directly.
}
