Skip to content

Commit

Permalink
Test EmatrixDecomposeSimple
Browse files Browse the repository at this point in the history
  • Loading branch information
@I-7
I-7 committed Jun 12, 2024
1 parent 9cd9601 commit ac161ba
Show file tree
Hide file tree
Showing 2 changed files with 110 additions and 98 deletions.
191 changes: 94 additions & 97 deletions src/phg/sfm/ematrix.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,7 @@ namespace {
copy(Ecv, E);

Eigen::JacobiSVD<Eigen::MatrixXd> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
throw std::runtime_error("not implemented yet");
// TODO
E = svd.matrixU() * Eigen::Vector3d({svd.singularValues()[0], svd.singularValues()[0], 0}).asDiagonal() * svd.matrixV();

copy(E, Ecv);
}
Expand All @@ -28,12 +27,11 @@ namespace {

cv::Matx33d phg::fmatrix2ematrix(const cv::Matx33d &F, const phg::Calibration &calib0, const phg::Calibration &calib1)
{
throw std::runtime_error("not implemented yet");
// matrix3d E = TODO;
//
// ensureSpectralProperty(E);
//
// return E;
matrix3d E = calib1.K().t() * F * calib0.K();

ensureSpectralProperty(E);

return E;
}

namespace {
Expand Down Expand Up @@ -61,21 +59,20 @@ namespace {

bool depthTest(const vector2d &m0, const vector2d &m1, const phg::Calibration &calib0, const phg::Calibration &calib1, const matrix34d &P0, const matrix34d &P1)
{
throw std::runtime_error("not implemented yet");
// // скомпенсировать калибровки камер
// vector3d p0 = TODO;
// vector3d p1 = TODO;
//
// vector3d ps[2] = {p0, p1};
// matrix34d Ps[2] = {P0, P1};
//
// vector4d X = phg::triangulatePoint(Ps, ps, 2);
// if (X[3] != 0) {
// X /= X[3];
// }
//
// // точка должна иметь положительную глубину для обеих камер
// return TODO && TODO;
// скомпенсировать калибровки камер
vector3d p0 = calib0.unproject(m0);
vector3d p1 = calib1.unproject(m1);

vector3d ps[2] = {p0, p1};
matrix34d Ps[2] = {P0, P1};

vector4d X = phg::triangulatePoint(Ps, ps, 2);
if (X[3] != 0) {
X /= X[3];
}

// точка должна иметь положительную глубину для обеих камер
return calib0.project(P0 * X)[2] > 0 && calib1.project(P1 * X)[2] > 0;
}
}

Expand All @@ -88,80 +85,80 @@ namespace {
// первичное разложение существенной матрицы (а из него, взаимное расположение камер) для последующего уточнения методом нелинейной оптимизации
void phg::decomposeEMatrix(cv::Matx34d &P0, cv::Matx34d &P1, const cv::Matx33d &Ecv, const std::vector<cv::Vec2d> &m0, const std::vector<cv::Vec2d> &m1, const Calibration &calib0, const Calibration &calib1)
{
throw std::runtime_error("not implemented yet");
// if (m0.size() != m1.size()) {
// throw std::runtime_error("decomposeEMatrix : m0.size() != m1.size()");
// }
//
// using mat = Eigen::MatrixXd;
// using vec = Eigen::VectorXd;
//
// mat E;
// copy(Ecv, E);
//
// // (см. Hartley & Zisserman p.258)
//
// Eigen::JacobiSVD<mat> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
//
// mat U = svd.matrixU();
// vec s = svd.singularValues();
// mat V = svd.matrixV();
//
// // U, V must be rotation matrices, not just orthogonal
// if (U.determinant() < 0) U = -U;
// if (V.determinant() < 0) V = -V;
//
// std::cout << "U:\n" << U << std::endl;
// std::cout << "s:\n" << s << std::endl;
// std::cout << "V:\n" << V << std::endl;
//
//
// mat R0 = TODO;
// mat R1 = TODO;
//
// std::cout << "R0:\n" << R0 << std::endl;
// std::cout << "R1:\n" << R1 << std::endl;
//
// vec t0 = TODO;
// vec t1 = TODO;
//
// std::cout << "t0:\n" << t0 << std::endl;
//
// P0 = matrix34d::eye();
//
// // 4 possible solutions
// matrix34d P10 = composeP(R0, t0);
// matrix34d P11 = composeP(R0, t1);
// matrix34d P12 = composeP(R1, t0);
// matrix34d P13 = composeP(R1, t1);
// matrix34d P1s[4] = {P10, P11, P12, P13};
//
// // need to select best of 4 solutions: 3d points should be in front of cameras (positive depths)
// int best_count = 0;
// int best_idx = -1;
// for (int i = 0; i < 4; ++i) {
// int count = 0;
// for (int j = 0; j < (int) m0.size(); ++j) {
// if (depthTest(m0[j], m1[j], calib0, calib1, P0, P1s[i])) {
// ++count;
// }
// }
// std::cout << "decomposeEMatrix: count: " << count << std::endl;
// if (count > best_count) {
// best_count = count;
// best_idx = i;
// }
// }
//
// if (best_count == 0) {
// throw std::runtime_error("decomposeEMatrix : can't decompose ematrix");
// }
//
// P1 = P1s[best_idx];
//
// std::cout << "best idx: " << best_idx << std::endl;
// std::cout << "P0: \n" << P0 << std::endl;
// std::cout << "P1: \n" << P1 << std::endl;
if (m0.size() != m1.size()) {
throw std::runtime_error("decomposeEMatrix : m0.size() != m1.size()");
}

using mat = Eigen::MatrixXd;
using vec = Eigen::VectorXd;

mat E;
copy(Ecv, E);

// (см. Hartley & Zisserman p.258)

Eigen::JacobiSVD<mat> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);

mat U = svd.matrixU();
vec s = svd.singularValues();
mat V = svd.matrixV();

// U, V must be rotation matrices, not just orthogonal
if (U.determinant() < 0) U = -U;
if (V.determinant() < 0) V = -V;

std::cout << "U:\n" << U << std::endl;
std::cout << "s:\n" << s << std::endl;
std::cout << "V:\n" << V << std::endl;

Eigen::Matrix3d W;
W << 0, -1, 0, 1, 0, 0, 0, 0, 1;
mat R0 = U * W * V.transpose();
mat R1 = U * W.transpose() * V.transpose();

std::cout << "R0:\n" << R0 << std::endl;
std::cout << "R1:\n" << R1 << std::endl;

vec t0 = U.col(2);
vec t1 = -t0;

std::cout << "t0:\n" << t0 << std::endl;

P0 = matrix34d::eye();

// 4 possible solutions
matrix34d P10 = composeP(R0, t0);
matrix34d P11 = composeP(R0, t1);
matrix34d P12 = composeP(R1, t0);
matrix34d P13 = composeP(R1, t1);
matrix34d P1s[4] = {P10, P11, P12, P13};

// need to select best of 4 solutions: 3d points should be in front of cameras (positive depths)
int best_count = 0;
int best_idx = -1;
for (int i = 0; i < 4; ++i) {
int count = 0;
for (int j = 0; j < (int) m0.size(); ++j) {
if (depthTest(m0[j], m1[j], calib0, calib1, P0, P1s[i])) {
++count;
}
}
std::cout << "decomposeEMatrix: count: " << count << std::endl;
if (count > best_count) {
best_count = count;
best_idx = i;
}
}

if (best_count == 0) {
throw std::runtime_error("decomposeEMatrix : can't decompose ematrix");
}

P1 = P1s[best_idx];

std::cout << "best idx: " << best_idx << std::endl;
std::cout << "P0: \n" << P0 << std::endl;
std::cout << "P1: \n" << P1 << std::endl;
}

void phg::decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P)
Expand Down
17 changes: 16 additions & 1 deletion src/phg/sfm/triangulation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,5 +12,20 @@ cv::Vec4d phg::triangulatePoint(const cv::Matx34d *Ps, const cv::Vec3d *ms, int
{
// составление однородной системы + SVD
// без подвохов
throw std::runtime_error("not implemented yet");

Eigen::MatrixXd A(2 * count, 4);
for (int i = 0; i < count; i++)
{
cv::Matx t0 = ms[i][0] * Ps[i].row(2).t() - Ps[i].row(0).t();
cv::Matx t1 = ms[i][1] * Ps[i].row(2).t() - Ps[i].row(1).t();
A.row(2 * i) << t0(0, 0), t0(1, 0), t0(2, 0), t0(3, 0);
A.row(2 * i + 1) << t1(0, 0), t1(1, 0), t1(2, 0), t1(3, 0);
}
Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
cv::Vec4d res;
for (int i = 0; i < 4; i++)
{
res[i] = svd.matrixV().transpose()(3, i);
}
return res;
}

0 comments on commit ac161ba

Please sign in to comment.