#include
#include
#include
#include
#include
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;
struct Point3D {
double x, y, z;
};
struct Line {
Point3D p1, p2;
double length() const {
return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2));
}
};
vector readPLY(const string& filename) {
vector points;
ifstream fin(filename);
string line;
while (getline(fin, line)) {
if (line == "end_header") break;
}
while (getline(fin, line)) {
double x, y, z;
sscanf(line.c_str(), "%lf %lf %lf", &x, &y, &z);
points.push_back({ x, y, z });
}
return points;
}
vector detectLines(const vector& points, int threshold) {
const double pi = 3.1415926535897932384626433832795;
const int theta_bins = 360;
const int phi_bins = 180;
const double theta_step = pi / theta_bins;
const double phi_step = pi / phi_bins;
vector<vector> accumulator(theta_bins, vector(phi_bins, 0));
for (size_t i = 0; i < points.size(); i++) {
for (size_t j = i + 1; j < points.size(); j++) {
double dx = points[i].x - points[j].x;
double dy = points[i].y - points[j].y;
double dz = points[i].z - points[j].z;
double d = sqrt(dx * dx + dy * dy + dz * dz);
if (d == 0) continue;
double theta = atan2(dy, dx);
double phi = acos(dz / d);
int theta_idx = round((theta + pi) / theta_step);
int phi_idx = round(phi / phi_step);
if (theta_idx < 0 || theta_idx >= theta_bins || phi_idx < 0 || phi_idx >= phi_bins) continue;
accumulator[theta_idx][phi_idx]++;
}
}
vector<pair<int, pair<int, int>>> lines;
for (int i = 0; i < theta_bins; i++) {
for (int j = 0; j < phi_bins; j++) {
if (accumulator[i][j] >= threshold) {
lines.push_back({ accumulator[i][j], {i, j} });
}
}
}
sort(lines.rbegin(), lines.rend());
vector detected_lines;
for (size_t i = 0; i < lines.size(); i++) {
double theta = lines[i].second.first * theta_step - pi;
double phi = lines[i].second.second * phi_step;
double x1 = -1000 * cos(theta) * sin(phi);
double y1 = -1000 * sin(theta) * sin(phi);
double z1 = -1000 * cos(phi);
double x2 = -x1;
double y2 = -y1;
double z2 = -z1;
Line line = { { x1, y1, z1 }, { x2, y2, z2 } };
bool overlap = false;
for (size_t j = 0; j < detected_lines.size(); j++) {
double a1 = detected_lines[j].p1.x;
double b1 = detected_lines[j].p1.y;
double c1 = detected_lines[j].p1.z;
double a2 = detected_lines[j].p2.x;
double b2 = detected_lines[j].p2.y;
double c2 = detected_lines[j].p2.z;
double d1 = line.p1.x;
double e1 = line.p1.y;
double f1 = line.p1.z;
double d2 = line.p2.x;
double e2 = line.p2.y;
double f2 = line.p2.z;
double dist = sqrt(pow(a1 - d1, 2) + pow(b1 - e1, 2) + pow(c1 - f1, 2));
if (dist < 1e-6) {
overlap = true;
break;
}
dist = sqrt(pow(a2 - d2, 2) + pow(b2 - e2, 2) + pow(c2 - f2, 2));
if (dist < 1e-6) {
overlap = true;
break;
}
double a = pow(b1 - b2, 2) + pow(c1 - c2, 2) + pow(a1 - a2, 2);
double b = (d2 - d1) * (b1 - b2) + (e2 - e1) * (c1 - c2) + (f2 - f1) * (a1 - a2);
double c = pow(d1 - d2, 2) + pow(e1 - e2, 2) + pow(f1 - f2, 2);
double t = -b / a;
if (t < 0 || t > 1) continue;
double d = sqrt(c - b * b / a);
if (d < 1e-6) {
overlap = true;
break;
}
}
if (!overlap) {
detected_lines.push_back(line);
}
}
return detected_lines;
}
void visualize(const vector& points, const vector& lines) {
const int width = 800, height = 600;
Mat image(height, width, CV_8UC3, Scalar(255, 255, 255));
double max_length = 0;
for (const auto& line : lines) {
max_length = max(max_length, line.length());
}
for (const auto& point : points) {
double u = (point.x + max_length) / (2 * max_length) * width;
double v = (1 - (point.y + max_length) / (2 * max_length)) * height;
circle(image, Point(u, v), 2, Scalar(0, 0, 255), -1);
}
for (const auto& line : lines) {
double u1 = (line.p1.x + max_length) / (2 * max_length) * width;
double v1 = (1 - (line.p1.y + max_length) / (2 * max_length)) * height;
double u2 = (line.p2.x + max_length) / (2 * max_length) * width;
double v2 = (1 - (line.p2.y + max_length) / (2 * max_length)) * height;
line(image, Point(u1, v1), Point(u2, v2), Scalar(0, 255, 0), 2);
}
namedWindow('Visualization', WINDOW_NORMAL);
resizeWindow('Visualization', width, height);
imshow('Visualization', image);
waitKey(0);
}
int main(int argc, char** argv) {
if (argc < 2) {
cerr << "Usage: " << argv[0] << " filename.ply" << endl;
return 1;
}
vector points = readPLY(argv[1]);
vector lines = detectLines(points, 500);
visualize(points, lines);
return 0;
}
此代码首先读取 PLY 文件中的点云数据，然后使用 3D Hough 变换检测其中的直线。检测过程中，每个点对之间的距离和方向都被量化为离散的 theta 和 phi 值，并在 Hough 累加器中增加对应位置的投票数。最终，根据累加器中的投票数，找到投票数最多的直线并可视化。函数 readPLY() 读取 PLY 文件，函数 detectLines() 执行 3D Hough 变换，函数 visualize() 将点云和检测到的直线可视化。在此示例中，投票数阈值设置为 500，用于控制检测到的直线数量。
