Skip to content

Task03 Василий Можаев СПбГУ #45

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
mozhaa wants to merge 5 commits into
base: task03
Choose a base branch
from
Open

Task03 Василий Можаев СПбГУ #45

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
5a56232
Squashed commit of the following:
mozhaa Mar 19, 2026
8917156
done
mozhaa Mar 19, 2026
3d0da59
fix: ensure positive X(3) in triangulatePoint
mozhaa Mar 19, 2026
0772632
chore: increase n_trials
mozhaa Mar 19, 2026
2e808ce
chore: increase n_trials more
mozhaa Mar 19, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
111 changes: 70 additions & 41 deletions src/phg/matching/descriptor_matcher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,11 +7,16 @@ void phg::DescriptorMatcher::filterMatchesRatioTest(const std::vector<std::vecto
std::vector<cv::DMatch> &filtered_matches)
{
filtered_matches.clear();

throw std::runtime_error("not implemented yet");
const float ratio_threshold = 0.4f;
for (const auto &knn : matches) {
if (knn.size() >= 2) {
if (knn[0].distance < ratio_threshold * knn[1].distance) {
filtered_matches.push_back(knn[0]);
}
}
}
}


void phg::DescriptorMatcher::filterMatchesClusters(const std::vector<cv::DMatch> &matches,
const std::vector<cv::KeyPoint> keypoints_query,
const std::vector<cv::KeyPoint> keypoints_train,
Expand All @@ -35,42 +40,66 @@ void phg::DescriptorMatcher::filterMatchesClusters(const std::vector<cv::DMatch>
points_query.at<cv::Point2f>(i) = keypoints_query[matches[i].queryIdx].pt;
points_train.at<cv::Point2f>(i) = keypoints_train[matches[i].trainIdx].pt;
}
//
// // размерность всего 2, так что точное KD-дерево
// std::shared_ptr<cv::flann::IndexParams> index_params = flannKdTreeIndexParams(TODO);
// std::shared_ptr<cv::flann::SearchParams> search_params = flannKsTreeSearchParams(TODO);
//
// std::shared_ptr<cv::flann::Index> index_query = flannKdTreeIndex(points_query, index_params);
// std::shared_ptr<cv::flann::Index> index_train = flannKdTreeIndex(points_train, index_params);
//
// // для каждой точки найти total neighbors ближайших соседей
// cv::Mat indices_query(n_matches, total_neighbours, CV_32SC1);
// cv::Mat distances2_query(n_matches, total_neighbours, CV_32FC1);
// cv::Mat indices_train(n_matches, total_neighbours, CV_32SC1);
// cv::Mat distances2_train(n_matches, total_neighbours, CV_32FC1);
//
// index_query->knnSearch(points_query, indices_query, distances2_query, total_neighbours, *search_params);
// index_train->knnSearch(points_train, indices_train, distances2_train, total_neighbours, *search_params);
//
// // оценить радиус поиска для каждой картинки
// // NB: radius2_query, radius2_train: квадраты радиуса!
// float radius2_query, radius2_train;
// {
// std::vector<double> max_dists2_query(n_matches);
// std::vector<double> max_dists2_train(n_matches);
// for (int i = 0; i < n_matches; ++i) {
// max_dists2_query[i] = distances2_query.at<float>(i, total_neighbours - 1);
// max_dists2_train[i] = distances2_train.at<float>(i, total_neighbours - 1);
// }
//
// int median_pos = n_matches / 2;
// std::nth_element(max_dists2_query.begin(), max_dists2_query.begin() + median_pos, max_dists2_query.end());
// std::nth_element(max_dists2_train.begin(), max_dists2_train.begin() + median_pos, max_dists2_train.end());
//
// radius2_query = max_dists2_query[median_pos] * radius_limit_scale * radius_limit_scale;
// radius2_train = max_dists2_train[median_pos] * radius_limit_scale * radius_limit_scale;
// }
//
// метч остается, если левое и правое множества первых total_neighbors соседей в радиусах поиска(radius2_query, radius2_train) имеют как минимум consistent_matches общих элементов
// // TODO заполнить filtered_matches

// размерность всего 2, так что точное KD-дерево
std::shared_ptr<cv::flann::IndexParams> index_params = flannKdTreeIndexParams(4);
std::shared_ptr<cv::flann::SearchParams> search_params = flannKsTreeSearchParams(32);

std::shared_ptr<cv::flann::Index> index_query = flannKdTreeIndex(points_query, index_params);
std::shared_ptr<cv::flann::Index> index_train = flannKdTreeIndex(points_train, index_params);

// для каждой точки найти total neighbors ближайших соседей
cv::Mat indices_query(n_matches, total_neighbours, CV_32SC1);
cv::Mat distances2_query(n_matches, total_neighbours, CV_32FC1);
cv::Mat indices_train(n_matches, total_neighbours, CV_32SC1);
cv::Mat distances2_train(n_matches, total_neighbours, CV_32FC1);

index_query->knnSearch(points_query, indices_query, distances2_query, total_neighbours, *search_params);
index_train->knnSearch(points_train, indices_train, distances2_train, total_neighbours, *search_params);

// оценить радиус поиска для каждой картинки
// NB: radius2_query, radius2_train: квадраты радиуса!
float radius2_query, radius2_train;
{
std::vector<double> max_dists2_query(n_matches);
std::vector<double> max_dists2_train(n_matches);
for (int i = 0; i < n_matches; ++i) {
max_dists2_query[i] = distances2_query.at<float>(i, total_neighbours - 1);
max_dists2_train[i] = distances2_train.at<float>(i, total_neighbours - 1);
}

int median_pos = n_matches / 2;
std::nth_element(max_dists2_query.begin(), max_dists2_query.begin() + median_pos, max_dists2_query.end());
std::nth_element(max_dists2_train.begin(), max_dists2_train.begin() + median_pos, max_dists2_train.end());

radius2_query = max_dists2_query[median_pos] * radius_limit_scale * radius_limit_scale;
radius2_train = max_dists2_train[median_pos] * radius_limit_scale * radius_limit_scale;
}

// метч остается, если левое и правое множества первых total_neighbors соседей в радиусах поиска
// (radius2_query, radius2_train) имеют как минимум consistent_matches общих элементов
for (int i = 0; i < n_matches; ++i) {
std::vector<int> neighbors_query, neighbors_train;
for (int j = 0; j < total_neighbours; ++j) {
if (distances2_query.at<float>(i, j) <= radius2_query) {
neighbors_query.push_back(indices_query.at<int>(i, j));
}
}
for (int j = 0; j < total_neighbours; ++j) {
if (distances2_train.at<float>(i, j) <= radius2_train) {
neighbors_train.push_back(indices_train.at<int>(i, j));
}
}

std::sort(neighbors_query.begin(), neighbors_query.end());
std::sort(neighbors_train.begin(), neighbors_train.end());
std::vector<int> common;
std::set_intersection(neighbors_query.begin(), neighbors_query.end(),
neighbors_train.begin(), neighbors_train.end(),
std::back_inserter(common));

if (common.size() >= consistent_matches) {
filtered_matches.push_back(matches[i]);
}
}
}
18 changes: 15 additions & 3 deletions src/phg/matching/flann_matcher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,8 @@
phg::FlannMatcher::FlannMatcher()
{
// параметры для приближенного поиска
// index_params = flannKdTreeIndexParams(TODO);
// search_params = flannKsTreeSearchParams(TODO);
index_params = flannKdTreeIndexParams(4);
search_params = flannKsTreeSearchParams(32);
}

void phg::FlannMatcher::train(const cv::Mat &train_desc)
Expand All @@ -17,5 +17,17 @@ void phg::FlannMatcher::train(const cv::Mat &train_desc)

void phg::FlannMatcher::knnMatch(const cv::Mat &query_desc, std::vector<std::vector<cv::DMatch>> &matches, int k) const
{
throw std::runtime_error("not implemented yet");
cv::Mat indices(query_desc.rows, k, CV_32S);
cv::Mat dists(query_desc.rows, k, CV_32F);
flann_index->knnSearch(query_desc, indices, dists, k, *search_params);

matches.resize(query_desc.rows);
for (int i = 0; i < query_desc.rows; ++i) {
matches[i].resize(k);
for (int j = 0; j < k; ++j) {
int trainIdx = indices.at<int>(i, j);
float distance = dists.at<float>(i, j);
matches[i][j] = cv::DMatch(i, trainIdx, 0, distance);
}
}
}
211 changes: 114 additions & 97 deletions src/phg/sfm/ematrix.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,26 @@ namespace {
copy(Ecv, E);

Eigen::JacobiSVD<Eigen::MatrixXd> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
throw std::runtime_error("not implemented yet");
// TODO
Eigen::MatrixXd U = svd.matrixU();
Eigen::VectorXd s = svd.singularValues();
Eigen::MatrixXd V = svd.matrixV();

if (U.determinant() < 0) {
U = -U;
}
if (V.determinant() < 0) {
V = -V;
}

double avg = (s[0] + s[1]) / 2.0;
s[0] = avg;
s[1] = avg;
s[2] = 0;

Eigen::MatrixXd S = Eigen::MatrixXd::Zero(3, 3);
S(0, 0) = s[0];
S(1, 1) = s[1];
E = U * S * V.transpose();

copy(E, Ecv);
}
Expand All @@ -28,12 +46,9 @@ 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 +76,22 @@ 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];
}

// точка должна иметь положительную глубину для обеих камер
double depth0 = P0(2, 0) * X[0] + P0(2, 1) * X[1] + P0(2, 2) * X[2] + P0(2, 3);
double depth1 = P1(2, 0) * X[0] + P1(2, 1) * X[1] + P1(2, 2) * X[2] + P1(2, 3);
return depth0 > 0 && depth1 > 0;
}
}

Expand All @@ -88,80 +104,81 @@ 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;

mat W(3, 3);
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 = -U.col(2);

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
Loading