Skip to content

Commit

Permalink
Added nocalib marks
Browse files Browse the repository at this point in the history
  • Loading branch information
@metametamoon
metametamoon committed Mar 18, 2024
1 parent c2bf586 commit e998e2b
Show file tree
Hide file tree
Showing 5 changed files with 103 additions and 102 deletions.
158 changes: 79 additions & 79 deletions src/phg/sfm/ematrix.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,95 +80,95 @@ namespace {
}

// Матрицы камер для фундаментальной матрицы определены с точностью до проективного преобразования
// То есть, можно исказить трехмерный мир (применив 4-мерную однородную матрицу), и одновременно поменять матрицы P0, P1 так, что проекции в пикселях не изменятся
// Если мы знаем калибровки камер (матрицы K0, K1 в структуре матриц P0, P1), то можем наложить дополнительные ограничения, в частности, известно, что
// То есть, можно исказить трехмерный мир (применив 4-мерную однородную матрицу), и одновременно поменять матрицы P0_nocalib, P1_nocalib так, что проекции в пикселях не изменятся
// Если мы знаем калибровки камер (матрицы K0, K1 в структуре матриц P0_nocalib, P1_nocalib), то можем наложить дополнительные ограничения, в частности, известно, что
// существенная матрица (Essential matrix = K1t * F * K0) имеет ровно два совпадающих ненулевых сингулярных значения, тогда как для фундаментальной матрицы они могут различаться
// Это дополнительное ограничение позволяет разложить существенную матрицу с точностью до 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)
void phg::decomposeEMatrix(cv::Matx34d &P0_nocalib, cv::Matx34d &P1_nocalib, 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;


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_nocalib = 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_nocalib, 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_nocalib = P1s[best_idx];

std::cout << "best idx: " << best_idx << std::endl;
std::cout << "P0_nocalib: \n" << P0_nocalib << std::endl;
std::cout << "P1_nocalib: \n" << P1_nocalib << std::endl;
}

void phg::decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P)
void phg::decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P_nocalib)
{
R = P.get_minor<3, 3>(0, 0);
R = P_nocalib.get_minor<3, 3>(0, 0);

cv::Matx31d O_mat = -R.t() * P.get_minor<3, 1>(0, 3);
cv::Matx31d O_mat = -R.t() * P_nocalib.get_minor<3, 1>(0, 3);
O(0) = O_mat(0);
O(1) = O_mat(1);
O(2) = O_mat(2);
Expand Down
4 changes: 2 additions & 2 deletions src/phg/sfm/ematrix.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,9 @@ namespace phg {

cv::Matx33d fmatrix2ematrix(const cv::Matx33d &F, const Calibration &calib0, const Calibration &calib1);

void decomposeEMatrix(cv::Matx34d &P0, cv::Matx34d &P1, const cv::Matx33d &E, const std::vector<cv::Vec2d> &m0, const std::vector<cv::Vec2d> &m1, const Calibration &calib0, const Calibration &calib1);
void decomposeEMatrix(cv::Matx34d &P0_nocalib, cv::Matx34d &P1_nocalib, const cv::Matx33d &E, const std::vector<cv::Vec2d> &m0, const std::vector<cv::Vec2d> &m1, const Calibration &calib0, const Calibration &calib1);

void decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P);
void decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P_nocalib);
cv::Matx34d composeCameraMatrixRO(const cv::Matx33d &R, const cv::Vec3d &O);

cv::Matx33d composeEMatrixRT(const cv::Matx33d &R, const cv::Vec3d &T);
Expand Down
17 changes: 9 additions & 8 deletions src/phg/sfm/resection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,12 +78,13 @@ namespace {
}


// По трехмерным точкам и их проекциям на изображении определяем положение камеры
cv::Matx34d estimateCameraMatrixRANSAC(const phg::Calibration &calib, const std::vector<cv::Vec3d> &X, const std::vector<cv::Vec2d> &x)
// По трехмерным точкам и их проекциям на изображении определяем положение камеры без калибровки
// (то есть функция возвращает матрицу (R|t); см. также функцию canonicalizeP
cv::Matx34d estimateCameraMatrixNocalibRANSAC(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()");
// throw std::runtime_error("estimateCameraMatrixNocalibRANSAC: X.size() != x.size()");
// }
//
// const int n_points = X.size();
Expand Down Expand Up @@ -125,18 +126,18 @@ namespace {
// best_support = support;
// best_P = P;
//
// std::cout << "estimateCameraMatrixRANSAC : support: " << best_support << "/" << n_points << std::endl;
// std::cout << "estimateCameraMatrixNocalibRANSAC : support: " << best_support << "/" << n_points << std::endl;
//
// if (best_support == n_points) {
// break;
// }
// }
// }
//
// std::cout << "estimateCameraMatrixRANSAC : best support: " << best_support << "/" << n_points << std::endl;
// std::cout << "estimateCameraMatrixNocalibRANSAC : best support: " << best_support << "/" << n_points << std::endl;
//
// if (best_support == 0) {
// throw std::runtime_error("estimateCameraMatrixRANSAC : failed to estimate camera matrix");
// throw std::runtime_error("estimateCameraMatrixNocalibRANSAC : failed to estimate camera matrix");
// }
//
// return best_P;
Expand All @@ -145,6 +146,6 @@ namespace {

}

cv::Matx34d phg::findCameraMatrix(const Calibration &calib, const std::vector <cv::Vec3d> &X, const std::vector <cv::Vec2d> &x) {
return estimateCameraMatrixRANSAC(calib, X, x);
cv::Matx34d phg::findCameraMatrixNocalib(const Calibration &calib, const std::vector <cv::Vec3d> &X, const std::vector <cv::Vec2d> &x) {
return estimateCameraMatrixNocalibRANSAC(calib, X, x);
}
2 changes: 1 addition & 1 deletion src/phg/sfm/resection.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,6 @@

namespace phg {

cv::Matx34d findCameraMatrix(const Calibration &calib, const std::vector<cv::Vec3d> &X, const std::vector<cv::Vec2d> &x);
cv::Matx34d findCameraMatrixNocalib(const Calibration &calib, const std::vector<cv::Vec3d> &X, const std::vector<cv::Vec2d> &x);

}
24 changes: 12 additions & 12 deletions tests/test_sfm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
#include <random>


#define ENABLE_MY_SFM 0
#define ENABLE_MY_SFM 1

namespace {

Expand Down Expand Up @@ -550,13 +550,13 @@ TEST (SFM, Resection) {
matrix3d F = phg::findFMatrix(points1, points2);
matrix3d E = phg::fmatrix2ematrix(F, calib0, calib1);

matrix34d P0, P1;
phg::decomposeEMatrix(P0, P1, E, points1, points2, calib0, calib1);
matrix34d P0_nocalib, P1_nocalib;
phg::decomposeEMatrix(P0_nocalib, P1_nocalib, E, points1, points2, calib0, calib1);

matrix3d R0, R1;
vector3d O0, O1;
phg::decomposeUndistortedPMatrix(R0, O0, P0);
phg::decomposeUndistortedPMatrix(R1, O1, P1);
phg::decomposeUndistortedPMatrix(R0, O0, P0_nocalib);
phg::decomposeUndistortedPMatrix(R1, O1, P1_nocalib);

std::cout << "Camera positions: " << std::endl;
std::cout << "R0:\n" << R0 << std::endl;
Expand All @@ -568,7 +568,7 @@ TEST (SFM, Resection) {
std::vector<cv::Vec2d> x0s;
std::vector<cv::Vec2d> x1s;

matrix34d Ps[2] = {P0, P1};
matrix34d Ps[2] = {P0_nocalib, P1_nocalib};
for (int i = 0; i < (int) good_matches_gms.size(); ++i) {
vector3d ms[2] = {calib0.unproject(points1[i]), calib1.unproject(points2[i])};
vector4d X = phg::triangulatePoint(Ps, ms, 2);
Expand All @@ -583,12 +583,12 @@ TEST (SFM, Resection) {
x1s.push_back(points2[i]);
}

matrix34d P0res = phg::findCameraMatrix(calib0, Xs, x0s);
matrix34d P1res = phg::findCameraMatrix(calib1, Xs, x1s);
matrix34d P0res = phg::findCameraMatrixNocalib(calib0, Xs, x0s);
matrix34d P1res = phg::findCameraMatrixNocalib(calib1, Xs, x1s);

double rms0 = matRMS(P0res, P0);
double rms1 = matRMS(P1res, P1);
double rms2 = matRMS(P0, P1);
double rms0 = matRMS(P0res, P0_nocalib);
double rms1 = matRMS(P1res, P1_nocalib);
double rms2 = matRMS(P0_nocalib, P1_nocalib);

EXPECT_LT(rms0, 0.005);
EXPECT_LT(rms1, 0.005);
Expand Down Expand Up @@ -742,7 +742,7 @@ TEST (SFM, ReconstructNViews) {
}
}

matrix34d P = phg::findCameraMatrix(calib0, Xs, xs);
matrix34d P = phg::findCameraMatrixNocalib(calib0, Xs, xs);

cameras[i_camera] = P;
aligned[i_camera] = true;
Expand Down

0 comments on commit e998e2b

Please sign in to comment.