删除无用代码
This commit is contained in:
parent
3aae9159ed
commit
916f2c7662
@ -18,18 +18,6 @@ static bool IsValidPoint(const SVzNLPointXYZ& pt) {
|
||||
return !(std::abs(pt.x) < eps && std::abs(pt.y) < eps && std::abs(pt.z) < eps);
|
||||
}
|
||||
|
||||
static SVzNLPointXYZ InverseTransformPoint(
|
||||
const SVzNLPointXYZ& pt,
|
||||
const float invRotation[9],
|
||||
const SVzNL3DPointF& planeCentroid
|
||||
) {
|
||||
SVzNLPointXYZ transformed = {};
|
||||
transformed.x = invRotation[0] * pt.x + invRotation[1] * pt.y + invRotation[2] * pt.z + planeCentroid.x;
|
||||
transformed.y = invRotation[3] * pt.x + invRotation[4] * pt.y + invRotation[5] * pt.z + planeCentroid.y;
|
||||
transformed.z = invRotation[6] * pt.x + invRotation[7] * pt.y + invRotation[8] * pt.z + planeCentroid.z;
|
||||
return transformed;
|
||||
}
|
||||
|
||||
static void ClearPoint(SVzNLPointXYZ* pt) {
|
||||
if (!pt) {
|
||||
return;
|
||||
@ -130,347 +118,7 @@ static void FilterLaserLineNoiseInPlace(
|
||||
}
|
||||
}
|
||||
|
||||
struct SProjectedClusterPoint {
|
||||
SVzNLPointXYZ point;
|
||||
float majorCoord;
|
||||
float minorCoord;
|
||||
};
|
||||
|
||||
struct SClusterLineMetrics {
|
||||
bool isValid;
|
||||
SVzNLPointXYZ axisPointA;
|
||||
SVzNLPointXYZ axisPointB;
|
||||
float meanDistance;
|
||||
float distanceStdDev;
|
||||
float maxDistanceDeviation;
|
||||
float inlierRatio;
|
||||
float majorSpan;
|
||||
|
||||
SClusterLineMetrics()
|
||||
: isValid(false)
|
||||
, axisPointA()
|
||||
, axisPointB()
|
||||
, meanDistance(0.0f)
|
||||
, distanceStdDev(std::numeric_limits<float>::max())
|
||||
, maxDistanceDeviation(std::numeric_limits<float>::max())
|
||||
, inlierRatio(0.0f)
|
||||
, majorSpan(0.0f)
|
||||
{}
|
||||
};
|
||||
|
||||
static float ComputePointToLineDistance(
|
||||
const SVzNLPointXYZ& pt,
|
||||
const SVzNLPointXYZ& lineStart,
|
||||
const SVzNLPointXYZ& lineEnd
|
||||
) {
|
||||
const float dirX = lineEnd.x - lineStart.x;
|
||||
const float dirY = lineEnd.y - lineStart.y;
|
||||
const float dirZ = lineEnd.z - lineStart.z;
|
||||
|
||||
const float vX = pt.x - lineStart.x;
|
||||
const float vY = pt.y - lineStart.y;
|
||||
const float vZ = pt.z - lineStart.z;
|
||||
|
||||
const float crossX = vY * dirZ - vZ * dirY;
|
||||
const float crossY = vZ * dirX - vX * dirZ;
|
||||
const float crossZ = vX * dirY - vY * dirX;
|
||||
const float crossNorm = std::sqrt(crossX * crossX + crossY * crossY + crossZ * crossZ);
|
||||
const float dirNorm = std::sqrt(dirX * dirX + dirY * dirY + dirZ * dirZ);
|
||||
if (dirNorm < 1e-6f) {
|
||||
return std::numeric_limits<float>::max();
|
||||
}
|
||||
|
||||
return crossNorm / dirNorm;
|
||||
}
|
||||
|
||||
static SVzNLPointXYZ ComputeDirectionVector(
|
||||
const SVzNLPointXYZ& startPoint,
|
||||
const SVzNLPointXYZ& endPoint
|
||||
) {
|
||||
SVzNLPointXYZ dir = {};
|
||||
dir.x = endPoint.x - startPoint.x;
|
||||
dir.y = endPoint.y - startPoint.y;
|
||||
dir.z = endPoint.z - startPoint.z;
|
||||
return dir;
|
||||
}
|
||||
|
||||
static float ComputeVectorNorm(const SVzNLPointXYZ& vec) {
|
||||
return std::sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
|
||||
}
|
||||
|
||||
static float ComputeNormalizedAbsDot(
|
||||
const SVzNLPointXYZ& lhs,
|
||||
const SVzNLPointXYZ& rhs
|
||||
) {
|
||||
const float lhsNorm = ComputeVectorNorm(lhs);
|
||||
const float rhsNorm = ComputeVectorNorm(rhs);
|
||||
if (lhsNorm < 1e-6f || rhsNorm < 1e-6f) {
|
||||
return 1.0f;
|
||||
}
|
||||
|
||||
const float dot = lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z;
|
||||
return std::abs(dot) / (lhsNorm * rhsNorm);
|
||||
}
|
||||
|
||||
static bool FitCircleCenter2D(
|
||||
const std::vector<SProjectedClusterPoint>& slicePoints,
|
||||
float* outMinorCenter,
|
||||
float* outZCenter,
|
||||
float* outRadius
|
||||
) {
|
||||
if (!outMinorCenter || !outZCenter || !outRadius || slicePoints.size() < 3) {
|
||||
return false;
|
||||
}
|
||||
|
||||
Eigen::MatrixXf A(static_cast<int>(slicePoints.size()), 3);
|
||||
Eigen::VectorXf b(static_cast<int>(slicePoints.size()));
|
||||
for (int i = 0; i < static_cast<int>(slicePoints.size()); ++i) {
|
||||
const float u = slicePoints[i].minorCoord;
|
||||
const float z = slicePoints[i].point.z;
|
||||
A(i, 0) = 2.0f * u;
|
||||
A(i, 1) = 2.0f * z;
|
||||
A(i, 2) = 1.0f;
|
||||
b(i) = u * u + z * z;
|
||||
}
|
||||
|
||||
const Eigen::Vector3f solution = A.colPivHouseholderQr().solve(b);
|
||||
if (!solution.allFinite()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const float centerU = solution(0);
|
||||
const float centerZ = solution(1);
|
||||
const float radiusSq = solution(2) + centerU * centerU + centerZ * centerZ;
|
||||
if (radiusSq <= 1e-6f) {
|
||||
return false;
|
||||
}
|
||||
|
||||
*outMinorCenter = centerU;
|
||||
*outZCenter = centerZ;
|
||||
*outRadius = std::sqrt(radiusSq);
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool CollectSlicePoints(
|
||||
const std::vector<SProjectedClusterPoint>& projectedPoints,
|
||||
float targetMajorCoord,
|
||||
float initialHalfWidth,
|
||||
int minPointCount,
|
||||
std::vector<SProjectedClusterPoint>* outSlicePoints,
|
||||
float* outMeanMajorCoord
|
||||
) {
|
||||
if (!outSlicePoints || !outMeanMajorCoord || projectedPoints.empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
float halfWidth = initialHalfWidth;
|
||||
for (int expand = 0; expand < 4; ++expand) {
|
||||
outSlicePoints->clear();
|
||||
float majorSum = 0.0f;
|
||||
for (size_t i = 0; i < projectedPoints.size(); ++i) {
|
||||
if (std::abs(projectedPoints[i].majorCoord - targetMajorCoord) > halfWidth) {
|
||||
continue;
|
||||
}
|
||||
outSlicePoints->push_back(projectedPoints[i]);
|
||||
majorSum += projectedPoints[i].majorCoord;
|
||||
}
|
||||
|
||||
if (static_cast<int>(outSlicePoints->size()) >= minPointCount) {
|
||||
*outMeanMajorCoord = majorSum / static_cast<float>(outSlicePoints->size());
|
||||
return true;
|
||||
}
|
||||
|
||||
halfWidth *= 1.5f;
|
||||
}
|
||||
|
||||
outSlicePoints->clear();
|
||||
return false;
|
||||
}
|
||||
|
||||
static SClusterLineMetrics EvaluateClusterLinearity(
|
||||
const SGrowthCluster& cluster,
|
||||
const SVzNLPointXYZ* alignedPoints,
|
||||
int totalPoints,
|
||||
const SGrowthParams& growthParams
|
||||
) {
|
||||
SClusterLineMetrics metrics;
|
||||
if (!alignedPoints || cluster.pointIndices.size() < 6) {
|
||||
return metrics;
|
||||
}
|
||||
|
||||
std::vector<SVzNLPointXYZ> clusterPoints;
|
||||
clusterPoints.reserve(cluster.pointIndices.size());
|
||||
for (size_t i = 0; i < cluster.pointIndices.size(); ++i) {
|
||||
const int linearIdx = cluster.pointIndices[i];
|
||||
if (linearIdx < 0 || linearIdx >= totalPoints) {
|
||||
continue;
|
||||
}
|
||||
if (!IsValidPoint(alignedPoints[linearIdx])) {
|
||||
continue;
|
||||
}
|
||||
clusterPoints.push_back(alignedPoints[linearIdx]);
|
||||
}
|
||||
|
||||
if (clusterPoints.size() < 6) {
|
||||
return metrics;
|
||||
}
|
||||
|
||||
float centroidX = 0.0f;
|
||||
float centroidY = 0.0f;
|
||||
for (size_t i = 0; i < clusterPoints.size(); ++i) {
|
||||
centroidX += clusterPoints[i].x;
|
||||
centroidY += clusterPoints[i].y;
|
||||
}
|
||||
centroidX /= static_cast<float>(clusterPoints.size());
|
||||
centroidY /= static_cast<float>(clusterPoints.size());
|
||||
|
||||
float covXX = 0.0f;
|
||||
float covXY = 0.0f;
|
||||
float covYY = 0.0f;
|
||||
for (size_t i = 0; i < clusterPoints.size(); ++i) {
|
||||
const float dx = clusterPoints[i].x - centroidX;
|
||||
const float dy = clusterPoints[i].y - centroidY;
|
||||
covXX += dx * dx;
|
||||
covXY += dx * dy;
|
||||
covYY += dy * dy;
|
||||
}
|
||||
|
||||
const float theta = 0.5f * std::atan2(2.0f * covXY, covXX - covYY);
|
||||
const float majorDirX = std::cos(theta);
|
||||
const float majorDirY = std::sin(theta);
|
||||
const float minorDirX = -majorDirY;
|
||||
const float minorDirY = majorDirX;
|
||||
|
||||
std::vector<SProjectedClusterPoint> projectedPoints;
|
||||
projectedPoints.reserve(clusterPoints.size());
|
||||
float minMajorCoord = std::numeric_limits<float>::max();
|
||||
float maxMajorCoord = -std::numeric_limits<float>::max();
|
||||
for (size_t i = 0; i < clusterPoints.size(); ++i) {
|
||||
SProjectedClusterPoint projectedPoint;
|
||||
projectedPoint.point = clusterPoints[i];
|
||||
projectedPoint.majorCoord =
|
||||
(clusterPoints[i].x - centroidX) * majorDirX +
|
||||
(clusterPoints[i].y - centroidY) * majorDirY;
|
||||
projectedPoint.minorCoord =
|
||||
(clusterPoints[i].x - centroidX) * minorDirX +
|
||||
(clusterPoints[i].y - centroidY) * minorDirY;
|
||||
projectedPoints.push_back(projectedPoint);
|
||||
if (projectedPoint.majorCoord < minMajorCoord) minMajorCoord = projectedPoint.majorCoord;
|
||||
if (projectedPoint.majorCoord > maxMajorCoord) maxMajorCoord = projectedPoint.majorCoord;
|
||||
}
|
||||
|
||||
metrics.majorSpan = maxMajorCoord - minMajorCoord;
|
||||
const float radialTolerance = std::max(1.0f, std::max(growthParams.thresholdY, growthParams.thresholdZ));
|
||||
const float minLineSpan =
|
||||
std::max(radialTolerance * 6.0f, std::max(growthParams.thresholdX, growthParams.thresholdY) * 3.0f);
|
||||
if (metrics.majorSpan < minLineSpan) {
|
||||
return metrics;
|
||||
}
|
||||
|
||||
const float sliceHalfWidth =
|
||||
std::max(metrics.majorSpan * 0.08f, std::max(growthParams.thresholdX, growthParams.thresholdY));
|
||||
const int minSlicePointCount = std::max(6, cluster.pointCount / 10);
|
||||
|
||||
std::vector<SProjectedClusterPoint> slicePointsA;
|
||||
std::vector<SProjectedClusterPoint> slicePointsB;
|
||||
float sliceMajorCoordA = 0.0f;
|
||||
float sliceMajorCoordB = 0.0f;
|
||||
const float targetMajorCoordA = minMajorCoord + metrics.majorSpan * 0.30f;
|
||||
const float targetMajorCoordB = maxMajorCoord - metrics.majorSpan * 0.30f;
|
||||
if (!CollectSlicePoints(
|
||||
projectedPoints, targetMajorCoordA, sliceHalfWidth, minSlicePointCount,
|
||||
&slicePointsA, &sliceMajorCoordA
|
||||
)) {
|
||||
return metrics;
|
||||
}
|
||||
if (!CollectSlicePoints(
|
||||
projectedPoints, targetMajorCoordB, sliceHalfWidth, minSlicePointCount,
|
||||
&slicePointsB, &sliceMajorCoordB
|
||||
)) {
|
||||
return metrics;
|
||||
}
|
||||
|
||||
float centerMinorA = 0.0f;
|
||||
float centerZA = 0.0f;
|
||||
float radiusA = 0.0f;
|
||||
if (!FitCircleCenter2D(slicePointsA, ¢erMinorA, ¢erZA, &radiusA)) {
|
||||
return metrics;
|
||||
}
|
||||
|
||||
float centerMinorB = 0.0f;
|
||||
float centerZB = 0.0f;
|
||||
float radiusB = 0.0f;
|
||||
if (!FitCircleCenter2D(slicePointsB, ¢erMinorB, ¢erZB, &radiusB)) {
|
||||
return metrics;
|
||||
}
|
||||
|
||||
metrics.axisPointA.x = centroidX + sliceMajorCoordA * majorDirX + centerMinorA * minorDirX;
|
||||
metrics.axisPointA.y = centroidY + sliceMajorCoordA * majorDirY + centerMinorA * minorDirY;
|
||||
metrics.axisPointA.z = centerZA;
|
||||
metrics.axisPointB.x = centroidX + sliceMajorCoordB * majorDirX + centerMinorB * minorDirX;
|
||||
metrics.axisPointB.y = centroidY + sliceMajorCoordB * majorDirY + centerMinorB * minorDirY;
|
||||
metrics.axisPointB.z = centerZB;
|
||||
|
||||
const float axisLength = std::sqrt(
|
||||
(metrics.axisPointB.x - metrics.axisPointA.x) * (metrics.axisPointB.x - metrics.axisPointA.x) +
|
||||
(metrics.axisPointB.y - metrics.axisPointA.y) * (metrics.axisPointB.y - metrics.axisPointA.y) +
|
||||
(metrics.axisPointB.z - metrics.axisPointA.z) * (metrics.axisPointB.z - metrics.axisPointA.z)
|
||||
);
|
||||
if (axisLength < 1e-6f) {
|
||||
return metrics;
|
||||
}
|
||||
|
||||
const float axisDirZ = std::abs(metrics.axisPointB.z - metrics.axisPointA.z) / axisLength;
|
||||
const float clampedAxisDeviationDeg = std::max(0.0f, std::min(growthParams.maxAxisDeviationFromXYDeg, 89.0f));
|
||||
const float maxAxisDirZ = std::sin(clampedAxisDeviationDeg * 3.1415926f / 180.0f);
|
||||
if (axisDirZ > maxAxisDirZ) {
|
||||
return metrics;
|
||||
}
|
||||
|
||||
float distanceSum = 0.0f;
|
||||
float distanceSqSum = 0.0f;
|
||||
std::vector<float> distances;
|
||||
distances.reserve(clusterPoints.size());
|
||||
for (size_t i = 0; i < clusterPoints.size(); ++i) {
|
||||
const float distance = ComputePointToLineDistance(
|
||||
clusterPoints[i],
|
||||
metrics.axisPointA,
|
||||
metrics.axisPointB
|
||||
);
|
||||
distances.push_back(distance);
|
||||
distanceSum += distance;
|
||||
distanceSqSum += distance * distance;
|
||||
}
|
||||
|
||||
metrics.meanDistance = distanceSum / static_cast<float>(distances.size());
|
||||
const float meanSqDistance = distanceSqSum / static_cast<float>(distances.size());
|
||||
metrics.distanceStdDev = std::sqrt(std::max(0.0f, meanSqDistance - metrics.meanDistance * metrics.meanDistance));
|
||||
|
||||
const float radiusConsistencyTolerance = std::max(radialTolerance, metrics.meanDistance * 0.12f);
|
||||
int inlierCount = 0;
|
||||
float maxDeviation = 0.0f;
|
||||
for (size_t i = 0; i < distances.size(); ++i) {
|
||||
const float deviation = std::abs(distances[i] - metrics.meanDistance);
|
||||
if (deviation > maxDeviation) {
|
||||
maxDeviation = deviation;
|
||||
}
|
||||
if (deviation <= radiusConsistencyTolerance) {
|
||||
inlierCount++;
|
||||
}
|
||||
}
|
||||
|
||||
metrics.maxDistanceDeviation = maxDeviation;
|
||||
metrics.inlierRatio = static_cast<float>(inlierCount) / static_cast<float>(distances.size());
|
||||
metrics.isValid =
|
||||
std::abs(radiusA - radiusB) <= radiusConsistencyTolerance &&
|
||||
metrics.inlierRatio >= 0.75f &&
|
||||
metrics.distanceStdDev <= radiusConsistencyTolerance &&
|
||||
metrics.maxDistanceDeviation <= radiusConsistencyTolerance * 2.0f;
|
||||
|
||||
return metrics;
|
||||
}
|
||||
|
||||
BAR_INTERSECTION_API int DetectBarIntersections(
|
||||
int DetectBarIntersections(
|
||||
const SVzNLPointXYZ* points,
|
||||
int rows,
|
||||
int cols,
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user