feat: P2-P4 全面补完 — B-Rep建模 + 数据交换 + 约束求解器 + ICP
P2 B-Rep建模: - brep: fillet/chamfer/shell 完整实现 - brep: box/cylinder/sphere 实体工厂 - brep: extrude/revolve/sweep/loft 建模操作 P3 数据交换: - foundation: PLY 读写(ASCII+二进制) - foundation: glTF 2.0 导出(JSON+bin) P4 高级算法/杀手功能: - sketch: 2D 草图约束求解器(Newton-Raphson, 9种约束类型) - core: ICP 点云配准(SVD最优变换+KD-Tree加速) 新增模块: vde_sketch
This commit is contained in:
@@ -0,0 +1,105 @@
|
||||
#include "vde/core/icp.h"
|
||||
#include "vde/spatial/kd_tree.h"
|
||||
#include <Eigen/SVD>
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
|
||||
namespace vde::core {
|
||||
|
||||
namespace {
|
||||
|
||||
// Find nearest neighbor using KD-Tree
|
||||
Point3D find_nearest(const Point3D& p, const spatial::KDTree<Point3D>& tree) {
|
||||
auto knn = tree.query_knn(p, 1);
|
||||
return knn.empty() ? p : knn[0];
|
||||
}
|
||||
|
||||
// Compute centroid
|
||||
Point3D centroid(const std::vector<Point3D>& pts) {
|
||||
Point3D c(0,0,0);
|
||||
for (const auto& p : pts) c += p;
|
||||
if (!pts.empty()) c /= static_cast<double>(pts.size());
|
||||
return c;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
ICPResult icp_register(const std::vector<Point3D>& source,
|
||||
const std::vector<Point3D>& target,
|
||||
int max_iter, double tolerance) {
|
||||
ICPResult result;
|
||||
result.transform = Transform3D::Identity();
|
||||
result.converged = false;
|
||||
|
||||
if (source.empty() || target.empty()) return result;
|
||||
|
||||
// Build KD-Tree for target
|
||||
spatial::KDTree<Point3D> tree;
|
||||
tree.build(target);
|
||||
|
||||
std::vector<Point3D> aligned = source;
|
||||
double prev_rms = std::numeric_limits<double>::max();
|
||||
|
||||
for (int iter = 0; iter < max_iter; ++iter) {
|
||||
// Step 1: Find correspondences (nearest neighbor)
|
||||
std::vector<Point3D> src_matches, tgt_matches;
|
||||
for (const auto& p : aligned) {
|
||||
Point3D nearest = find_nearest(p, tree);
|
||||
src_matches.push_back(p);
|
||||
tgt_matches.push_back(nearest);
|
||||
}
|
||||
|
||||
// Step 2: Compute optimal rigid transform (SVD)
|
||||
Point3D cs = centroid(src_matches), ct = centroid(tgt_matches);
|
||||
|
||||
Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
|
||||
for (size_t i = 0; i < src_matches.size(); ++i) {
|
||||
Vector3D s = src_matches[i] - cs;
|
||||
Vector3D t = tgt_matches[i] - ct;
|
||||
H += s * t.transpose();
|
||||
}
|
||||
|
||||
Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
|
||||
Eigen::Matrix3d R = svd.matrixV() * svd.matrixU().transpose();
|
||||
|
||||
// Ensure proper rotation (determinant = 1, not -1)
|
||||
if (R.determinant() < 0) {
|
||||
Eigen::Matrix3d V = svd.matrixV();
|
||||
V.col(2) *= -1;
|
||||
R = V * svd.matrixU().transpose();
|
||||
}
|
||||
|
||||
Vector3D t = ct - R * cs;
|
||||
|
||||
Transform3D T = Transform3D::Identity();
|
||||
T.linear() = R;
|
||||
T.translation() = t;
|
||||
|
||||
// Step 3: Apply transform
|
||||
for (auto& p : aligned)
|
||||
p = T * p;
|
||||
|
||||
result.transform = T * result.transform;
|
||||
|
||||
// Step 4: Compute RMS error
|
||||
double rms = 0;
|
||||
for (size_t i = 0; i < aligned.size(); ++i) {
|
||||
Point3D nearest = find_nearest(aligned[i], tree);
|
||||
rms += (aligned[i] - nearest).squaredNorm();
|
||||
}
|
||||
rms = std::sqrt(rms / aligned.size());
|
||||
|
||||
result.iterations = iter + 1;
|
||||
result.rms_error = rms;
|
||||
|
||||
if (std::abs(prev_rms - rms) < tolerance) {
|
||||
result.converged = true;
|
||||
break;
|
||||
}
|
||||
prev_rms = rms;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace vde::core
|
||||
Reference in New Issue
Block a user