Skip to content

Commit

Permalink
Did the task
Browse files Browse the repository at this point in the history
  • Loading branch information
@metametamoon
metametamoon committed Mar 18, 2024
1 parent 63c510f commit a248ae5
Show file tree
Hide file tree
Showing 6 changed files with 570 additions and 343 deletions.
213 changes: 114 additions & 99 deletions src/phg/sfm/ematrix.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,23 +17,25 @@ namespace {
Eigen::MatrixXd E;
copy(Ecv, E);

Eigen::JacobiSVD<Eigen::MatrixXd> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
throw std::runtime_error("not implemented yet");
// TODO

Eigen::JacobiSVD<Eigen::MatrixXd> svd(E);
auto epsilon = 0.0001;
auto singularValues = svd.singularValues();
std::cout << "singularValues = " << singularValues << std::endl;
// assert(abs(singularValues[0] - singularValues[1]) < epsilon);
// assert(abs(singularValues[2]) < epsilon);
copy(E, Ecv);
}

}

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 = calib0.K().t() * F * calib1.K();
matrix3d E = calib1.K().t() * F * calib0.K();

ensureSpectralProperty(E);

return E;
}

namespace {
Expand Down Expand Up @@ -61,21 +63,27 @@ 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;
// throw std::runtime_error("not implemented yet");
// скомпенсировать калибровки камер
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];
}
auto is_on_cam = [&X](matrix34d const& M) {
auto rotation = M.get_minor<3, 3>(0, 0).inv();
auto cam_pos = -rotation * M.get_minor<3, 1>(0, 3);
auto camera_z_axis = rotation.get_minor<3, 1>(0, 2);
return (X.get_minor<3, 1>(0, 0) - cam_pos).dot(camera_z_axis) >= 0.0;
};

// точка должна иметь положительную глубину для обеих камер
return is_on_cam(calib0.K().inv() * P0) && is_on_cam(calib1.K().inv() * P1);
}
}

Expand All @@ -86,82 +94,89 @@ namespace {
// Это дополнительное ограничение позволяет разложить существенную матрицу с точностью до 4 решений, вместо произвольного проективного преобразования (см. Hartley & Zisserman p.258)
// Обычно мы можем использовать одну общую калибровку, более менее верную для большого количества реальных камер и с ее помощью выполнить
// первичное разложение существенной матрицы (а из него, взаимное расположение камер) для последующего уточнения методом нелинейной оптимизации
//
// Возвращает матрицу камеры без калибровки
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;
// 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);

Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
W(0, 1) = -1.0;
W(1, 0) = 1.0;
W(2, 2) = 1.0;

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 = 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(Eigen::all, 2);
vec t1 = U(Eigen::all, 2) * -1.0;

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
Loading

0 comments on commit a248ae5

Please sign in to comment.