From 32d431b4b81648215a34a1c2425ffbe1f57023d2 Mon Sep 17 00:00:00 2001 From: saveliy_barkovskiy Date: Mon, 20 Apr 2026 21:26:59 +0300 Subject: [PATCH] task05 --- src/phg/mvs/depth_maps/pm_depth_maps.cpp | 342 +++++++++++++++-------- tests/test_depth_maps_pm.cpp | 70 +++-- 2 files changed, 254 insertions(+), 158 deletions(-) diff --git a/src/phg/mvs/depth_maps/pm_depth_maps.cpp b/src/phg/mvs/depth_maps/pm_depth_maps.cpp index 7021ddd0..808a5c85 100644 --- a/src/phg/mvs/depth_maps/pm_depth_maps.cpp +++ b/src/phg/mvs/depth_maps/pm_depth_maps.cpp @@ -1,4 +1,12 @@ #include "pm_depth_maps.h" + +#include +#include +#include +#include +#include +#include + #include #include #include @@ -8,6 +16,69 @@ #include "pm_geometry.h" namespace phg { +namespace { + +vector3d cameraCenter(const matrix34d& PtoWorld) +{ + return vector3d(PtoWorld(0, 3), PtoWorld(1, 3), PtoWorld(2, 3)); +} + +vector3f normalizeSafe(const vector3f& v, const vector3f& fallback) +{ + float len2 = norm2(v); + if (len2 <= 1e-12f) { + float fallback_len2 = norm2(fallback); + if (fallback_len2 <= 1e-12f) { + return vector3f(0.0f, 0.0f, -1.0f); + } + return fallback * (1.0f / std::sqrt(fallback_len2)); + } + return v * (1.0f / std::sqrt(len2)); +} + +int makeOddAtLeast(int value, int min_value) +{ + value = std::max(value, min_value); + if (value % 2 == 0) { + ++value; + } + return value; +} + +bool sampleBilinearGrey(const cv::Mat& img, double x, double y, float& intensity) +{ + if (!(x >= 0.5 && x <= img.cols - 0.5 && y >= 0.5 && y <= img.rows - 0.5)) { + return false; + } + + double fx = x - 0.5; + double fy = y - 0.5; + + int x0 = static_cast(std::floor(fx)); + int y0 = static_cast(std::floor(fy)); + + x0 = std::max(0, std::min(x0, img.cols - 1)); + y0 = std::max(0, std::min(y0, img.rows - 1)); + + int x1 = std::min(x0 + 1, img.cols - 1); + int y1 = std::min(y0 + 1, img.rows - 1); + + double ax = fx - x0; + double ay = fy - y0; + + float i00 = img.at(y0, x0) / 255.0f; + float i10 = img.at(y0, x1) / 255.0f; + float i01 = img.at(y1, x0) / 255.0f; + float i11 = img.at(y1, x1) / 255.0f; + + intensity = static_cast((1.0 - ax) * (1.0 - ay) * i00 + + ax * (1.0 - ay) * i10 + + (1.0 - ax) * ay * i01 + + ax * ay * i11); + return true; +} + +} // namespace matrix3d extractR(const matrix34d& P) { @@ -41,19 +112,19 @@ vector3d project(const vector3d& global_point, const phg::Calibration& calibrati double depth = local_point[2]; vector3f pixel_with_depth = calibration.project(local_point); - pixel_with_depth[2] = depth; // на самом деле это не глубина, это координата по оси +Z (вдоль которой смотрит камера в ее локальной системе координат) + pixel_with_depth[2] = depth; return pixel_with_depth; } -// TODO 101 реализуйте unproject (вам поможет тест на идемпотентность project -> unproject в test_depth_maps_pm) vector3d unproject(const vector3d& pixel, const phg::Calibration& calibration, const matrix34d& PtoWorld) { - double depth = pixel[2]; // на самом деле это не глубина, это координата по оси +Z (вдоль которой смотрит камера в ее локальной системе координат) + double depth = pixel[2]; - vector3d local_point; // TODO 102 пустите луч pixel из calibration а затем возьмите ан нем точку у которой по оси +Z координата=depth + vector3d ray_local = calibration.unproject(vector2d(pixel[0], pixel[1])); + vector3d local_point = ray_local * depth; - vector3d global_point; // TODO 103 переведите точку из локальной системы в глобальную + vector3d global_point = PtoWorld * homogenize(local_point); return global_point; } @@ -70,14 +141,12 @@ void PMDepthMapsBuilder::buildDepthMap(unsigned int camera_key, cv::Mat& depth_m width = calibration.width(); height = calibration.height(); - // в этих трех картинках мы будем хранить для каждого пикселя лучшую на данный момент найденную гипотезу - depth_map = cv::Mat::zeros(height, width, CV_32FC1); // глубина (точнее координата по оси Z в локальной системе камеры) на которой находится текущая гипотеза (если гипотезы нет - то глубина=0) - normal_map = cv::Mat::zeros(height, width, CV_32FC3); // нормаль к плоскости поверхности текущей гипотезы (единичный вектор в глобальной системе координат) - cost_map = cv::Mat::zeros(height, width, CV_32FC1); // оценка качества этой гипотезы (от 0.0 до 1.0, чем меньше - тем лучше гипотеза) + depth_map = cv::Mat::zeros(height, width, CV_32FC1); + normal_map = cv::Mat::zeros(height, width, CV_32FC3); + cost_map = cv::Mat::zeros(height, width, CV_32FC1); iter = 0; - // в первую очередь нам надо заполнить случайными гипотезами, этим займется refinement refinement(); for (iter = 1; iter <= NITERATIONS; ++iter) { @@ -98,50 +167,53 @@ void PMDepthMapsBuilder::refinement() #pragma omp parallel for schedule(dynamic, 1) for (ptrdiff_t j = 0; j < height; ++j) { for (ptrdiff_t i = 0; i < width; ++i) { - // хотим полного детерминизма, поэтому seed для рандома порождаем из номера итерации + из номера нашего пикселя, - // тем самым получаем полный детерминизм и результат не зависит от числа ядер процессора и в теории может воспроизводиться даже на видеокарте FastRandom r(iter, j * width + i); - // хотим попробовать улучшить текущие гипотезы рассмотрев взаимные комбинации следующих гипотез: float d0, dp, dr; vector3f n0, np, nr; { - // 1) текущей гипотезы (то что уже смогли найти) + float progress = (NITERATIONS > 0) ? static_cast(iter) / static_cast(NITERATIONS) : 1.0f; + float depth_span = ref_depth_max - ref_depth_min; + float depth_rel_radius = std::max(0.03f, 0.5f * (1.0f - progress) + 0.05f); + float depth_abs_radius = depth_span * 0.02f; + float normal_radius = std::max(0.05f, 0.5f * (1.0f - progress) + 0.05f); + d0 = depth_map.at(j, i); n0 = normal_map.at(j, i); - // 2) случайной пертурбации текущей гипотезы (мутация и уточнение того что уже смогли найти) - dp = r.nextf(d0 * 0.5f, d0 * 1.5); // TODO 104: сделайте так чтобы отклонение было тем меньше, чем номер итерации ближе к NITERATIONS, улучшило ли это результат? - np = cv::normalize(n0 + randomNormalObservedFromCamera(cameras_RtoWorld[ref_cam], r) * 0.5); // TODO 105: сделайте так чтобы отклонение было тем меньше, чем номер итерации ближе к NITERATIONS, улучшило ли это результат? + if (d0 == NO_DEPTH || !(d0 >= ref_depth_min && d0 <= ref_depth_max) || norm2(n0) < 0.5f) { + d0 = r.nextf(ref_depth_min, ref_depth_max); + n0 = randomNormalObservedFromCamera(cameras_RtoWorld[ref_cam], r); + } + n0 = normalizeSafe(n0, randomNormalObservedFromCamera(cameras_RtoWorld[ref_cam], r)); - dp = std::max(ref_depth_min, std::min(ref_depth_max, dp)); + depth_abs_radius = std::max(depth_span * 0.02f, d0 * depth_rel_radius); + float dp_min = std::max(ref_depth_min, d0 - depth_abs_radius); + float dp_max = std::min(ref_depth_max, d0 + depth_abs_radius); + dp = (dp_min < dp_max) ? r.nextf(dp_min, dp_max) : d0; + np = normalizeSafe(n0 + randomNormalObservedFromCamera(cameras_RtoWorld[ref_cam], r) * normal_radius, n0); - // 3) новой случайной гипотезы из фрустума поиска (новые идеи, вечный поиск во всем пространстве) - // TODO 106: создайте случайную гипотезу dr+nr, вам поможет: - // - r.nextf(...) - // - ref_depth_min, ref_depth_max - // - randomNormalObservedFromCamera - поможет создать нормаль которая гарантированно смотрит на нас + dr = r.nextf(ref_depth_min, ref_depth_max); + nr = randomNormalObservedFromCamera(cameras_RtoWorld[ref_cam], r); } - float best_depth = d0; - vector3f best_normal = n0; + float best_depth = depth_map.at(j, i); + vector3f best_normal = normal_map.at(j, i); float best_cost = cost_map.at(j, i); - if (d0 == NO_DEPTH) { + if (best_depth == NO_DEPTH) { best_cost = NO_COST; } float depths[3] = { d0, dr, dp }; vector3f normals[3] = { n0, nr, np }; - // перебираем все комбинации этих гипотез, т.е. 3х3=9 вариантов for (size_t hi = 0; hi < 3 * 3; ++hi) { - // эту комбинацию-гипотезу мы сейчас рассматриваем как очередного кандидата float d = depths[hi / 3]; vector3f n = normals[hi % 3]; - // оцениваем cost для каждого соседа std::vector costs; + costs.reserve(ncameras > 0 ? ncameras - 1 : 0); for (size_t ni = 0; ni < ncameras; ++ni) { if (ni == ref_cam) continue; @@ -150,15 +222,12 @@ void PMDepthMapsBuilder::refinement() costs.push_back(costi); } - // объединяем cost-ы всех соседей в одну общую оценку качества текущей гипотезы (условно "усредняем") float total_cost = avgCost(costs); - // WTA (winner takes all) if (total_cost < best_cost) { best_depth = d; best_normal = n; - best_cost = total_cost; // TODO 206: добавьте подсчет статистики, какая комбинация гипотез чаще всего побеждает? есть ли комбинации на которых мы можем сэкономить? а какие гипотезы при refinement рассматривает например - // Colmap? + best_cost = total_cost; } } @@ -173,18 +242,12 @@ void PMDepthMapsBuilder::refinement() printCurrentStats(); #endif #ifdef DEBUG_DIR - debugCurrentPoints(to_string(ref_cam) + "_" + to_string(iter) + "_refinement"); + debugCurrentPoints(std::to_string(ref_cam) + "_" + std::to_string(iter) + "_refinement"); #endif } void PMDepthMapsBuilder::tryToPropagateDonor(ptrdiff_t ni, ptrdiff_t nj, int chessboard_pattern_step, std::vector& hypos_depth, std::vector& hypos_normal, std::vector& hypos_cost) { - // rassert-ы или любой другой способ явной фиксации инвариантов со встроенной их проверкой в runtime - - // это очень приятный способ ускорить отладку и гарантировать что ожидания в голове сойдутся с реальностью в коде, - // а если разойдутся - то узнать об этом в самом первом сломавшемся предположении - // (в данном случае мы явно проверяем что нигде не промахнулись и все соседи - другого шахматного цвета) - // пусть лучше эта проверка упадет, мы сразу это заметим и отладим, чем бага будет тихо портить результаты - // а мы это может быть даже не заметим rassert((ni + nj) % 2 != chessboard_pattern_step, 2391249129510120); if (ni < 0 || ni >= width || nj < 0 || nj >= height) @@ -207,6 +270,18 @@ void PMDepthMapsBuilder::propagation() timer t; verbose_cout << "Iteration #" << iter << "/" << NITERATIONS << ": propagation..." << std::endl; + int adaptive_far_step = PROPAGATION_STEP; + if (iter > 0 && NITERATIONS > 0) { + float progress = static_cast(iter) / static_cast(NITERATIONS); + adaptive_far_step = static_cast(std::round(PROPAGATION_STEP * (1.0f - 0.6f * progress))); + } + adaptive_far_step = makeOddAtLeast(adaptive_far_step, 3); + + int adaptive_mid_step = makeOddAtLeast(std::max(3, adaptive_far_step / 2), 3); + if (adaptive_mid_step >= adaptive_far_step) { + adaptive_mid_step = makeOddAtLeast(std::max(3, adaptive_far_step - 2), 3); + } + for (int chessboard_pattern_step = 0; chessboard_pattern_step < 2; ++chessboard_pattern_step) { #pragma omp parallel for schedule(dynamic, 1) for (ptrdiff_t j = 0; j < height; ++j) { @@ -214,24 +289,10 @@ void PMDepthMapsBuilder::propagation() std::vector hypos_depth; std::vector hypos_normal; std::vector hypos_cost; + hypos_depth.reserve(24); + hypos_normal.reserve(24); + hypos_cost.reserve(24); - /* 4 прямых соседа A, 8 соседей B через диагональ, 4 соседа C вдалеке (условный рисунок для PROPAGATION_STEP=5): - * (удобно подсвечивать через Ctrl+F) - * center - * | - * v - * o o o o o C o o o o o - * o o o o o o o o o o o - * o o o o o o o v o o o - * o o o o B o B o v o o - * o o o B o A o B o o o - * C o o o A . A o o o C <- center - * o o o B o A o B o o o - * o o o o B o B o v o o - * o o o o o o o v o o o - * o o o o o o o o o o o - * o o o o o C o o o o o - */ tryToPropagateDonor(i - 1, j + 0, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); tryToPropagateDonor(i + 0, j - 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); tryToPropagateDonor(i + 1, j + 0, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); @@ -246,17 +307,15 @@ void PMDepthMapsBuilder::propagation() tryToPropagateDonor(i - 1, j + 2, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); tryToPropagateDonor(i - 2, j + 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - // в таких случаях очень приятно использовать множественный курсор (чтобы скопировав четыре строки выше, затем просто колесиком мышки сделать четыре каретки для того чтобы дважды вставить *PROPAGATION_STEP): - tryToPropagateDonor(i - 1 * PROPAGATION_STEP, j + 0 * PROPAGATION_STEP, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i + 0 * PROPAGATION_STEP, j - 1 * PROPAGATION_STEP, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i + 1 * PROPAGATION_STEP, j + 0 * PROPAGATION_STEP, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i + 0 * PROPAGATION_STEP, j + 1 * PROPAGATION_STEP, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i - 1 * adaptive_mid_step, j + 0 * adaptive_mid_step, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 0 * adaptive_mid_step, j - 1 * adaptive_mid_step, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 1 * adaptive_mid_step, j + 0 * adaptive_mid_step, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 0 * adaptive_mid_step, j + 1 * adaptive_mid_step, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - // TODO 201 переделайте чтобы было как в ACMH: - // TODO 202 - паттерн донорства - // TODO 203 - логика про "берем 8 лучших по их личной оценке - по их личному cost" и только их примеряем уже на себя для рассчета cost в нашей точке - // TODO 301 - сделайте вместо наивного переноса depth+normal в наш пиксель - логику про "пересекли луч из нашего пикселя с плоскостью которую задает донор-сосед" и оценку cost в нашей точке тогда можно провести для более - // релевантной точки-пересечения + tryToPropagateDonor(i - 1 * adaptive_far_step, j + 0 * adaptive_far_step, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 0 * adaptive_far_step, j - 1 * adaptive_far_step, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 1 * adaptive_far_step, j + 0 * adaptive_far_step, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 0 * adaptive_far_step, j + 1 * adaptive_far_step, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); float best_depth = depth_map.at(j, i); vector3f best_normal = normal_map.at(j, i); @@ -265,28 +324,31 @@ void PMDepthMapsBuilder::propagation() best_cost = NO_COST; } - for (size_t hi = 0; hi < hypos_depth.size(); ++hi) { - // эту гипотезу мы сейчас рассматриваем как очередного кандидата + std::vector order(hypos_depth.size()); + std::iota(order.begin(), order.end(), 0); + std::sort(order.begin(), order.end(), [&](size_t lhs, size_t rhs) { + return hypos_cost[lhs] < hypos_cost[rhs]; + }); + + size_t donors_limit = std::min(8, order.size()); + for (size_t oi = 0; oi < donors_limit; ++oi) { + size_t hi = order[oi]; + float d = hypos_depth[hi]; vector3f n = hypos_normal[hi]; - // оцениваем cost для каждого соседа std::vector costs; + costs.reserve(ncameras > 0 ? ncameras - 1 : 0); for (size_t ni = 0; ni < ncameras; ++ni) { if (ni == ref_cam) continue; float costi = estimateCost(i, j, d, n, ni); - if (costi == NO_COST) - continue; - costs.push_back(costi); } - // объединяем cost-ы всех соседей в одну общую оценку качества текущей гипотезы (условно "усредняем") float total_cost = avgCost(costs); - // WTA (winner takes all) if (total_cost < best_cost) { best_depth = d; best_normal = n; @@ -306,21 +368,30 @@ void PMDepthMapsBuilder::propagation() printCurrentStats(); #endif #ifdef DEBUG_DIR - debugCurrentPoints(to_string(ref_cam) + "_" + to_string(iter) + "_propagation"); + debugCurrentPoints(std::to_string(ref_cam) + "_" + std::to_string(iter) + "_propagation"); #endif } float PMDepthMapsBuilder::estimateCost(ptrdiff_t i, ptrdiff_t j, double d, const vector3d& global_normal, size_t neighb_cam) { - vector3d pixel(i + 0.5, j + 0.5, d); - vector3d global_point = unproject(pixel, calibration, cameras_PtoWorld[ref_cam]); - + if (!(d > 0.0) || d < ref_depth_min || d > ref_depth_max) + return NO_COST; if (!(i - COST_PATCH_RADIUS >= 0 && i + COST_PATCH_RADIUS < width)) return NO_COST; if (!(j - COST_PATCH_RADIUS >= 0 && j + COST_PATCH_RADIUS < height)) return NO_COST; - std::vector patch0, patch1; + vector3d pixel(i + 0.5, j + 0.5, d); + vector3d global_point = unproject(pixel, calibration, cameras_PtoWorld[ref_cam]); + + vector3d ref_camera_center = cameraCenter(cameras_PtoWorld[ref_cam]); + if (dot(global_normal, ref_camera_center - global_point) <= 1e-9) + return NO_COST; + + std::vector patch0; + std::vector patch1; + patch0.reserve((2 * COST_PATCH_RADIUS + 1) * (2 * COST_PATCH_RADIUS + 1)); + patch1.reserve((2 * COST_PATCH_RADIUS + 1) * (2 * COST_PATCH_RADIUS + 1)); for (ptrdiff_t dj = -COST_PATCH_RADIUS; dj <= COST_PATCH_RADIUS; ++dj) { for (ptrdiff_t di = -COST_PATCH_RADIUS; di <= COST_PATCH_RADIUS; ++di) { @@ -329,62 +400,67 @@ float PMDepthMapsBuilder::estimateCost(ptrdiff_t i, ptrdiff_t j, double d, const patch0.push_back(cameras_imgs_grey[ref_cam].at(nj, ni) / 255.0f); - vector3d point_on_ray = unproject(vector3d(ni + 0.5, nj + 0.5, 1.0), calibration, cameras_PtoWorld[ref_cam]); - vector3d camera_center = unproject( - vector3d(ni + 0.5, nj + 0.5, 0.0), calibration, cameras_PtoWorld[ref_cam]); // TODO 204: это немного неестественный способ, можно поправить его на более явный вариант, например хранить центр камер в поле cameras_O - - vector3d ray_dir = cv::normalize(point_on_ray - camera_center); - vector3d ray_org = camera_center; + vector3d local_ray = calibration.unproject(vector2d(ni + 0.5, nj + 0.5)); + vector3d ray_dir = cameras_RtoWorld[ref_cam] * local_ray; + double ray_len = std::sqrt(dot(ray_dir, ray_dir)); + if (ray_len <= 1e-12) + return NO_COST; + ray_dir *= 1.0 / ray_len; vector3d global_intersection; - if (!intersectWithPlane(global_point, global_normal, ray_org, ray_dir, global_intersection)) - return NO_COST; // луч не пересек плоскость (например наблюдаем ее под близким к прямому углу) + if (!intersectWithPlane(global_point, global_normal, ref_camera_center, ray_dir, global_intersection)) + return NO_COST; rassert(neighb_cam != ref_cam, 2334195412410286); vector3d neighb_proj = project(global_intersection, calibration, cameras_PtoLocal[neighb_cam]); - if (neighb_proj[2] < 0.0) + if (neighb_proj[2] <= 0.0) return NO_COST; double x = neighb_proj[0]; double y = neighb_proj[1]; - // TODO 205: замените этот наивный вариант nearest neighbor сэмплирования текстуры на билинейную интерполяцию (учтите что центр пикселя - .5 после запятой) - ptrdiff_t u = x; - ptrdiff_t v = y; - - // TODO 108: добавьте проверку "попали ли мы в камеру номер neighb_cam?" если не попали - возвращаем NO_COST + float intensity = 0.0f; + if (!sampleBilinearGrey(cameras_imgs_grey[neighb_cam], x, y, intensity)) + return NO_COST; - float intensity = cameras_imgs_grey[neighb_cam].at(v, u) / 255.0f; patch1.push_back(intensity); } } - // TODO 109: реализуйте ZNCC https://en.wikipedia.org/wiki/Cross-correlation#Zero-normalized_cross-correlation_(ZNCC) - // или слайд #25 в лекции 5 про SGM и Cost-функции - https://my.compscicenter.ru/attachments/classes/slides_w2n8WNLY/photogrammetry_lecture_090321.pdf rassert(patch0.size() == patch1.size(), 12489185129326); size_t n = patch0.size(); + if (n == 0) + return NO_COST; + float mean0 = 0.0f; float mean1 = 0.0f; - // ... for (size_t k = 0; k < n; ++k) { - float a = patch0[k]; - float b = patch1[k]; - mean0 += a; - mean1 += b; - // ... + mean0 += patch0[k]; + mean1 += patch1[k]; } - mean0 /= n; - mean1 /= n; - // ... + mean0 /= static_cast(n); + mean1 /= static_cast(n); + + float num = 0.0f; + float den0 = 0.0f; + float den1 = 0.0f; + for (size_t k = 0; k < n; ++k) { + float a = patch0[k] - mean0; + float b = patch1[k] - mean1; + num += a * b; + den0 += a * a; + den1 += b * b; + } + float zncc = 0.0f; + if (den0 > 1e-12f && den1 > 1e-12f) { + zncc = num / std::sqrt(den0 * den1); + } - // ZNCC в диапазоне [-1; 1], 1: идеальное совпадение, -1: ничего общего - rassert(zncc == zncc, 23141241210380); // проверяем что не nan + rassert(zncc == zncc, 23141241210380); zncc = std::max(-1.0f, std::min(1.0f, zncc)); rassert(zncc >= -1.0 && zncc <= 1.0, 141251251541357); - // переводим в cost от [0; 1] (NO_COST=1) - // чем ближе cost к нулю - тем лучше сопоставление float cost = (1.0f - zncc) / 2.0f; rassert(cost >= 0.0f && cost <= NO_COST, 23123912049102361); @@ -393,22 +469,42 @@ float PMDepthMapsBuilder::estimateCost(ptrdiff_t i, ptrdiff_t j, double d, const float PMDepthMapsBuilder::avgCost(std::vector& costs) { - if (costs.size() == 0) + std::vector valid_costs; + valid_costs.reserve(costs.size()); + for (float c : costs) { + if (c >= 0.0f && c < NO_COST) { + valid_costs.push_back(c); + } + } + + if (valid_costs.empty()) return NO_COST; - std::sort(costs.begin(), costs.end()); + std::sort(valid_costs.begin(), valid_costs.end()); - float best_cost = costs[0]; + float best_cost = valid_costs[0]; + float max_allowed_cost = std::max(best_cost, std::max(best_cost * COSTS_K_RATIO, best_cost + 0.05f)); + max_allowed_cost = std::min(0.95f, max_allowed_cost); - float cost_sum = best_cost; - float cost_w = 1.0f; + float cost_sum = 0.0f; + float cost_w = 0.0f; + size_t used = 0; + for (float c : valid_costs) { + if (used >= COSTS_BEST_K_LIMIT) + break; + if (used > 0 && c > max_allowed_cost) + break; - // TODO 110 реализуйте какое-то "усреднение cost-ов по всем соседям", с ограничением что участвуют только COSTS_BEST_K_LIMIT лучших - // TODO 111 добавьте к этому усреднению еще одно ограничение: если cost больше чем best_cost*COSTS_K_RATIO - то такой cost подозрительно плохой и мы его не хотим учитывать (вероятно occlusion) - // TODO 112 а что если в пикселе occlusion, но best_cost - большой и поэтому отсечение по best_cost*COSTS_K_RATIO не срабатывает? можно ли это отсечение как-то выправить для такого случая? - // TODO 207 а что если добавить какой-нибудь бонус в случае если больше чем Х камер засчиталось? улучшается/ухудшается ли от этого что-то на herzjezu25? а при большем числе фотографий + cost_sum += c; + cost_w += 1.0f; + ++used; + } + + if (cost_w <= 0.0f) + return NO_COST; float avg_cost = cost_sum / cost_w; + avg_cost = std::max(0.0f, std::min(NO_COST, avg_cost)); return avg_cost; } @@ -439,8 +535,10 @@ void PMDepthMapsBuilder::printCurrentStats() } } double ntotal = width * height; - verbose_cout << to_percent(costs_n, ntotal) << "% pixels with " << (costs_sum / costs_n) << " avg cost, "; - verbose_cout << to_percent(good_costs_n, ntotal) << "% pixels with good " << (good_costs_sum / good_costs_n) << " avg cost"; + double avg_cost = (costs_n > 0.0) ? (costs_sum / costs_n) : NO_COST; + double avg_good_cost = (good_costs_n > 0.0) ? (good_costs_sum / good_costs_n) : NO_COST; + verbose_cout << to_percent(costs_n, ntotal) << "% pixels with " << avg_cost << " avg cost, "; + verbose_cout << to_percent(good_costs_n, ntotal) << "% pixels with good " << avg_good_cost << " avg cost"; verbose_cout << std::endl; } @@ -517,4 +615,4 @@ void PMDepthMapsBuilder::buildGoodPoints(const cv::Mat& depth_map, const cv::Mat } } -} +} \ No newline at end of file diff --git a/tests/test_depth_maps_pm.cpp b/tests/test_depth_maps_pm.cpp index 3a03547e..6f66c0a2 100644 --- a/tests/test_depth_maps_pm.cpp +++ b/tests/test_depth_maps_pm.cpp @@ -33,44 +33,42 @@ TEST(test_depth_maps_pm, SingleDepthMap) { - // TODO этот код надо раскомментировать чтобы запустить тестирование: - // Dataset dataset = loadDataset(DATASET_DIR, DATASET_DOWNSCALE); - // phg::PMDepthMapsBuilder builder(dataset.ncameras, dataset.cameras_imgs, dataset.cameras_imgs_grey, dataset.cameras_labels, dataset.cameras_P, dataset.calibration); - // - // size_t ci = 2; // строим карту глубины для третьей камеры (индексация с нуля) - // size_t cameras_limit = 5; // учитывая первые пять фотографий датасета, т.е. две камеры слева и две камеры справа - // - // dataset.ncameras = cameras_limit; - // cv::Mat depth_map, normal_map, cost_map; - // builder.buildDepthMap(ci, depth_map, cost_map, normal_map, dataset.cameras_depth_min[ci], dataset.cameras_depth_max[ci]); + Dataset dataset = loadDataset(DATASET_DIR, DATASET_DOWNSCALE); + phg::PMDepthMapsBuilder builder(dataset.ncameras, dataset.cameras_imgs, dataset.cameras_imgs_grey, dataset.cameras_labels, dataset.cameras_P, dataset.calibration); + + size_t ci = 2; // строим карту глубины для третьей камеры (индексация с нуля) + size_t cameras_limit = 5; // учитывая первые пять фотографий датасета, т.е. две камеры слева и две камеры справа + + dataset.ncameras = cameras_limit; + cv::Mat depth_map, normal_map, cost_map; + builder.buildDepthMap(ci, depth_map, normal_map, cost_map, dataset.cameras_depth_min[ci], dataset.cameras_depth_max[ci]); } TEST(test_depth_maps_pm, AllDepthMaps) { - // TODO этот код можно раскомментировать чтобы построить много карт глубины и сохранить их облака точек: - // Dataset full_dataset = loadDataset(DATASET_DIR, DATASET_DOWNSCALE); - // - // const size_t ref_camera_shift = 2; - // const size_t to_shift = 5; - // - // std::vector all_points; - // std::vector all_colors; - // std::vector all_normals; - // - // size_t ndepth_maps = 0; - // - // for (size_t from = 0; from + to_shift <= full_dataset.ncameras; ++from) { - // size_t to = from + to_shift; - // - // Dataset dataset = full_dataset.subset(from, to); - // - // phg::PMDepthMapsBuilder builder(dataset.ncameras, dataset.cameras_imgs, dataset.cameras_imgs_grey, dataset.cameras_labels, dataset.cameras_P, dataset.calibration); - // cv::Mat depth_map, normal_map, cost_map; - // builder.buildDepthMap(ref_camera_shift, depth_map, cost_map, normal_map, dataset.cameras_depth_min[ref_camera_shift], dataset.cameras_depth_max[ref_camera_shift]); - // phg::PMDepthMapsBuilder::buildGoodPoints(depth_map, normal_map, cost_map, dataset.cameras_imgs[ref_camera_shift], dataset.calibration, builder.getCameraPtoWorld(ref_camera_shift), all_points, all_colors, all_normals); - // ++ndepth_maps; - // - // std::string tie_points_filename = std::string("data/debug/test_depth_maps_pm/") + getTestName() + "/all_points_" + to_string(ndepth_maps) + ".ply"; - // phg::exportPointCloud(all_points, tie_points_filename, all_colors, all_normals); - // } + Dataset full_dataset = loadDataset(DATASET_DIR, DATASET_DOWNSCALE); + + const size_t ref_camera_shift = 2; + const size_t to_shift = 5; + + std::vector all_points; + std::vector all_colors; + std::vector all_normals; + + size_t ndepth_maps = 0; + + for (size_t from = 0; from + to_shift <= full_dataset.ncameras; ++from) { + size_t to = from + to_shift; + + Dataset dataset = full_dataset.subset(from, to); + + phg::PMDepthMapsBuilder builder(dataset.ncameras, dataset.cameras_imgs, dataset.cameras_imgs_grey, dataset.cameras_labels, dataset.cameras_P, dataset.calibration); + cv::Mat depth_map, normal_map, cost_map; + builder.buildDepthMap(ref_camera_shift, depth_map, normal_map, cost_map, dataset.cameras_depth_min[ref_camera_shift], dataset.cameras_depth_max[ref_camera_shift]); + phg::PMDepthMapsBuilder::buildGoodPoints(depth_map, normal_map, cost_map, dataset.cameras_imgs[ref_camera_shift], dataset.calibration, builder.getCameraPtoWorld(ref_camera_shift), all_points, all_colors, all_normals); + ++ndepth_maps; + + std::string tie_points_filename = std::string("data/debug/test_depth_maps_pm/") + getTestName() + "/all_points_" + to_string(ndepth_maps) + ".ply"; + phg::exportPointCloud(all_points, tie_points_filename, all_colors, all_normals); + } }