You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
231 lines
8.3 KiB
231 lines
8.3 KiB
// License: Apache 2.0. See LICENSE file in root directory.
|
|
// Copyright(c) 2024 Intel Corporation. All Rights Reserved.
|
|
//
|
|
// Plane Fit implementation follows http://www.ilikebigbits.com/blog/2015/3/2/plane-from-points algorithm
|
|
#pragma once
|
|
|
|
#include "float3.h"
|
|
#include "plane.h"
|
|
|
|
#ifndef _USE_MATH_DEFINES
|
|
#define _USE_MATH_DEFINES
|
|
#endif
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <math.h>
|
|
#include <mutex>
|
|
#include <vector>
|
|
#include <cmath>
|
|
|
|
#include <librealsense2/rs.hpp>
|
|
|
|
|
|
namespace rs2
|
|
{
|
|
namespace depth_quality
|
|
{
|
|
struct snapshot_metrics
|
|
{
|
|
int width;
|
|
int height;
|
|
|
|
rs2::region_of_interest roi;
|
|
|
|
float distance;
|
|
float angle;
|
|
float angle_x;
|
|
float angle_y;
|
|
|
|
plane p;
|
|
std::array<float3, 4> plane_corners;
|
|
};
|
|
|
|
struct single_metric_data
|
|
{
|
|
single_metric_data(std::string name, float val) :
|
|
val(val), name(name) {}
|
|
|
|
float val;
|
|
std::string name;
|
|
};
|
|
|
|
using callback_type = std::function<void(
|
|
const std::vector<rs2::float3>& points,
|
|
const plane p,
|
|
const rs2::region_of_interest roi,
|
|
const float baseline_mm,
|
|
const float focal_length_pixels,
|
|
const int ground_thruth_mm,
|
|
const bool plane_fit,
|
|
const float plane_fit_to_ground_truth_mm,
|
|
const float distance_mm,
|
|
bool record,
|
|
std::vector<single_metric_data>& samples)>;
|
|
|
|
inline plane plane_from_point_and_normal(const rs2::float3& point, const rs2::float3& normal)
|
|
{
|
|
return{ normal.x, normal.y, normal.z, -(normal.x*point.x + normal.y*point.y + normal.z*point.z) };
|
|
}
|
|
|
|
//Based on: http://www.ilikebigbits.com/blog/2015/3/2/plane-from-points
|
|
inline plane plane_from_points(const std::vector<rs2::float3> points)
|
|
{
|
|
if (points.size() < 3) throw std::runtime_error("Not enough points to calculate plane");
|
|
|
|
rs2::float3 sum = { 0,0,0 };
|
|
for (auto point : points) sum = sum + point;
|
|
|
|
rs2::float3 centroid = sum / float(points.size());
|
|
|
|
double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
|
|
for (auto point : points) {
|
|
rs2::float3 temp = point - centroid;
|
|
xx += temp.x * temp.x;
|
|
xy += temp.x * temp.y;
|
|
xz += temp.x * temp.z;
|
|
yy += temp.y * temp.y;
|
|
yz += temp.y * temp.z;
|
|
zz += temp.z * temp.z;
|
|
}
|
|
|
|
double det_x = yy*zz - yz*yz;
|
|
double det_y = xx*zz - xz*xz;
|
|
double det_z = xx*yy - xy*xy;
|
|
|
|
double det_max = std::max({ det_x, det_y, det_z });
|
|
if (det_max <= 0) return{ 0, 0, 0, 0 };
|
|
|
|
rs2::float3 dir{};
|
|
if (det_max == det_x)
|
|
{
|
|
float a = static_cast<float>((xz*yz - xy*zz) / det_x);
|
|
float b = static_cast<float>((xy*yz - xz*yy) / det_x);
|
|
dir = { 1, a, b };
|
|
}
|
|
else if (det_max == det_y)
|
|
{
|
|
float a = static_cast<float>((yz*xz - xy*zz) / det_y);
|
|
float b = static_cast<float>((xy*xz - yz*xx) / det_y);
|
|
dir = { a, 1, b };
|
|
}
|
|
else
|
|
{
|
|
float a = static_cast<float>((yz*xy - xz*yy) / det_z);
|
|
float b = static_cast<float>((xz*xy - yz*xx) / det_z);
|
|
dir = { a, b, 1 };
|
|
}
|
|
|
|
return plane_from_point_and_normal(centroid, dir.normalized());
|
|
}
|
|
|
|
inline double evaluate_pixel(const plane& p, const rs2_intrinsics* intrin, float x, float y, float distance, float3& output)
|
|
{
|
|
float pixel[2] = { x, y };
|
|
rs2_deproject_pixel_to_point(&output.x, intrin, pixel, distance);
|
|
return evaluate_plane(p, output);
|
|
}
|
|
|
|
inline float3 approximate_intersection(const plane& p, const rs2_intrinsics* intrin, float x, float y, float min, float max)
|
|
{
|
|
float3 point;
|
|
auto f = evaluate_pixel(p, intrin, x, y, max, point);
|
|
if (fabs(max - min) < 1e-3) return point;
|
|
auto n = evaluate_pixel(p, intrin, x, y, min, point);
|
|
if (f*n > 0) return{ 0, 0, 0 };
|
|
|
|
auto avg = (max + min) / 2;
|
|
auto mid = evaluate_pixel(p, intrin, x, y, avg, point);
|
|
if (mid*n < 0) return approximate_intersection(p, intrin, x, y, min, avg);
|
|
return approximate_intersection(p, intrin, x, y, avg, max);
|
|
}
|
|
|
|
inline float3 approximate_intersection(const plane& p, const rs2_intrinsics* intrin, float x, float y)
|
|
{
|
|
return approximate_intersection(p, intrin, x, y, 0.f, 1000.f);
|
|
}
|
|
|
|
inline snapshot_metrics analyze_depth_image(
|
|
const rs2::video_frame& frame,
|
|
float units, float baseline_mm,
|
|
const rs2_intrinsics * intrin,
|
|
rs2::region_of_interest roi,
|
|
const int ground_truth_mm,
|
|
bool plane_fit_present,
|
|
std::vector<single_metric_data>& samples,
|
|
bool record,
|
|
callback_type callback)
|
|
{
|
|
auto pixels = (const uint16_t*)frame.get_data();
|
|
const auto w = frame.get_width();
|
|
const auto h = frame.get_height();
|
|
|
|
snapshot_metrics result{ w, h, roi, {} };
|
|
|
|
std::mutex m;
|
|
|
|
std::vector<rs2::float3> roi_pixels;
|
|
|
|
//#pragma omp parallel for - TODO optimization envisaged
|
|
for (int y = roi.min_y; y < roi.max_y; ++y)
|
|
for (int x = roi.min_x; x < roi.max_x; ++x)
|
|
{
|
|
auto depth_raw = pixels[y*w + x];
|
|
|
|
if (depth_raw)
|
|
{
|
|
// units is float
|
|
float pixel[2] = { float(x), float(y) };
|
|
float point[3];
|
|
auto distance = depth_raw * units;
|
|
|
|
rs2_deproject_pixel_to_point(point, intrin, pixel, distance);
|
|
|
|
std::lock_guard<std::mutex> lock(m);
|
|
roi_pixels.push_back({ point[0], point[1], point[2] });
|
|
}
|
|
}
|
|
|
|
if (roi_pixels.size() < 3) { // Not enough pixels in RoI to fit a plane
|
|
return result;
|
|
}
|
|
|
|
plane p = plane_from_points(roi_pixels);
|
|
|
|
if (p == plane{ 0, 0, 0, 0 }) { // The points in RoI don't span a valid plane
|
|
return result;
|
|
}
|
|
|
|
// Calculate intersection of the plane fit with a ray along the center of ROI
|
|
// that by design coincides with the center of the frame
|
|
float3 plane_fit_pivot = approximate_intersection(p, intrin, intrin->width / 2.f, intrin->height / 2.f);
|
|
float plane_fit_to_gt_offset_mm = (ground_truth_mm > 0.f) ? (plane_fit_pivot.z * 1000 - ground_truth_mm) : 0;
|
|
|
|
result.p = p;
|
|
result.plane_corners[0] = approximate_intersection(p, intrin, float(roi.min_x), float(roi.min_y));
|
|
result.plane_corners[1] = approximate_intersection(p, intrin, float(roi.max_x), float(roi.min_y));
|
|
result.plane_corners[2] = approximate_intersection(p, intrin, float(roi.max_x), float(roi.max_y));
|
|
result.plane_corners[3] = approximate_intersection(p, intrin, float(roi.min_x), float(roi.max_y));
|
|
|
|
// Distance of origin (the camera) from the plane is encoded in parameter D of the plane
|
|
// The parameter represents the euclidian distance (along plane normal) from camera to the plane
|
|
result.distance = static_cast<float>(-p.d * 1000);
|
|
// Angle can be calculated from param C
|
|
result.angle = static_cast<float>(std::acos(std::abs(p.c)) / M_PI * 180.);
|
|
|
|
callback(roi_pixels, p, roi, baseline_mm, intrin->fx, ground_truth_mm, plane_fit_present,
|
|
plane_fit_to_gt_offset_mm, result.distance, record, samples);
|
|
|
|
// Calculate normal
|
|
auto n = float3{ p.a, p.b, p.c };
|
|
auto cam = float3{ 0.f, 0.f, -1.f };
|
|
auto dot = n * cam;
|
|
auto u = cam - n * dot;
|
|
|
|
result.angle_x = u.x;
|
|
result.angle_y = u.y;
|
|
|
|
return result;
|
|
}
|
|
}
|
|
}
|