その3ではさらっと流したRobust Estimationについて、ソースコードレベルでもう少し詳しく覗いてみたいと思います。
src\openMVG_Samples\multiview_robust_essential\robust_essential.cpp
//B. Compute the relative pose thanks to a essential matrix estimation
const std::pair<size_t, size_t> size_imaL(imageL.Width(), imageL.Height());
const std::pair<size_t, size_t> size_imaR(imageR.Width(), imageR.Height());
sfm::RelativePose_Info relativePose_info;
if (!sfm::robustRelativePose(&camL, &camR, xL, xR, relativePose_info, size_imaL, size_imaR, 256))
openMVG::sfm::robustRelativePose
src\openMVG\sfm\pipelines\sfm_robust_model_estimation.cpp
// Compute the bearing vectors
const Mat3X
bearing1 = (*intrinsics1)(x1),
bearing2 = (*intrinsics2)(x2);
const Mat3X
bearing1 = (*intrinsics1)(x1),
bearing2 = (*intrinsics2)(x2);
ここで画像上の座標x1とx2を正規化しています。この正規化については、「Multiple View Geometry in Computer Vision(Richard Hartley, Andrew Zisserman著)」の「9.6 The Essential Matrix」に記載されています。正規化された画像上の座標は元の座標に対してキャリブレーション行列Kの逆数を掛けることで算出できます。CCDカメラの場合のキャリブレーション行列Kは同じ本の式(6.9)に定義されています。要はF行列(Fundamental行列)はカメラの歪み係数が分かっていない状態での算出は難しいので、画像上の座標を正規化してE行列(Essential行列)を求める方式をとっていると私は理解しました。
if (isPinhole(intrinsics1->getType())
&& isPinhole(intrinsics2->getType()))
{
// Define the AContrario adaptor to use the 5 point essential matrix solver.
using KernelType = robust::ACKernelAdaptorEssential<
openMVG::essential::kernel::FivePointSolver,
openMVG::fundamental::kernel::EpipolarDistanceError,
Mat3>;
KernelType kernel(x1, bearing1, size_ima1.first, size_ima1.second,
x2, bearing2, size_ima2.first, size_ima2.second,
dynamic_cast<const cameras::Pinhole_Intrinsic*>(intrinsics1)->K(),
dynamic_cast<const cameras::Pinhole_Intrinsic*>(intrinsics2)->K());
&& isPinhole(intrinsics2->getType()))
{
// Define the AContrario adaptor to use the 5 point essential matrix solver.
using KernelType = robust::ACKernelAdaptorEssential<
openMVG::essential::kernel::FivePointSolver,
openMVG::fundamental::kernel::EpipolarDistanceError,
Mat3>;
KernelType kernel(x1, bearing1, size_ima1.first, size_ima1.second,
x2, bearing2, size_ima2.first, size_ima2.second,
dynamic_cast<const cameras::Pinhole_Intrinsic*>(intrinsics1)->K(),
dynamic_cast<const cameras::Pinhole_Intrinsic*>(intrinsics2)->K());
// Robustly estimation of the Model and its precision
5つの点(FivePointSolver)を使ってEssential行列を求めるカーネルインスタンスを生成します。
const auto ac_ransac_output = robust::ACRANSAC(
kernel, relativePose_info.vec_inliers,
max_iteration_count, &relativePose_info.essential_matrix,
relativePose_info.initial_residual_tolerance, false);
そのカーネルインスタンスを引数にRobust Estimation本体であるrobust::ACRANSACを呼び出します。
openMVG::robust::ACRANSAC
src\openMVG\robust_estimation\robust_estimator_ACRansac.hpp
const double maxThreshold = (precision == std::numeric_limits<double>::infinity()) ?
std::numeric_limits<double>::infinity() :
precision * kernel.normalizer2()(0,0) * kernel.normalizer2()(0,0);
precision * kernel.normalizer2()(0,0) * kernel.normalizer2()(0,0);
// Initialize the NFA computation interface
// (quantified NFA computation is used if a valid upper bound is provided)
acransac_nfa_internal::NFA_Interface<Kernel> nfa_interface
(kernel, maxThreshold, (precision != std::numeric_limits<double>::infinity()));
// (quantified NFA computation is used if a valid upper bound is provided)
acransac_nfa_internal::NFA_Interface<Kernel> nfa_interface
(kernel, maxThreshold, (precision != std::numeric_limits<double>::infinity()));
// Main estimation loop.
for (unsigned int iter = 0; iter < nIter && iter < num_max_iteration; ++iter)
for (unsigned int iter = 0; iter < nIter && iter < num_max_iteration; ++iter)
// Get random samples
if (bACRansacMode)
UniformSample(sizeSample, random_generator, &vec_index, &vec_sample);
if (bACRansacMode)
UniformSample(sizeSample, random_generator, &vec_index, &vec_sample);
全ての特徴点の中からランダムにいくつかの点を抽出します。ここではFivePointSolverが定義されているので5つの点がvec_sampleに抽出されます。
// Fit model(s). Can find up to Kernel::MAX_MODELS solution(s)
std::vector<typename Kernel::Model> vec_models;
kernel.Fit(vec_sample, &vec_models);
std::vector<typename Kernel::Model> vec_models;
kernel.Fit(vec_sample, &vec_models);
抽出したvec_sampleからモデル、つまりEssential行列を算出します。ここでは4つのモデル候補vec_modelsが算出されます。以下で全てのモデル候補を使って、全特徴点の誤差を計算し、最も誤差の小さいモデルを最終候補として選択します。
for (const auto& model_it : vec_models)
{
// Compute residual values
kernel.Errors(model_it, nfa_interface.residuals());
{
// Compute residual values
kernel.Errors(model_it, nfa_interface.residuals());
現在選択されているモデル候補model_itを使って全ての特徴点の誤差を算出します。
// NFA evaluation; If better than the previous: update scoring & inliers indices
std::pair<double, double> nfa_threshold(minNFA, 0.0);
const bool b_better_model_found =
nfa_interface.ComputeNFA_and_inliers(vec_inliers, nfa_threshold);
std::pair<double, double> nfa_threshold(minNFA, 0.0);
const bool b_better_model_found =
nfa_interface.ComputeNFA_and_inliers(vec_inliers, nfa_threshold);
このComputeNFA_and_inliersの中で全ての特徴点の誤差の大きさから誤差の少ないものだけを抽出し、vec_inliersにinlierとして登録します。結果として誤差が今までの最有力候補より小さい場合には、返値bool b_better_model_foundがTrueとなります。
if (b_better_model_found)
{
better = true;
minNFA = nfa_threshold.first;
errorMax = nfa_threshold.second;
if (model) *model = model_it;
{
better = true;
minNFA = nfa_threshold.first;
errorMax = nfa_threshold.second;
if (model) *model = model_it;
そして、新しい最有力候補となるモデル候補が見つかれば、それらの誤差情報と候補となるモデルmodel_itを保持します。
これで全てのモデル候補vec_modelsを精査し、最終候補が *modelとして返されることになります。これが求めようとしていたEssential行列です。
かなり、ざっくりとした説明ですが、なんとなく流れはつかんでいただけたのではないかと思います。