Skip to content

Commit

Permalink
Done resection
Browse files Browse the repository at this point in the history
  • Loading branch information
I-7 committed Jun 12, 2024
1 parent 43dfade commit 4483d60
Showing 1 changed file with 104 additions and 83 deletions.
187 changes: 104 additions & 83 deletions src/phg/sfm/resection.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,95 +49,116 @@ namespace {
// (см. Hartley & Zisserman p.178)
cv::Matx34d estimateCameraMatrixDLT(const cv::Vec3d *Xs, const cv::Vec3d *xs, int count)
{
throw std::runtime_error("not implemented yet");
// using mat = Eigen::MatrixXd;
// using vec = Eigen::VectorXd;
//
// mat A(TODO);
//
// for (int i = 0; i < count; ++i) {
//
// double x = xs[i][0];
// double y = xs[i][1];
// double w = xs[i][2];
//
// double X = Xs[i][0];
// double Y = Xs[i][1];
// double Z = Xs[i][2];
// double W = 1.0;
//
// TODO
// }
//
// matrix34d result;
// TODO
//
// return canonicalizeP(result);
using mat = Eigen::MatrixXd;
using vec = Eigen::VectorXd;

mat A(2 * count, 12);

for (int i = 0; i < count; ++i) {

double x = xs[i][0];
double y = xs[i][1];
double w = xs[i][2];

double X = Xs[i][0];
double Y = Xs[i][1];
double Z = Xs[i][2];
double W = 1.0;

A.row(2 * i) <<
0, 0, 0, 0,
-w * X, -w * Y, -w * Z, -w * W,
y * X, y * Y, y * Z, y * W;
A.row(2 * i + 1) <<
w * X, w * Y, w * Z, w * W,
0, 0, 0, 0,
-x * X, -x * Y, -x * Z, -x * W;
}

mat result(3, 4);

Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::VectorXd null_space = svd.matrixV().col(11);

result.row(0) << null_space[0], null_space[1], null_space[2], null_space[3];
result.row(1) << null_space[4], null_space[5], null_space[6], null_space[7];
result.row(2) << null_space[8], null_space[9], null_space[10], null_space[11];

cv::Matx34d resultcv;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 4; j++) {
resultcv(i, j) = result(i, j);
}
}

return canonicalizeP(resultcv);
}


// По трехмерным точкам и их проекциям на изображении определяем положение камеры
cv::Matx34d estimateCameraMatrixRANSAC(const phg::Calibration &calib, const std::vector<cv::Vec3d> &X, const std::vector<cv::Vec2d> &x)
{
throw std::runtime_error("not implemented yet");
// if (X.size() != x.size()) {
// throw std::runtime_error("estimateCameraMatrixRANSAC: X.size() != x.size()");
// }
//
// const int n_points = X.size();
//
// // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters
// // будет отличаться от случая с гомографией
// const int n_trials = TODO;
//
// const double threshold_px = 3;
//
// const int n_samples = TODO;
// uint64_t seed = 1;
//
// int best_support = 0;
// cv::Matx34d best_P;
//
// std::vector<int> sample;
// for (int i_trial = 0; i_trial < n_trials; ++i_trial) {
// phg::randomSample(sample, n_points, n_samples, &seed);
//
// cv::Vec3d ms0[n_samples];
// cv::Vec3d ms1[n_samples];
// for (int i = 0; i < n_samples; ++i) {
// ms0[i] = TODO;
// ms1[i] = TODO;
// }
//
// cv::Matx34d P = estimateCameraMatrixDLT(ms0, ms1, n_samples);
//
// int support = 0;
// for (int i = 0; i < n_points; ++i) {
// cv::Vec2d px = TODO спроецировать 3Д точку в пиксель с использованием P и calib;
// if (cv::norm(px - x[i]) < threshold_px) {
// ++support;
// }
// }
//
// if (support > best_support) {
// best_support = support;
// best_P = P;
//
// std::cout << "estimateCameraMatrixRANSAC : support: " << best_support << "/" << n_points << std::endl;
//
// if (best_support == n_points) {
// break;
// }
// }
// }
//
// std::cout << "estimateCameraMatrixRANSAC : best support: " << best_support << "/" << n_points << std::endl;
//
// if (best_support == 0) {
// throw std::runtime_error("estimateCameraMatrixRANSAC : failed to estimate camera matrix");
// }
//
// return best_P;
if (X.size() != x.size()) {
throw std::runtime_error("estimateCameraMatrixRANSAC: X.size() != x.size()");
}

const int n_points = X.size();

// https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters
// будет отличаться от случая с гомографией

const double threshold_px = 3;

const int n_samples = 6;
const double prob_ok = 0.42;
const int n_trials = std::round(1 + 4 * std::log(1 - 0.999) / std::log(1 - std::pow(prob_ok, n_samples)));
std::cout << n_trials << std::endl;
uint64_t seed = 1;

int best_support = 0;
cv::Matx34d best_P;

std::vector<int> sample;
for (int i_trial = 0; i_trial < n_trials; ++i_trial) {
phg::randomSample(sample, n_points, n_samples, &seed);

cv::Vec3d ms0[n_samples];
cv::Vec3d ms1[n_samples];
for (int i = 0; i < n_samples; ++i) {
ms0[i] = X[sample[i]];
ms1[i] = calib.unproject(x[sample[i]]);
}

cv::Matx34d P = estimateCameraMatrixDLT(ms0, ms1, n_samples);

int support = 0;
for (int i = 0; i < n_points; ++i) {
cv::Vec3d px = calib.project(P * cv::Vec4d(X[i](0), X[i](1), X[i](2), 1));
cv::Vec2d x1(px(0) / px(2), px(1) / px(2));
if (cv::norm(x1 - x[i]) < threshold_px) {
++support;
}
}

if (support > best_support) {
best_support = support;
best_P = P;

std::cout << "estimateCameraMatrixRANSAC : support: " << best_support << "/" << n_points << std::endl;

if (best_support == n_points) {
break;
}
}
}

std::cout << "estimateCameraMatrixRANSAC : best support: " << best_support << "/" << n_points << std::endl;

if (best_support == 0) {
throw std::runtime_error("estimateCameraMatrixRANSAC : failed to estimate camera matrix");
}

return best_P;
}


Expand Down

0 comments on commit 4483d60

Please sign in to comment.