290 lines
11 KiB
C++
290 lines
11 KiB
C++
//
|
||
// Created by lgv on 2026/2/26.
|
||
//
|
||
|
||
#pragma once
|
||
|
||
#include <algorithm>
|
||
#include <cmath>
|
||
#include <cstdint>
|
||
#include <limits>
|
||
#include <vector>
|
||
|
||
#include <Eigen/Core>
|
||
#include "devices/camera/abstract_camera.h"
|
||
#include <opencv2/core.hpp>
|
||
|
||
namespace cmvr {
|
||
|
||
/**
|
||
* @brief 图像几何处理静态工具类。
|
||
*
|
||
* 提供像素射线反投影、3D 点投影和深度稳健采样等通用能力。
|
||
*/
|
||
class ImageProcess final {
|
||
public:
|
||
/**
|
||
* @brief 将像素坐标反投影为相机坐标系下单位射线（无畸变 pinhole）。
|
||
*
|
||
* @param intrinsics 相机内参。
|
||
* @param u 像素横坐标（列）。
|
||
* @param v 像素纵坐标（行）。
|
||
* @param ray_c 输出单位射线方向（相机坐标系）。
|
||
* @return 成功返回 `true`，失败返回 `false`。
|
||
*/
|
||
static inline bool pixelToRayCamera(const device::Rs2Intrinsics& intrinsics,
|
||
int u,
|
||
int v,
|
||
Eigen::Vector3d& ray_c) {
|
||
const double fx = static_cast<double>(intrinsics.fx);
|
||
const double fy = static_cast<double>(intrinsics.fy);
|
||
const double cx = static_cast<double>(intrinsics.cx);
|
||
const double cy = static_cast<double>(intrinsics.cy);
|
||
if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0 ||
|
||
!std::isfinite(cx) || !std::isfinite(cy)) {
|
||
return false;
|
||
}
|
||
|
||
ray_c.z() = 1.0;
|
||
ray_c.x() = (static_cast<double>(u) - cx) / fx;
|
||
ray_c.y() = (static_cast<double>(v) - cy) / fy;
|
||
const double n = ray_c.norm();
|
||
if (!std::isfinite(n) || n <= 1e-12) {
|
||
return false;
|
||
}
|
||
ray_c /= n;
|
||
return true;
|
||
}
|
||
|
||
/**
|
||
* @brief 将相机坐标系下 3D 点投影到像素坐标（无畸变 pinhole）。
|
||
*
|
||
* @param intrinsics 相机内参。
|
||
* @param p_c 输入 3D 点（相机坐标系，单位米）。
|
||
* @param uv 输出像素坐标。
|
||
* @return 成功返回 `true`，失败返回 `false`。
|
||
*/
|
||
static inline bool projectCameraPointToPixel(const device::Rs2Intrinsics& intrinsics,
|
||
const Eigen::Vector3d& p_c,
|
||
Eigen::Vector2d& uv) {
|
||
if (!std::isfinite(p_c.x()) || !std::isfinite(p_c.y()) || !std::isfinite(p_c.z()) ||
|
||
p_c.z() <= 1e-9) {
|
||
return false;
|
||
}
|
||
|
||
const double fx = static_cast<double>(intrinsics.fx);
|
||
const double fy = static_cast<double>(intrinsics.fy);
|
||
const double cx = static_cast<double>(intrinsics.cx);
|
||
const double cy = static_cast<double>(intrinsics.cy);
|
||
if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0 ||
|
||
!std::isfinite(cx) || !std::isfinite(cy)) {
|
||
return false;
|
||
}
|
||
|
||
uv.x() = fx * (p_c.x() / p_c.z()) + cx;
|
||
uv.y() = fy * (p_c.y() / p_c.z()) + cy;
|
||
return std::isfinite(uv.x()) && std::isfinite(uv.y());
|
||
}
|
||
|
||
/**
|
||
* @brief 将像素坐标和深度值反投影为相机坐标系下 3D 点。
|
||
*
|
||
* @param intrinsics 相机内参。
|
||
* @param u 像素横坐标（列）。
|
||
* @param v 像素纵坐标（行）。
|
||
* @param z_m 深度值（米）。
|
||
* @param p_c 输出 3D 点（相机坐标系，单位米）。
|
||
* @return 成功返回 `true`，失败返回 `false`。
|
||
*/
|
||
static inline bool pixelToCameraPoint(const device::Rs2Intrinsics& intrinsics,
|
||
int u,
|
||
int v,
|
||
double z_m,
|
||
Eigen::Vector3d& p_c) {
|
||
const double fx = static_cast<double>(intrinsics.fx);
|
||
const double fy = static_cast<double>(intrinsics.fy);
|
||
const double cx = static_cast<double>(intrinsics.cx);
|
||
const double cy = static_cast<double>(intrinsics.cy);
|
||
if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0 ||
|
||
!std::isfinite(cx) || !std::isfinite(cy)) {
|
||
return false;
|
||
}
|
||
if (!std::isfinite(z_m) || z_m <= 1e-9) {
|
||
return false;
|
||
}
|
||
|
||
p_c.z() = z_m;
|
||
p_c.x() = (static_cast<double>(u) - cx) / fx * z_m;
|
||
p_c.y() = (static_cast<double>(v) - cy) / fy * z_m;
|
||
return std::isfinite(p_c.x()) && std::isfinite(p_c.y()) && std::isfinite(p_c.z());
|
||
}
|
||
|
||
/**
|
||
* @brief 在像素邻域采样深度并取中值，支持 MAD 离群剔除，输出单位米。
|
||
*
|
||
* 支持深度图类型：
|
||
* - `CV_16UC1`：毫米（内部转换为米）
|
||
* - `CV_32FC1`：米（若值明显为毫米尺度会自动转米）
|
||
* - `CV_64FC1`：米（若值明显为毫米尺度会自动转米）
|
||
*
|
||
* @param depth 深度图。
|
||
* @param u 像素横坐标（列）。
|
||
* @param v 像素纵坐标（行）。
|
||
* @param z_m 输出深度值（米）。
|
||
* @param window_size_px 采样窗口边长（像素，内部会修正为正奇数）。
|
||
* @param enable_outlier_reject 是否启用离群值剔除。
|
||
* @param outlier_sigma MAD 门限倍数。
|
||
* @param outlier_min_dev_m 最小离群门限（米）。
|
||
* @param min_valid_m 最小有效深度（米）。
|
||
* @return 成功返回 `true`，失败返回 `false`。
|
||
*/
|
||
static inline bool sampleDepthMeters(const cv::Mat& depth,
|
||
int u,
|
||
int v,
|
||
double& z_m,
|
||
int window_size_px = 3,
|
||
bool enable_outlier_reject = true,
|
||
double outlier_sigma = 2.5,
|
||
double outlier_min_dev_m = 0.002,
|
||
double min_valid_m = 1e-4) {
|
||
if (depth.empty() || depth.channels() != 1) {
|
||
return false;
|
||
}
|
||
if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) {
|
||
return false;
|
||
}
|
||
|
||
const int type = depth.type();
|
||
if (type != CV_16UC1 && type != CV_32FC1 && type != CV_64FC1) {
|
||
return false;
|
||
}
|
||
|
||
auto medianOf = [](std::vector<double> values, double& m) -> bool {
|
||
if (values.empty()) {
|
||
return false;
|
||
}
|
||
const size_t k = values.size() / 2;
|
||
std::nth_element(values.begin(), values.begin() + k, values.end());
|
||
m = values[k];
|
||
return std::isfinite(m);
|
||
};
|
||
|
||
int w = std::max(1, window_size_px);
|
||
if ((w % 2) == 0) {
|
||
++w;
|
||
}
|
||
const int radius = w / 2;
|
||
|
||
std::vector<double> valid_depths;
|
||
valid_depths.reserve(static_cast<size_t>((2 * radius + 1) * (2 * radius + 1)));
|
||
for (int yy = std::max(0, v - radius); yy <= std::min(depth.rows - 1, v + radius); ++yy) {
|
||
for (int xx = std::max(0, u - radius); xx <= std::min(depth.cols - 1, u + radius); ++xx) {
|
||
double z = std::numeric_limits<double>::quiet_NaN();
|
||
if (type == CV_16UC1) {
|
||
const uint16_t raw = depth.at<uint16_t>(yy, xx);
|
||
if (raw == 0) {
|
||
continue;
|
||
}
|
||
z = static_cast<double>(raw) * 1e-3; // mm -> m
|
||
} else if (type == CV_32FC1) {
|
||
z = static_cast<double>(depth.at<float>(yy, xx));
|
||
} else { // CV_64FC1
|
||
z = depth.at<double>(yy, xx);
|
||
}
|
||
|
||
if (std::isfinite(z) && z > 100.0) {
|
||
z *= 1e-3; // 兼容浮点毫米
|
||
}
|
||
if (!std::isfinite(z) || z <= min_valid_m || z >= 50.0) {
|
||
continue;
|
||
}
|
||
valid_depths.push_back(z);
|
||
}
|
||
}
|
||
|
||
if (valid_depths.empty()) {
|
||
return false;
|
||
}
|
||
|
||
double med = 0.0;
|
||
if (!medianOf(valid_depths, med)) {
|
||
return false;
|
||
}
|
||
|
||
if (enable_outlier_reject && std::isfinite(outlier_sigma) && outlier_sigma > 0.0) {
|
||
std::vector<double> abs_dev;
|
||
abs_dev.reserve(valid_depths.size());
|
||
for (const double z : valid_depths) {
|
||
abs_dev.push_back(std::abs(z - med));
|
||
}
|
||
double mad = 0.0;
|
||
if (medianOf(abs_dev, mad)) {
|
||
const double robust_sigma = 1.4826 * mad;
|
||
const double gate = std::max(std::max(0.0, outlier_min_dev_m), outlier_sigma * robust_sigma);
|
||
std::vector<double> inliers;
|
||
inliers.reserve(valid_depths.size());
|
||
for (const double z : valid_depths) {
|
||
if (std::abs(z - med) <= gate) {
|
||
inliers.push_back(z);
|
||
}
|
||
}
|
||
if (!inliers.empty() && medianOf(inliers, med)) {
|
||
// use filtered median
|
||
}
|
||
}
|
||
}
|
||
|
||
z_m = med;
|
||
return std::isfinite(z_m) && z_m > 0.0;
|
||
}
|
||
|
||
/**
|
||
* @brief 从深度图直接将像素反投影为相机坐标系下 3D 点。
|
||
*
|
||
* 该函数先调用 `sampleDepthMeters(...)` 估计像素深度，再调用
|
||
* `pixelToCameraPoint(...)` 完成反投影。
|
||
*
|
||
* @param intrinsics 相机内参。
|
||
* @param depth 深度图。
|
||
* @param u 像素横坐标（列）。
|
||
* @param v 像素纵坐标（行）。
|
||
* @param p_c 输出 3D 点（相机坐标系，单位米）。
|
||
* @param window_size_px 采样窗口边长（像素，内部会修正为正奇数）。
|
||
* @param enable_outlier_reject 是否启用离群值剔除。
|
||
* @param outlier_sigma MAD 门限倍数。
|
||
* @param outlier_min_dev_m 最小离群门限（米）。
|
||
* @param min_valid_m 最小有效深度（米）。
|
||
* @return 成功返回 `true`，失败返回 `false`。
|
||
*/
|
||
static inline bool pixelToCameraPointWithDepth(const device::Rs2Intrinsics& intrinsics,
|
||
const cv::Mat& depth,
|
||
int u,
|
||
int v,
|
||
Eigen::Vector3d& p_c,
|
||
int window_size_px = 3,
|
||
bool enable_outlier_reject = true,
|
||
double outlier_sigma = 2.5,
|
||
double outlier_min_dev_m = 0.002,
|
||
double min_valid_m = 1e-4) {
|
||
double z_m = 0.0;
|
||
if (!sampleDepthMeters(depth,
|
||
u,
|
||
v,
|
||
z_m,
|
||
window_size_px,
|
||
enable_outlier_reject,
|
||
outlier_sigma,
|
||
outlier_min_dev_m,
|
||
min_valid_m)) {
|
||
return false;
|
||
}
|
||
return pixelToCameraPoint(intrinsics, u, v, z_m, p_c);
|
||
}
|
||
|
||
private:
|
||
/** @brief 禁止实例化。 */
|
||
ImageProcess() = delete;
|
||
};
|
||
|
||
} // namespace cmvr
|