jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
algoLib/sourceCode/SG_baseFunc.cpp

1982 lines
50 KiB
C++
Raw Normal View History

source code
2025-06-08 10:46:41 +08:00
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include <vector>
#include <corecrt_math_defines.h>
#include <cmath>
修正对opencv320版本的兼容问题
2025-09-15 21:22:32 +08:00
#include <unordered_map>
source code
2025-06-08 10:46:41 +08:00
SVzNL3DRangeD sg_getScanDataROI(
SVzNL3DLaserLine* laser3DPoints,
int lineNum)
{
SVzNL3DRangeD roi;
roi.xRange = { 0, -1 };
roi.yRange = { 0, -1 };
roi.zRange = { 0, -1 };
for (int line = 0; line < lineNum; line++)
{
for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
{
SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
if (pt3D->pt3D.z < 1e-4)
continue;
if (roi.xRange.max < roi.xRange.min)
{
roi.xRange.min = pt3D->pt3D.x;
roi.xRange.max = pt3D->pt3D.x;
}
else
{
if (roi.xRange.min > pt3D->pt3D.x)
roi.xRange.min = pt3D->pt3D.x;
if (roi.xRange.max < pt3D->pt3D.x)
roi.xRange.max = pt3D->pt3D.x;
}
//y
if (roi.yRange.max < roi.yRange.min)
{
roi.yRange.min = pt3D->pt3D.y;
roi.yRange.max = pt3D->pt3D.y;
}
else
{
if (roi.yRange.min > pt3D->pt3D.y)
roi.yRange.min = pt3D->pt3D.y;
if (roi.yRange.max < pt3D->pt3D.y)
roi.yRange.max = pt3D->pt3D.y;
}
//z
if (roi.zRange.max < roi.zRange.min)
{
roi.zRange.min = pt3D->pt3D.z;
roi.zRange.max = pt3D->pt3D.z;
}
else
{
if (roi.zRange.min > pt3D->pt3D.z)
roi.zRange.min = pt3D->pt3D.z;
if (roi.zRange.max < pt3D->pt3D.z)
roi.zRange.max = pt3D->pt3D.z;
}
}
}
return roi;
}
添加了拆包算法中编织袋正反识别和朝向识别算法，以及相应的测试程序
2025-07-15 21:39:09 +08:00
SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
SVzNL3DRangeD roi;
roi.xRange = { 0, -1 };
roi.yRange = { 0, -1 };
roi.zRange = { 0, -1 };
解决编译中的warning
2025-07-22 22:52:57 +08:00
int lineNum = (int)scanLines.size();
添加了拆包算法中编织袋正反识别和朝向识别算法，以及相应的测试程序
2025-07-15 21:39:09 +08:00
for (int line = 0; line < lineNum; line++)
{
解决编译中的warning
2025-07-22 22:52:57 +08:00
int nPositionCnt = (int)scanLines[line].size();
添加了拆包算法中编织袋正反识别和朝向识别算法，以及相应的测试程序
2025-07-15 21:39:09 +08:00
for (int i = 0; i < nPositionCnt; i++)
{
SVzNL3DPosition* pt3D = &scanLines[line][i];
if (pt3D->pt3D.z < 1e-4)
continue;
if (roi.xRange.max < roi.xRange.min)
{
roi.xRange.min = pt3D->pt3D.x;
roi.xRange.max = pt3D->pt3D.x;
}
else
{
if (roi.xRange.min > pt3D->pt3D.x)
roi.xRange.min = pt3D->pt3D.x;
if (roi.xRange.max < pt3D->pt3D.x)
roi.xRange.max = pt3D->pt3D.x;
}
//y
if (roi.yRange.max < roi.yRange.min)
{
roi.yRange.min = pt3D->pt3D.y;
roi.yRange.max = pt3D->pt3D.y;
}
else
{
if (roi.yRange.min > pt3D->pt3D.y)
roi.yRange.min = pt3D->pt3D.y;
if (roi.yRange.max < pt3D->pt3D.y)
roi.yRange.max = pt3D->pt3D.y;
}
//z
if (roi.zRange.max < roi.zRange.min)
{
roi.zRange.min = pt3D->pt3D.z;
roi.zRange.max = pt3D->pt3D.z;
}
else
{
if (roi.zRange.min > pt3D->pt3D.z)
roi.zRange.min = pt3D->pt3D.z;
if (roi.zRange.max < pt3D->pt3D.z)
roi.zRange.max = pt3D->pt3D.z;
}
}
}
return roi;
}
source code
2025-06-08 10:46:41 +08:00
void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b)
{
//<2F><>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>߲<EFBFBD><DFB2><EFBFBD>
double xx_sum = 0;
double x_sum = 0;
double y_sum = 0;
double xy_sum = 0;
int num = 0;
for (int i = 0; i < inliers.size(); i++)
{
x_sum += inliers[i].x; //x<><78><EFBFBD>ۼӺ<DBBC>
y_sum += inliers[i].y; //y<><79><EFBFBD>ۼӺ<DBBC>
xx_sum += inliers[i].x * inliers[i].x; //x<><78>ƽ<EFBFBD><C6BD><EFBFBD>ۼӺ<DBBC>
xy_sum += inliers[i].x * inliers[i].y; //x<><78>y<EFBFBD><79><EFBFBD>ۼӺ<DBBC>
num++;
}
*_k = (num * xy_sum - x_sum * y_sum) / (num * xx_sum - x_sum * x_sum); //<2F><><EFBFBD>ݹ<EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD>k
*_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//<2F><><EFBFBD>ݹ<EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD>b
}
拆包应用加强了旋转旋转的包的检测功能。这样功能由一个开关控制
2025-10-06 14:21:33 +08:00
SVzNL2DPointD sx_getFootPoint(double x0, double y0, double k, double b)
{
double A = k;
double B = -1;
double C = b;
SVzNL2DPointD foot;
foot.x = (B * B * x0 - A * B * y0 - A * C) / (A * A + B * B);
foot.y = (-A * B * x0 + A * A * y0 - B * C) / (A * A + B * B);
return foot;
}
source code
2025-06-08 10:46:41 +08:00
#if 0
void icvprCcaByTwoPass(const cv::Mat& binImg, cv::Mat& lableImg)
{
// connected component analysis (4-component)
// use two-pass algorithm
// 1. first pass: label each foreground pixel with a label
// 2. second pass: visit each labeled pixel and merge neighbor labels
//
// foreground pixel: binImg(x,y) = 1
// background pixel: binImg(x,y) = 0
if (binImg.empty() ||
binImg.type() != CV_8UC1)
{
return;
}
// 1. first pass
lableImg.release();
binImg.convertTo(lableImg, CV_32SC1);
int label = 1; // start by 2
std::vector<int> labelSet;
labelSet.push_back(0); // background: 0
labelSet.push_back(1); // foreground: 1
int rows = binImg.rows - 1;
int cols = binImg.cols - 1;
for (int i = 1; i < rows; i++)
{
int* data_preRow = lableImg.ptr<int>(i - 1);
int* data_curRow = lableImg.ptr<int>(i);
for (int j = 1; j < cols; j++)
{
if (data_curRow[j] == 1)
{
std::vector<int> neighborLabels;
neighborLabels.reserve(2);
int leftPixel = data_curRow[j - 1];
int upPixel = data_preRow[j];
if (leftPixel > 1)
{
neighborLabels.push_back(leftPixel);
}
if (upPixel > 1)
{
neighborLabels.push_back(upPixel);
}
if (neighborLabels.empty())
{
labelSet.push_back(++label); // assign to a new label
data_curRow[j] = label;
labelSet[label] = label;
}
else
{
std::sort(neighborLabels.begin(), neighborLabels.end());
int smallestLabel = neighborLabels[0];
data_curRow[j] = smallestLabel;
// save equivalence
for (size_t k = 1; k < neighborLabels.size(); k++)
{
int tempLabel = neighborLabels[k];
int& oldSmallestLabel = labelSet[tempLabel];
if (oldSmallestLabel > smallestLabel)
{
labelSet[oldSmallestLabel] = smallestLabel;
oldSmallestLabel = smallestLabel;
}
else if (oldSmallestLabel < smallestLabel)
{
labelSet[smallestLabel] = oldSmallestLabel;
}
}
}
}
}
}
// update equivalent labels
// assigned with the smallest label in each equivalent label set
for (size_t i = 2; i < labelSet.size(); i++)
{
int curLabel = labelSet[i];
int preLabel = labelSet[curLabel];
while (preLabel != curLabel)
{
curLabel = preLabel;
preLabel = labelSet[preLabel];
}
labelSet[i] = curLabel;
}
// 2. second pass
for (int i = 0; i < rows; i++)
{
int* data = lableImg.ptr<int>(i);
for (int j = 0; j < cols; j++)
{
int& pixelLabel = data[j];
pixelLabel = labelSet[pixelLabel];
}
}
}
#endif
#if 0
//Bresenham<61>㷨
void line(int x0, int y0, int x1, int y1, TGAImage& image, TGAColor color) {
bool steep = false;
if (std::abs(x1 - x0) < std::abs(y1 - y0)) {
std::swap(x0, y0);
std::swap(x1, y1);
steep = true;
}
if (x0 > x1) {
std::swap(x0, x1);
std::swap(y0, y1);
}
int dx = x1 - x0;
int dy = y1 - y0;
int deltaY = std::abs(dy << 1);
int middle = dx;
int y = y0;
for (int x = x0; x <= x1; ++x) {
if (steep) {
image.set(y, x, color);
}
else {
image.set(x, y, color);
}
deltaY += std::abs(dy << 1);
if (deltaY >= middle) {
y += (y1 > y0 ? 1 : -1);
middle += std::abs(dx << 1);
}
}
}
#endif
//Bresenham<61>㷨
void drawLine(
int x0,
int y0,
int x1,
int y1,
std::vector<SVzNL2DPoint>& pts)
{
// <20><><EFBFBD><EFBFBD>dx<64><78>dy<64>ľ<EFBFBD><C4BE><EFBFBD>ֵ
int dx = abs(x1 - x0);
int dy = abs(y1 - y0);
// ȷ<><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int sx = (x0 < x1) ? 1 : -1; // x<><78><EFBFBD>򲽽<EFBFBD>
int sy = (y0 < y1) ? 1 : -1; // y<><79><EFBFBD>򲽽<EFBFBD>
// <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>dx<64><78>dy<64>ķ<EFBFBD><C4B7><EFBFBD>
int err = dx - dy;
while (true) {
SVzNL2DPoint a_pt = { x0, y0 };
pts.push_back(a_pt);
// <20><><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD>ʱ<EFBFBD>˳<EFBFBD>ѭ<EFBFBD><D1AD>
if (x0 == x1 && y0 == y1) break;
int e2 = 2 * err; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (e2 > -dy) { // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>x<EFBFBD><78><EFBFBD>򲽽<EFBFBD>
err -= dy;
x0 += sx;
}
if (e2 < dx) { // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>y<EFBFBD><79><EFBFBD>򲽽<EFBFBD>
err += dx;
y0 += sy;
}
}
}
/// <summary>
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע
/// </summary>
/// <param name="bwImg"> Ŀ<><C4BF><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>1<EFBFBD><31><EFBFBD><EFBFBD> <20>հ׵<D5B0>Ϊ<EFBFBD><CEAA>0<EFBFBD><30></param>
/// <param name="labImg"> <20><>ע<EFBFBD><D7A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD>ΪrgnID, ID<49><44>2<EFBFBD><32>ʼ </param>
/// <param name="labelRgns"></param>
#if 0
void SG_TwoPassLabel(
const cv::Mat& bwImg,
cv::Mat& labImg,
std::vector<SSG_Region>& labelRgns,
int connectivity)
{
assert(bwImg.type() == CV_8UC1);
bwImg.convertTo(labImg, CV_32SC1);
int rows = bwImg.rows - 1;
int cols = bwImg.cols - 1;
//<2F><>ֵͼ<D6B5><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵΪ0<CEAA><30>1<EFBFBD><31>Ϊ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD>ͻ<EFBFBD><CDBB>label<65><6C>2<EFBFBD><32>ʼ
int label = 2;
std::vector<int> labelSet;
labelSet.push_back(0);
labelSet.push_back(1);
//<2F><>һ<EFBFBD><D2BB>ɨ<EFBFBD><C9A8>
int* data_prev = (int*)labImg.data;
int* data_cur = (int*)(labImg.data + labImg.step);
int left, up;//ָ<><D6B8>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD>󷽵<EFBFBD><F3B7BDB5><EFBFBD><EFBFBD>Ϸ<EFBFBD><CFB7><EFBFBD>
int neighborLabels[2];
for (int i = 1; i < rows; i++)// <20><><EFBFBD>Ե<EFBFBD>һ<EFBFBD>к͵<D0BA>һ<EFBFBD><D2BB>,<2C><>ʵ<EFBFBD><CAB5><EFBFBD>Խ<EFBFBD>labImg<6D>Ŀ<EFBFBD><C4BF>߼<EFBFBD>1<EFBFBD><31>Ȼ<EFBFBD><C8BB><EFBFBD>ڳ<EFBFBD>ʼ<EFBFBD><CABC>Ϊ0<CEAA>Ϳ<EFBFBD><CDBF><EFBFBD><EFBFBD><EFBFBD>
{
data_cur++;
data_prev++;
for (int j = 1; j < cols; j++, data_cur++, data_prev++)
{
if ((i == 1409) && (j == 432))
int kkk = 1;
if (*data_cur != 1)//<2F><>ǰ<EFBFBD>㲻Ϊ1<CEAA><31>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
continue;
left = *(data_cur - 1);
up = *data_prev;
int count = 0;
for (int curLabel : {left, up})
{
if (curLabel > 1)
neighborLabels[count++] = curLabel;
}
if (!count)//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>µ<EFBFBD>label
{
labelSet.push_back(label);
*data_cur = label;
label++;
continue;
}
//<2F><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>label<65><6C><EFBFBD><EFBFBD>Сֵ
int smallestLabel = neighborLabels[0];
if (count == 2 && neighborLabels[1] < smallestLabel)
smallestLabel = neighborLabels[1];
*data_cur = smallestLabel;
//<2F><><EFBFBD>õȼ۱<C8BC><DBB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD>п<EFBFBD><D0BF>ܱ<EFBFBD><DCB1>ϵ<EFBFBD>С<EFBFBD><D0A1>Ҳ<EFBFBD>п<EFBFBD><D0BF>ܱ<EFBFBD><DCB1>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>
//0 0 1 0 1 0 x x 2 x 3 x
//1 1 1 1 1 1 -> 4 4 2 2 2 2
//Ҫ<><D2AA>labelSet<65><74>3<EFBFBD><33>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ2
for (int k = 0; k < count; k++)
{
int neiLabel = neighborLabels[k];
int oldSmallestLabel = labelSet[neiLabel];
if (oldSmallestLabel > smallestLabel)
{
if ((oldSmallestLabel == 117) && (smallestLabel == 113))
int kkk = 1;
labelSet[oldSmallestLabel] = smallestLabel;
}
else if (oldSmallestLabel < smallestLabel)
{
if ((smallestLabel == 117) && (oldSmallestLabel == 113))
int kkk = 1;
if (labelSet[smallestLabel] != oldSmallestLabel)
{
}
labelSet[smallestLabel] = oldSmallestLabel;
}
}
}
data_cur++;
data_prev++;
}
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>,<2C>е<EFBFBD>labelSet<65><74>λ<EFBFBD>û<EFBFBD>δ<EFBFBD><CEB4>Ϊ<EFBFBD><CEAA>Сֵ<D0A1><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//0 0 1 0 1 x x 2 x 3
//0 1 1 1 1 -> x 4 2 2 2
//1 1 1 0 1 5 4 2 x 2
//<2F><><EFBFBD><EFBFBD><EFBFBD>Ⲩ<EFBFBD><E2B2A8><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD>labelSet[4]<5D><>Ϊ2<CEAA><32><EFBFBD><EFBFBD>labelSet[5]<5D><>Ϊ4
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD>labelSet[5]<5D><>Ϊ2
for (size_t i = 2; i < labelSet.size(); i++)
{
int curLabel = labelSet[i];
int prelabel = labelSet[curLabel];
while (prelabel != curLabel)
{
curLabel = prelabel;
prelabel = labelSet[prelabel];
}
labelSet[i] = curLabel;
}
//<2F>ڶ<EFBFBD><DAB6><EFBFBD>ɨ<EFBFBD>裬<EFBFBD><E8A3AC>labelSet<65><74><EFBFBD>и<EFBFBD><D0B8>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
std::vector<SSG_Region*> labelInfo;
labelInfo.resize(labelSet.size(), nullptr);
data_cur = (int*)labImg.data;
for (int i = 0; i < labImg.rows; i++)
{
for (int j = 0; j < labImg.cols; j++)
{
*data_cur = labelSet[*data_cur];
if (*data_cur > 1) //<2F><>Чlabel
{
//ͳ<><CDB3>Region<6F><6E>Ϣ
SSG_Region* info_cur = (SSG_Region*)labelInfo[*data_cur];
if (nullptr == info_cur)
{
SSG_Region new_rgn = { {j,j,i,i}, 1, *data_cur };
labelRgns.push_back(new_rgn); //push_back()<29><><EFBFBD><EFBFBD>vector<6F><72><EFBFBD>ڴ浥Ԫ<E6B5A5><D4AA><EFBFBD>ܻᱻ<DCBB>Ķ<EFBFBD>
for (int m = 0; m < labelRgns.size(); m++)
{
info_cur = &labelRgns[m];
labelInfo[info_cur->labelID] = info_cur;
}
}
else
{
assert(*data_cur == info_cur->labelID);
if (info_cur->roi.left > j)
info_cur->roi.left = j;
if (info_cur->roi.right < j)
info_cur->roi.right = j;
if (info_cur->roi.top > i)
info_cur->roi.top = i;
if (info_cur->roi.bottom < i)
info_cur->roi.bottom = i;
info_cur->ptCounter++;
}
}
data_cur++;
}
}
return;
}
#else
// <20><><EFBFBD>Һ<EFBFBD><D2BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><C2B7>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD>
int find(int x, std::vector<int>& parent) {
if (parent[x] != x) {
parent[x] = find(parent[x], parent);
}
return parent[x];
}
// <20>ϲ<EFBFBD><CFB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ⱥϲ<C8BA><CFB2><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD>
void unionSet(int x, int y, std::vector<int>& parent) {
int rootX = find(x, parent);
int rootY = find(y, parent);
if (rootX != rootY) {
if (rootX < rootY) {
parent[rootY] = rootX;
}
else {
parent[rootX] = rootY;
}
}
}
/**
* @brief <EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param image <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵͼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0Ϊǰ<EFBFBD><EFBFBD>
* @param labels <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param connectivity <EFBFBD><EFBFBD>ͨ<EFBFBD>ԣ<EFBFBD>4<EFBFBD><EFBFBD>8<EFBFBD><EFBFBD>
*/
void SG_TwoPassLabel(
const cv::Mat& bwImg,
cv::Mat& labImg,
std::vector<SSG_Region>& labelRgns,
int connectivity)
{
assert(bwImg.type() == CV_8UC1);
bwImg.convertTo(labImg, CV_32SC1);
if (bwImg.rows == 0)
return;
int rows = bwImg.rows - 1;
int cols = bwImg.cols - 1;
// <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>鼯<EFBFBD><E9BCAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܱ<EFBFBD>ǩ<EFBFBD><C7A9>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int max_label = rows * cols;
std::vector<int> parent(max_label + 1);
for (int i = 0; i <= max_label; ++i) {
parent[i] = i;
}
//<2F><>һ<EFBFBD><D2BB>ɨ<EFBFBD><C9A8>
int label_cnt = 2; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ,<2C><>ֵͼ<D6B5><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵΪ0<CEAA><30>1<EFBFBD><31>Ϊ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD>ͻ<EFBFBD><CDBB>label<65><6C>2<EFBFBD><32>ʼ
int* data_prev = (int*)labImg.data;
int* data_cur = (int*)(labImg.data + labImg.step);
// <20><>һ<EFBFBD><D2BB>ɨ<EFBFBD>裺<EFBFBD><E8A3BA>ʱ<EFBFBD><CAB1>ǩ<EFBFBD><C7A9><EFBFBD><EFBFBD>
for (int i = 1; i < rows; i++)
{
data_cur++;
data_prev++;
for (int j = 1; j < cols; j++, data_cur++, data_prev++)
{
if (*data_cur != 1)//<2F><>ǰ<EFBFBD>㲻Ϊ1<CEAA><31>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
continue;
int left = *(data_cur - 1);
int up = *data_prev;
int up_left = *(data_prev-1);
int up_right = *(data_prev + 1);
std::vector<int> neighbors;
auto add_neighbor = [&](int neiLabel) {
if (neiLabel != 0) {
neighbors.push_back(find(neiLabel, parent));
}
};
// <20><><EFBFBD><EFBFBD><EFBFBD>Ѵ<EFBFBD><D1B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(up > 1)
add_neighbor(up); // <20><>
if( (left > 1) && (left != up))
add_neighbor(left); // <20><>
if (connectivity == 8)
{
if( (up_left > 1) && (up_left != up) && (up_left != left))
add_neighbor(up_left); // <20><><EFBFBD><EFBFBD>
if( (up_right > 1) && (up_right != up) && (up_right != left) && (up_right != up_left))
add_neighbor(up_right); // <20><><EFBFBD><EFBFBD>
}
if (neighbors.empty()) { // <20><><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8>
*data_cur = label_cnt++;
}
else { // <20>ϲ<EFBFBD><CFB2><EFBFBD><EFBFBD><EFBFBD>
int min_root = *std::min_element(neighbors.begin(), neighbors.end());
*data_cur = min_root;
for (int root : neighbors)
{
if (root != min_root)
{
unionSet(root, min_root, parent);
}
}
}
}
data_cur++;
data_prev++;
}
for (int i = 2; i < label_cnt; i++)
parent[i] = find(parent[i], parent);
data_cur = (int*)labImg.data;
for (int i = 0; i < labImg.rows; i++)
{
for (int j = 0; j < labImg.cols; j++)
{
if (*data_cur > 1)
{
*data_cur = parent[*data_cur];
}
data_cur++;
}
}
std::vector<SSG_Region*> labelInfo;
labelInfo.resize(label_cnt, nullptr);
// <20><><EFBFBD><EFBFBD>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӳ<EFBFBD><D3B3>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ
std::unordered_map<int, int> label_map;
int new_label = 2;
data_cur = (int*)labImg.data;
for (int i = 0; i < labImg.rows; i++)
{
for (int j = 0; j < labImg.cols; j++)
{
if (j == 69)
int kkk = 1;
int lbl = *data_cur;
if (lbl > 1)
{
if (label_map.find(lbl) == label_map.end())
{
label_map[lbl] = new_label++;
}
*data_cur = label_map[lbl];
//ͳ<><CDB3>Region<6F><6E>Ϣ
SSG_Region* info_cur = (SSG_Region*)labelInfo[*data_cur];
if (nullptr == info_cur)
{
SSG_Region new_rgn = { {j,j,i,i}, 1, *data_cur };
labelRgns.push_back(new_rgn); //push_back()<29><><EFBFBD><EFBFBD>vector<6F><72><EFBFBD>ڴ浥Ԫ<E6B5A5><D4AA><EFBFBD>ܻᱻ<DCBB>Ķ<EFBFBD>
for (int m = 0; m < labelRgns.size(); m++)
{
info_cur = &labelRgns[m];
labelInfo[info_cur->labelID] = info_cur;
}
}
else
{
assert(*data_cur == info_cur->labelID);
if (info_cur->roi.left > j)
info_cur->roi.left = j;
if (info_cur->roi.right < j)
info_cur->roi.right = j;
if (info_cur->roi.top > i)
info_cur->roi.top = i;
if (info_cur->roi.bottom < i)
info_cur->roi.bottom = i;
info_cur->ptCounter++;
}
}
data_cur++;
}
}
}
#endif
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: z = Ax + By + C
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
void vzCaculateLaserPlane(std::vector<cv::Point3f> Points3ds, std::vector<double>& res)
{
//<2F><>С<EFBFBD><D0A1><EFBFBD>˷<EFBFBD><CBB7><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>
//<2F><>ȡcv::Mat<61><74><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊx<CEAA>ᣬ<EFBFBD><E1A3AC><EFBFBD><EFBFBD>Ϊy<CEAA>ᣬ<EFBFBD><E1A3AC>cvPoint<6E><74><EFBFBD><EFBFBD><EFBFBD>෴
//ϵ<><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
cv::Mat A = cv::Mat::zeros(3, 3, CV_64FC1);
//
cv::Mat B = cv::Mat::zeros(3, 1, CV_64FC1);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
cv::Mat X = cv::Mat::zeros(3, 1, CV_64FC1);
double x2 = 0, xiyi = 0, xi = 0, yi = 0, zixi = 0, ziyi = 0, zi = 0, y2 = 0;
for (int i = 0; i < Points3ds.size(); i++)
{
x2 += (double)Points3ds[i].x * (double)Points3ds[i].x;
y2 += (double)Points3ds[i].y * (double)Points3ds[i].y;
xiyi += (double)Points3ds[i].x * (double)Points3ds[i].y;
xi += (double)Points3ds[i].x;
yi += (double)Points3ds[i].y;
zixi += (double)Points3ds[i].z * (double)Points3ds[i].x;
ziyi += (double)Points3ds[i].z * (double)Points3ds[i].y;
zi += (double)Points3ds[i].z;
}
A.at<double>(0, 0) = x2;
A.at<double>(1, 0) = xiyi;
A.at<double>(2, 0) = xi;
A.at<double>(0, 1) = xiyi;
A.at<double>(1, 1) = y2;
A.at<double>(2, 1) = yi;
A.at<double>(0, 2) = xi;
A.at<double>(1, 2) = yi;
解决编译中的warning
2025-07-22 22:52:57 +08:00
A.at<double>(2, 2) = (double)((int)Points3ds.size());
source code
2025-06-08 10:46:41 +08:00
B.at<double>(0, 0) = zixi;
B.at<double>(1, 0) = ziyi;
B.at<double>(2, 0) = zi;
//<2F><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>ϵ<EFBFBD><CFB5>
X = A.inv() * B;
//A
res.push_back(X.at<double>(0, 0));
//B
res.push_back(X.at<double>(1, 0));
//Z<><5A>ϵ<EFBFBD><CFB5>Ϊ-1
res.push_back(-1.0);
//C
res.push_back(X.at<double>(2, 0));
return;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>淨<EFBFBD><E6B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>ǣ<EFBFBD>ZYX˳<58><CBB3><EFBFBD><EFBFBD>
SSG_EulerAngles planeNormalToEuler(double A, double B, double C) {
SSG_EulerAngles angles = { 0, 0, 0 };
// 1. <20><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double length = std::sqrt(A * A + B * B + C * C);
if (length < 1e-7)
return angles;
double nx = A / length;
double ny = B / length;
double nz = C / length;
// 2. <20><><EFBFBD>㸩<EFBFBD><E3B8A9><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD>Y<EFBFBD>ᣩ
angles.pitch = std::asin(nx) * (180.0 / M_PI); // תΪ<D7AA><CEAA><EFBFBD><EFBFBD>
// 3. <20><><EFBFBD><EFBFBD>Roll<6C><6C><EFBFBD><EFBFBD>X<EFBFBD>ᣩ
const double cos_pitch = std::sqrt(1 - nx * nx); // <20>ȼ<EFBFBD><C8BC><EFBFBD>cos(pitch)
if (cos_pitch > 1e-7) {
// <20><>cos_pitch<63><68><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>atan2<6E><32><EFBFBD><EFBFBD>Roll
angles.roll = std::asin(-ny/ cos_pitch) * (180.0 / M_PI);
}
else {
// <20><>Pitch<63>ӽ<EFBFBD><D3BD><EFBFBD><EFBFBD><EFBFBD>/2ʱ<32><CAB1>Roll<6C>޷<EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ0
angles.roll = 0.0;
}
// 4. <20><><EFBFBD><EFBFBD>yawΪ0<CEAA><30><EFBFBD><EFBFBD>Z<EFBFBD>ᣩ
angles.yaw= 0.0;
return angles;
}
// <20><><EFBFBD><EFBFBD>3x3<78><33>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>
struct RotationMatrix {
double data[3][3]; // <20><><EFBFBD><EFBFBD><EFBFBD>ȴ洢 [row][col]
};
// <20><><EFBFBD>Ƕ<EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
inline double degreesToRadians(double degrees) {
return degrees * M_PI / 180.0;
}
// <20><>ŷ<EFBFBD><C5B7><EFBFBD>Ǽ<EFBFBD><C7BC><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD> (ZYX˳<58><CBB3>: ƫ<><C6AB>Z -> <20><><EFBFBD><EFBFBD>Y -> <20><><EFBFBD><EFBFBD>X)
RotationMatrix eulerToRotationMatrix(double yaw_deg, double pitch_deg, double roll_deg) {
RotationMatrix R;
// <20>Ƕ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
double yaw = degreesToRadians(yaw_deg);
double pitch = degreesToRadians(pitch_deg);
double roll = degreesToRadians(roll_deg);
// Ԥ<><D4A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǻ<EFBFBD><C7BA><EFBFBD>
double cy = cos(yaw);
double sy = sin(yaw);
double cp = cos(pitch);
double sp = sin(pitch);
double cr = cos(roll);
double sr = sin(roll);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Ԫ<EFBFBD>أ<EFBFBD>ZYX˳<58><CBB3> = Rz * Ry * Rx<52><78>
R.data[0][0] = cy * cp;
R.data[0][1] = cy * sp * sr - sy * cr;
R.data[0][2] = cy * sp * cr + sy * sr;
R.data[1][0] = sy * cp;
R.data[1][1] = sy * sp * sr + cy * cr;
R.data[1][2] = sy * sp * cr - cy * sr;
R.data[2][0] = -sp;
R.data[2][1] = cp * sr;
R.data[2][2] = cp * cr;
return R;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ά<EFBFBD><CEAC><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>
struct Vector3 {
double x, y, z;
Vector3(double x_, double y_, double z_) : x(x_), y(y_), z(z_) {}
};
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD>ṹ<EFBFBD><E1B9B9>
struct Quaternion {
double w, x, y, z;
Quaternion(double w_, double x_, double y_, double z_)
: w(w_), x(x_), y(y_), z(z_) {}
};
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ԫ<EFBFBD><D4AA>
Quaternion rotationBetweenVectors(const Vector3& a, const Vector3& b) {
// <20><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
const double eps = 1e-7;
double a_len = std::sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
double b_len = std::sqrt(b.x * b.x + b.y * b.y + b.z * b.z);
if (a_len < eps || b_len < eps) {
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD>λ<EFBFBD><CEBB>Ԫ<EFBFBD><D4AA>
return Quaternion(1.0, 0.0, 0.0, 0.0);
}
Vector3 a_norm(a.x / a_len, a.y / a_len, a.z / a_len);
Vector3 b_norm(b.x / b_len, b.y / b_len, b.z / b_len);
double cos_theta = a_norm.x * b_norm.x + a_norm.y * b_norm.y + a_norm.z * b_norm.z;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (cos_theta > 1.0 - eps) {
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת
return Quaternion(1.0, 0.0, 0.0, 0.0);
}
else if (cos_theta < -1.0 + eps) {
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>෴<EFBFBD><E0B7B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ⴹֱ<E2B4B9><D6B1><EFBFBD><EFBFBD>ת180<38><30>
Vector3 axis(1.0, 0.0, 0.0); // Ĭ<><C4AC>ѡ<EFBFBD><D1A1>X<EFBFBD><58>
if (std::abs(a_norm.y) < eps && std::abs(a_norm.z) < eps) {
// <20><><EFBFBD><EFBFBD>a<EFBFBD>ӽ<EFBFBD>X<EFBFBD>ᣬ<EFBFBD><E1A3AC>ѡ<EFBFBD><D1A1>Y<EFBFBD><59><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ת<EFBFBD><D7AA>
axis = Vector3(0.0, 1.0, 0.0);
}
return Quaternion(0.0, axis.x, axis.y, axis.z); // 180<38><30><EFBFBD><EFBFBD>ת
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ͱ<EFBFBD><CDB0><EFBFBD>
Vector3 axis = Vector3(
a_norm.y * b_norm.z - a_norm.z * b_norm.y,
a_norm.z * b_norm.x - a_norm.x * b_norm.z,
a_norm.x * b_norm.y - a_norm.y * b_norm.x
);
double axis_len = std::sqrt(axis.x * axis.x + axis.y * axis.y + axis.z * axis.z);
if (axis_len < eps) { // <20><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>
return Quaternion(1.0, 0.0, 0.0, 0.0);
}
axis.x /= axis_len;
axis.y /= axis_len;
axis.z /= axis_len;
double half_cos = std::sqrt(0.5 * (1.0 + cos_theta));
double half_sin = std::sqrt(0.5 * (1.0 - cos_theta));
return Quaternion(
half_cos,
half_sin * axis.x,
half_sin * axis.y,
half_sin * axis.z
);
}
void quaternionToMatrix(const Quaternion& q, double mat[3][3]) {
double xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z;
double xy = q.x * q.y, xz = q.x * q.z, yz = q.y * q.z;
double wx = q.w * q.x, wy = q.w * q.y, wz = q.w * q.z;
mat[0][0] = 1 - 2 * (yy + zz);
mat[0][1] = 2 * (xy - wz);
mat[0][2] = 2 * (xz + wy);
mat[1][0] = 2 * (xy + wz);
mat[1][1] = 1 - 2 * (xx + zz);
mat[1][2] = 2 * (yz - wx);
mat[2][0] = 2 * (xz - wy);
mat[2][1] = 2 * (yz + wx);
mat[2][2] = 1 - 2 * (xx + yy);
}
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>п<EFBFBD><D0BF><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>Ͳο<CDB2><CEBF><EFBFBD>ƽƽ<C6BD>棬<EFBFBD><E6A3AC><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>ƽ
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>淨<EFBFBD><E6B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
SSG_planeCalibPara sg_getPlaneCalibPara(
SVzNL3DLaserLine* laser3DPoints,
int lineNum)
{
//<2F><><EFBFBD>ó<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
double initCalib[9]= {
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0 };
SSG_planeCalibPara planePara;
for (int i = 0; i < 9; i++)
planePara.planeCalib[i] = initCalib[i];
planePara.planeHeight = -1.0;
//ͳ<><CDB3>z<EFBFBD><7A>Χ
SVzNLRangeD zRange = { 0, -1 }; //< Z<><5A>Χ
for (int line = 0; line < lineNum; line++)
{
for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
{
SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
if (pt3D->pt3D.z < 1e-4)
continue;
//z
if (zRange.max < zRange.min)
{
zRange.min = pt3D->pt3D.z;
zRange.max = pt3D->pt3D.z;
}
else
{
if (zRange.min > pt3D->pt3D.z)
zRange.min = pt3D->pt3D.z;
if (zRange.max < pt3D->pt3D.z)
zRange.max = pt3D->pt3D.z;
}
}
}
//<2F><>Z<EFBFBD><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͳ<EFBFBD>ƣ<EFBFBD>ȡ<EFBFBD><C8A1>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ֵ
//<2F><>mmΪ<6D><CEAA>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int zHistSize = (int)(zRange.max - zRange.min) + 1;
if (zHistSize == 0)
return planePara;
std::vector<int> zHist;
zHist.resize(zHistSize);
int totalPntSize = 0;
for (int line = 0; line < lineNum; line++)
{
for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
{
SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
if (pt3D->pt3D.z < 1e-4)
continue;
totalPntSize++;
int histPos = (int)(pt3D->pt3D.z - zRange.min);
zHist[histPos] ++;
}
}
std::vector<int> zSumHist;
zSumHist.resize(zHistSize);
添加了拆包算法中编织袋正反识别和朝向识别算法，以及相应的测试程序
2025-07-15 21:39:09 +08:00
bool isSame = true;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD>ۼ<EFBFBD>
source code
2025-06-08 10:46:41 +08:00
for (int i = 0; i < zHistSize; i++)
{
int sumValue = 0;
for (int j = i - 5; j <= i + 5; j++)
{
if ((j >= 0) && (j < zHistSize))
sumValue += zHist[j];
}
zSumHist[i] = sumValue;
添加了拆包算法中编织袋正反识别和朝向识别算法，以及相应的测试程序
2025-07-15 21:39:09 +08:00
if (i > 0)
{
if (sumValue != zSumHist[i - 1])
isSame = false;
}
}
if(true == isSame)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۼӣ<DBBC><D3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۼӣ<DBBC><D3A3>ۼ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>ȣ<EFBFBD>
for (int i = 0; i < zHistSize; i++)
zSumHist[i] = zHist[i];
source code
2025-06-08 10:46:41 +08:00
}
//Ѱ<>Ҽ<EFBFBD>ֵ
int _state = 0;
int pre_i = -1;
int sEdgePtIdx = -1;
int eEdgePtIdx = -1;
int pre_data = -1;
std::vector< SSG_intPair> pkTop;
std::vector< SSG_intPair> pkBtm;
std::vector<int> pkBtmBackIndexing;
pkBtmBackIndexing.resize(zHistSize);
for (int i = 0; i < zHistSize; i++)
pkBtmBackIndexing[i] = -1;
for (int i = 0; i < zHistSize; i++)
{
int curr_data = zSumHist[i];
if (pre_data < 0)
{
sEdgePtIdx = i;
eEdgePtIdx = i;
pre_data = curr_data;
pre_i = i;
continue;
}
eEdgePtIdx = i;
double z_diff = curr_data - pre_data;
switch (_state)
{
case 0: //<2F><>̬
if (z_diff < 0) //<2F>½<EFBFBD>
{
_state = 2;
}
else if (z_diff > 0) //<2F><><EFBFBD><EFBFBD>
{
_state = 1;
}
break;
case 1: //<2F><><EFBFBD><EFBFBD>
if (z_diff < 0) //<2F>½<EFBFBD>
{
pkTop.push_back({pre_i, pre_data});
_state = 2;
}
添加了拆包算法中编织袋正反识别和朝向识别算法，以及相应的测试程序
2025-07-15 21:39:09 +08:00
else if(i == (zHistSize-1))
pkTop.push_back({ i, curr_data });
source code
2025-06-08 10:46:41 +08:00
break;
case 2: //<2F>½<EFBFBD>
if (z_diff > 0) // <20><><EFBFBD><EFBFBD>
{
解决编译中的warning
2025-07-22 22:52:57 +08:00
int pkBtmIdx = (int)pkBtm.size();
source code
2025-06-08 10:46:41 +08:00
pkBtmBackIndexing[pre_i] = pkBtmIdx;
pkBtm.push_back({ pre_i, pre_data });
_state = 1;
}
添加了拆包算法中编织袋正反识别和朝向识别算法，以及相应的测试程序
2025-07-15 21:39:09 +08:00
else if (i == (zHistSize - 1))
{
解决编译中的warning
2025-07-22 22:52:57 +08:00
int pkBtmIdx = (int)pkBtm.size();
添加了拆包算法中编织袋正反识别和朝向识别算法，以及相应的测试程序
2025-07-15 21:39:09 +08:00
pkBtmBackIndexing[i] = pkBtmIdx;
pkBtm.push_back({ i, curr_data });
}
source code
2025-06-08 10:46:41 +08:00
break;
default:
_state = 0;
break;
}
pre_data = curr_data;
pre_i = i;
}
//Ѱ<>ҵ<EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܵ<EFBFBD><DCB5><EFBFBD>1/3<>ļ<EFBFBD>ֵ<EFBFBD><D6B5>
if (pkTop.size() < 1)
return planePara;
代码备份
2025-06-27 23:04:51 +08:00
int pntSizeTh = totalPntSize / 10;
source code
2025-06-08 10:46:41 +08:00
SSG_intPair* vldPeak = NULL;
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int i = 0, i_max = (int)pkTop.size(); i < i_max; i++)
source code
2025-06-08 10:46:41 +08:00
{
if (pkTop[i].data_1 > pntSizeTh)
{
vldPeak = &pkTop[i];
break;
}
}
if (NULL == vldPeak)
return planePara;
//Ѱ<>ҿ<EFBFBD>ʼ<EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD>λ<EFBFBD><CEBB>
//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0>
int preBtmIdx = -1;
for (int j = vldPeak->data_0 - 1; j >= 0; j--)
{
if (pkBtmBackIndexing[j] >= 0)
{
int idx = pkBtmBackIndexing[j];
if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2))
{
preBtmIdx = j;
break;
}
}
}
int postBtmIdx = -1;
for (int j = vldPeak->data_0 + 1; j <zHistSize; j++)
{
if (pkBtmBackIndexing[j] >= 0)
{
int idx = pkBtmBackIndexing[j];
if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2))
{
postBtmIdx = j;
break;
}
}
}
SVzNLRangeD topZRange;
if (preBtmIdx < 0)
topZRange.min = zRange.min;
else
topZRange.min = (float)preBtmIdx + zRange.min;
if (postBtmIdx < 0)
topZRange.max = zRange.max;
else
topZRange.max = (float)postBtmIdx + zRange.min;
//ȡ<><C8A1><EFBFBD><EFBFBD>
std::vector<cv::Point3f> Points3ds;
for (int line = 0; line < lineNum; line++)
{
for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
{
SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
if (pt3D->pt3D.z < 1e-4)
continue;
if ((pt3D->pt3D.z >= topZRange.min) && (pt3D->pt3D.z <= topZRange.max))
{
cv::Point3f a_vldPt;
解决编译中的warning
2025-07-22 22:52:57 +08:00
a_vldPt.x = (float)pt3D->pt3D.x;
a_vldPt.y = (float)pt3D->pt3D.y;
a_vldPt.z = (float)pt3D->pt3D.z;
source code
2025-06-08 10:46:41 +08:00
Points3ds.push_back(a_vldPt);
}
}
}
//ƽ<><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<double> planceFunc;
vzCaculateLaserPlane(Points3ds, planceFunc);
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
#if 1 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ת<EFBFBD><D7AA>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]);
Vector3 b = Vector3(0, 0, -1.0);
Quaternion quanPara = rotationBetweenVectors(a, b);
RotationMatrix rMatrix;
quaternionToMatrix(quanPara, rMatrix.data);
//<2F><><EFBFBD>㷴<EFBFBD><E3B7B4><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
Quaternion invQuanPara = rotationBetweenVectors(b, a);
RotationMatrix invMatrix;
quaternionToMatrix(invQuanPara, invMatrix.data);
#else //<2F><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3>
eulerPra.roll = eulerPra.roll;
eulerPra.pitch = eulerPra.pitch;
eulerPra.yaw = eulerPra.yaw;
RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll);
#endif
planePara.planeCalib[0] = rMatrix.data[0][0];
planePara.planeCalib[1] = rMatrix.data[0][1];
planePara.planeCalib[2] = rMatrix.data[0][2];
planePara.planeCalib[3] = rMatrix.data[1][0];
planePara.planeCalib[4] = rMatrix.data[1][1];
planePara.planeCalib[5] = rMatrix.data[1][2];
planePara.planeCalib[6] = rMatrix.data[2][0];
planePara.planeCalib[7] = rMatrix.data[2][1];
planePara.planeCalib[8] = rMatrix.data[2][2];
planePara.invRMatrix[0] = invMatrix.data[0][0];
planePara.invRMatrix[1] = invMatrix.data[0][1];
planePara.invRMatrix[2] = invMatrix.data[0][2];
planePara.invRMatrix[3] = invMatrix.data[1][0];
planePara.invRMatrix[4] = invMatrix.data[1][1];
planePara.invRMatrix[5] = invMatrix.data[1][2];
planePara.invRMatrix[6] = invMatrix.data[2][0];
planePara.invRMatrix[7] = invMatrix.data[2][1];
planePara.invRMatrix[8] = invMatrix.data[2][2];
#if 0 //test: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij˻<C4B3><CBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>λ<EFBFBD><CEBB>
double testMatrix[3][3];
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
testMatrix[i][j] = 0;
for (int m = 0; m < 3; m++)
testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j];
}
}
#endif
//<2F><><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD>ת<EFBFBD><D7AA>
SVzNLRangeD calibZRange = { 0, -1 };
topZRange = { 0, -1 };
for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++)
{
//z
if (topZRange.max < topZRange.min)
{
topZRange.min = Points3ds[i].z;
topZRange.max = Points3ds[i].z;
}
else
{
if (topZRange.min > Points3ds[i].z)
topZRange.min = Points3ds[i].z;
if (topZRange.max < Points3ds[i].z)
topZRange.max = Points3ds[i].z;
}
cv::Point3f a_calibPt;
a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]);
a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]);
a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]);
//z
if (calibZRange.max < calibZRange.min)
{
calibZRange.min = a_calibPt.z;
calibZRange.max = a_calibPt.z;
}
else
{
if (calibZRange.min > a_calibPt.z)
calibZRange.min = a_calibPt.z;
if (calibZRange.max < a_calibPt.z)
calibZRange.max = a_calibPt.z;
}
}
planePara.planeHeight = calibZRange.min;
return planePara;
}
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>п<EFBFBD><D0BF><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>Ͳο<CDB2><CEBF><EFBFBD>ƽƽ<C6BD>棬<EFBFBD><E6A3AC><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>ƽ
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>淨<EFBFBD><E6B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
SSG_planeCalibPara sg_getPlaneCalibPara2(
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
//<2F><><EFBFBD>ó<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
double initCalib[9] = {
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0 };
SSG_planeCalibPara planePara;
for (int i = 0; i < 9; i++)
planePara.planeCalib[i] = initCalib[i];
planePara.planeHeight = -1.0;
int lineNum = (int)scanLines.size();
//ͳ<><CDB3>z<EFBFBD><7A>Χ
SVzNLRangeD zRange = { 0, -1 }; //< Z<><5A>Χ
for (int line = 0; line < lineNum; line++)
{
int nPositionCnt = (int)scanLines[line].size();
for (int i = 0; i < nPositionCnt; i++)
{
SVzNL3DPosition* pt3D = &scanLines[line][i];
if (pt3D->pt3D.z < 1e-4)
continue;
//z
if (zRange.max < zRange.min)
{
zRange.min = pt3D->pt3D.z;
zRange.max = pt3D->pt3D.z;
}
else
{
if (zRange.min > pt3D->pt3D.z)
zRange.min = pt3D->pt3D.z;
if (zRange.max < pt3D->pt3D.z)
zRange.max = pt3D->pt3D.z;
}
}
}
//<2F><>Z<EFBFBD><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͳ<EFBFBD>ƣ<EFBFBD>ȡ<EFBFBD><C8A1>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ֵ
//<2F><>mmΪ<6D><CEAA>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int zHistSize = (int)(zRange.max - zRange.min) + 1;
if (zHistSize == 0)
return planePara;
std::vector<int> zHist;
zHist.resize(zHistSize);
int totalPntSize = 0;
for (int line = 0; line < lineNum; line++)
{
int nPositionCnt = (int)scanLines[line].size();
for (int i = 0; i < nPositionCnt; i++)
{
SVzNL3DPosition* pt3D = &scanLines[line][i];
if (pt3D->pt3D.z < 1e-4)
continue;
totalPntSize++;
int histPos = (int)(pt3D->pt3D.z - zRange.min);
zHist[histPos] ++;
}
}
std::vector<int> zSumHist;
zSumHist.resize(zHistSize);
bool isSame = true;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD>ۼ<EFBFBD>
for (int i = 0; i < zHistSize; i++)
{
int sumValue = 0;
for (int j = i - 5; j <= i + 5; j++)
{
if ((j >= 0) && (j < zHistSize))
sumValue += zHist[j];
}
zSumHist[i] = sumValue;
if (i > 0)
{
if (sumValue != zSumHist[i - 1])
isSame = false;
}
}
if (true == isSame)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۼӣ<DBBC><D3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۼӣ<DBBC><D3A3>ۼ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>ȣ<EFBFBD>
for (int i = 0; i < zHistSize; i++)
zSumHist[i] = zHist[i];
}
//Ѱ<>Ҽ<EFBFBD>ֵ
int _state = 0;
int pre_i = -1;
int sEdgePtIdx = -1;
int eEdgePtIdx = -1;
int pre_data = -1;
std::vector< SSG_intPair> pkTop;
std::vector< SSG_intPair> pkBtm;
std::vector<int> pkBtmBackIndexing;
pkBtmBackIndexing.resize(zHistSize);
for (int i = 0; i < zHistSize; i++)
pkBtmBackIndexing[i] = -1;
for (int i = 0; i < zHistSize; i++)
{
int curr_data = zSumHist[i];
if (pre_data < 0)
{
sEdgePtIdx = i;
eEdgePtIdx = i;
pre_data = curr_data;
pre_i = i;
continue;
}
eEdgePtIdx = i;
double z_diff = curr_data - pre_data;
switch (_state)
{
case 0: //<2F><>̬
if (z_diff < 0) //<2F>½<EFBFBD>
{
_state = 2;
}
else if (z_diff > 0) //<2F><><EFBFBD><EFBFBD>
{
_state = 1;
}
break;
case 1: //<2F><><EFBFBD><EFBFBD>
if (z_diff < 0) //<2F>½<EFBFBD>
{
pkTop.push_back({ pre_i, pre_data });
_state = 2;
}
else if (i == (zHistSize - 1))
pkTop.push_back({ i, curr_data });
break;
case 2: //<2F>½<EFBFBD>
if (z_diff > 0) // <20><><EFBFBD><EFBFBD>
{
int pkBtmIdx = (int)pkBtm.size();
pkBtmBackIndexing[pre_i] = pkBtmIdx;
pkBtm.push_back({ pre_i, pre_data });
_state = 1;
}
else if (i == (zHistSize - 1))
{
int pkBtmIdx = (int)pkBtm.size();
pkBtmBackIndexing[i] = pkBtmIdx;
pkBtm.push_back({ i, curr_data });
}
break;
default:
_state = 0;
break;
}
pre_data = curr_data;
pre_i = i;
}
//Ѱ<>ҵ<EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܵ<EFBFBD><DCB5><EFBFBD>1/3<>ļ<EFBFBD>ֵ<EFBFBD><D6B5>
if (pkTop.size() < 1)
return planePara;
int pntSizeTh = totalPntSize / 10;
SSG_intPair* vldPeak = NULL;
for (int i = 0, i_max = (int)pkTop.size(); i < i_max; i++)
{
if (pkTop[i].data_1 > pntSizeTh)
{
vldPeak = &pkTop[i];
break;
}
}
if (NULL == vldPeak)
return planePara;
//Ѱ<>ҿ<EFBFBD>ʼ<EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD>λ<EFBFBD><CEBB>
//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0>
int preBtmIdx = -1;
for (int j = vldPeak->data_0 - 1; j >= 0; j--)
{
if (pkBtmBackIndexing[j] >= 0)
{
int idx = pkBtmBackIndexing[j];
if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2))
{
preBtmIdx = j;
break;
}
}
}
int postBtmIdx = -1;
for (int j = vldPeak->data_0 + 1; j < zHistSize; j++)
{
if (pkBtmBackIndexing[j] >= 0)
{
int idx = pkBtmBackIndexing[j];
if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2))
{
postBtmIdx = j;
break;
}
}
}
SVzNLRangeD topZRange;
if (preBtmIdx < 0)
topZRange.min = zRange.min;
else
topZRange.min = (float)preBtmIdx + zRange.min;
if (postBtmIdx < 0)
topZRange.max = zRange.max;
else
topZRange.max = (float)postBtmIdx + zRange.min;
//ȡ<><C8A1><EFBFBD><EFBFBD>
std::vector<cv::Point3f> Points3ds;
for (int line = 0; line < lineNum; line++)
{
int nPositionCnt = (int)scanLines[line].size();
for (int i = 0; i < nPositionCnt; i++)
{
SVzNL3DPosition* pt3D = &scanLines[line][i];
if (pt3D->pt3D.z < 1e-4)
continue;
if ((pt3D->pt3D.z >= topZRange.min) && (pt3D->pt3D.z <= topZRange.max))
{
cv::Point3f a_vldPt;
a_vldPt.x = (float)pt3D->pt3D.x;
a_vldPt.y = (float)pt3D->pt3D.y;
a_vldPt.z = (float)pt3D->pt3D.z;
Points3ds.push_back(a_vldPt);
}
}
}
//ƽ<><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<double> planceFunc;
vzCaculateLaserPlane(Points3ds, planceFunc);
source code
2025-06-08 10:46:41 +08:00
#if 1 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ת<EFBFBD><D7AA>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]);
Vector3 b = Vector3(0, 0, -1.0);
Quaternion quanPara = rotationBetweenVectors(a, b);
RotationMatrix rMatrix;
quaternionToMatrix(quanPara, rMatrix.data);
//<2F><><EFBFBD>㷴<EFBFBD><E3B7B4><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
Quaternion invQuanPara = rotationBetweenVectors(b, a);
RotationMatrix invMatrix;
quaternionToMatrix(invQuanPara, invMatrix.data);
#else //<2F><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3>
eulerPra.roll = eulerPra.roll;
eulerPra.pitch = eulerPra.pitch;
eulerPra.yaw = eulerPra.yaw;
RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll);
#endif
planePara.planeCalib[0] = rMatrix.data[0][0];
planePara.planeCalib[1] = rMatrix.data[0][1];
planePara.planeCalib[2] = rMatrix.data[0][2];
planePara.planeCalib[3] = rMatrix.data[1][0];
planePara.planeCalib[4] = rMatrix.data[1][1];
planePara.planeCalib[5] = rMatrix.data[1][2];
planePara.planeCalib[6] = rMatrix.data[2][0];
planePara.planeCalib[7] = rMatrix.data[2][1];
planePara.planeCalib[8] = rMatrix.data[2][2];
planePara.invRMatrix[0] = invMatrix.data[0][0];
planePara.invRMatrix[1] = invMatrix.data[0][1];
planePara.invRMatrix[2] = invMatrix.data[0][2];
planePara.invRMatrix[3] = invMatrix.data[1][0];
planePara.invRMatrix[4] = invMatrix.data[1][1];
planePara.invRMatrix[5] = invMatrix.data[1][2];
planePara.invRMatrix[6] = invMatrix.data[2][0];
planePara.invRMatrix[7] = invMatrix.data[2][1];
planePara.invRMatrix[8] = invMatrix.data[2][2];
#if 0 //test: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij˻<C4B3><CBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>λ<EFBFBD><CEBB>
double testMatrix[3][3];
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
testMatrix[i][j] = 0;
for (int m = 0; m < 3; m++)
testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j];
}
}
#endif
//<2F><><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD>ת<EFBFBD><D7AA>
SVzNLRangeD calibZRange = { 0, -1 };
topZRange = { 0, -1 };
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
double sumMeanZ = 0;
int sumSize = 0;
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++)
source code
2025-06-08 10:46:41 +08:00
{
//z
if (topZRange.max < topZRange.min)
{
topZRange.min = Points3ds[i].z;
topZRange.max = Points3ds[i].z;
}
else
{
if (topZRange.min > Points3ds[i].z)
topZRange.min = Points3ds[i].z;
if (topZRange.max < Points3ds[i].z)
topZRange.max = Points3ds[i].z;
}
cv::Point3f a_calibPt;
解决编译中的warning
2025-07-22 22:52:57 +08:00
a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]);
a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]);
a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]);
source code
2025-06-08 10:46:41 +08:00
//z
if (calibZRange.max < calibZRange.min)
{
calibZRange.min = a_calibPt.z;
calibZRange.max = a_calibPt.z;
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
sumMeanZ += a_calibPt.z;
sumSize++;
source code
2025-06-08 10:46:41 +08:00
}
else
{
if (calibZRange.min > a_calibPt.z)
calibZRange.min = a_calibPt.z;
if (calibZRange.max < a_calibPt.z)
calibZRange.max = a_calibPt.z;
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
sumMeanZ += a_calibPt.z;
sumSize++;
source code
2025-06-08 10:46:41 +08:00
}
}
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
if (sumSize > 0)
sumMeanZ = sumMeanZ / (double)sumSize;
planePara.planeHeight = sumMeanZ; // calibZRange.min;
source code
2025-06-08 10:46:41 +08:00
return planePara;
}
拆包应用加强了旋转旋转的包的检测功能。这样功能由一个开关控制
2025-10-06 14:21:33 +08:00
SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
SVzNL3DRangeD roi)
{
//<2F><><EFBFBD>ó<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
double initCalib[9] = {
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0 };
SSG_planeCalibPara planePara;
for (int i = 0; i < 9; i++)
planePara.planeCalib[i] = initCalib[i];
planePara.planeHeight = -1.0;
int lineNum = scanLines.size();
//ȡ<><C8A1><EFBFBD><EFBFBD>
std::vector<cv::Point3f> Points3ds;
for (int line = 0; line < lineNum; line++)
{
int nPositionCnt = (int)scanLines[line].size();
for (int i = 0; i < nPositionCnt; i++)
{
SVzNL3DPosition* pt3D = &scanLines[line][i];
if (pt3D->pt3D.z < 1e-4)
continue;
bool isValid = false;
if ((pt3D->pt3D.x >= roi.xRange.min) && (pt3D->pt3D.x <= roi.xRange.max) &&
(pt3D->pt3D.y >= roi.yRange.min) && (pt3D->pt3D.y <= roi.yRange.max) &&
(pt3D->pt3D.z >= roi.zRange.min) && (pt3D->pt3D.y <= roi.zRange.max))
{
cv::Point3f a_vldPt;
a_vldPt.x = (float)pt3D->pt3D.x;
a_vldPt.y = (float)pt3D->pt3D.y;
a_vldPt.z = (float)pt3D->pt3D.z;
Points3ds.push_back(a_vldPt);
}
}
}
//ƽ<><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<double> planceFunc;
vzCaculateLaserPlane(Points3ds, planceFunc);
#if 1 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ת<EFBFBD><D7AA>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]);
Vector3 b = Vector3(0, 0, -1.0);
Quaternion quanPara = rotationBetweenVectors(a, b);
RotationMatrix rMatrix;
quaternionToMatrix(quanPara, rMatrix.data);
//<2F><><EFBFBD>㷴<EFBFBD><E3B7B4><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
Quaternion invQuanPara = rotationBetweenVectors(b, a);
RotationMatrix invMatrix;
quaternionToMatrix(invQuanPara, invMatrix.data);
#else //<2F><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3>
eulerPra.roll = eulerPra.roll;
eulerPra.pitch = eulerPra.pitch;
eulerPra.yaw = eulerPra.yaw;
RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll);
#endif
planePara.planeCalib[0] = rMatrix.data[0][0];
planePara.planeCalib[1] = rMatrix.data[0][1];
planePara.planeCalib[2] = rMatrix.data[0][2];
planePara.planeCalib[3] = rMatrix.data[1][0];
planePara.planeCalib[4] = rMatrix.data[1][1];
planePara.planeCalib[5] = rMatrix.data[1][2];
planePara.planeCalib[6] = rMatrix.data[2][0];
planePara.planeCalib[7] = rMatrix.data[2][1];
planePara.planeCalib[8] = rMatrix.data[2][2];
planePara.invRMatrix[0] = invMatrix.data[0][0];
planePara.invRMatrix[1] = invMatrix.data[0][1];
planePara.invRMatrix[2] = invMatrix.data[0][2];
planePara.invRMatrix[3] = invMatrix.data[1][0];
planePara.invRMatrix[4] = invMatrix.data[1][1];
planePara.invRMatrix[5] = invMatrix.data[1][2];
planePara.invRMatrix[6] = invMatrix.data[2][0];
planePara.invRMatrix[7] = invMatrix.data[2][1];
planePara.invRMatrix[8] = invMatrix.data[2][2];
#if 0 //test: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij˻<C4B3><CBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>λ<EFBFBD><CEBB>
double testMatrix[3][3];
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
testMatrix[i][j] = 0;
for (int m = 0; m < 3; m++)
testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j];
}
}
#endif
//<2F><><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD>ת<EFBFBD><D7AA>
SVzNLRangeD calibZRange = { 0, -1 };
for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++)
{
cv::Point3f a_calibPt;
a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]);
a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]);
a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]);
//z
if (calibZRange.max < calibZRange.min)
{
calibZRange.min = a_calibPt.z;
calibZRange.max = a_calibPt.z;
}
else
{
if (calibZRange.min > a_calibPt.z)
calibZRange.min = a_calibPt.z;
if (calibZRange.max < a_calibPt.z)
calibZRange.max = a_calibPt.z;
}
}
planePara.planeHeight = calibZRange.min;
return planePara;
}
source code
2025-06-08 10:46:41 +08:00
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ROI<4F><49><EFBFBD>ڵĵ<DAB5><C4B5><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>淨<EFBFBD><E6B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
SSG_planeCalibPara sg_getPlaneCalibPara_ROIs(
SVzNL3DLaserLine* laser3DPoints,
int lineNum,
std::vector<SVzNL3DRangeD>& ROIs)
{
//<2F><><EFBFBD>ó<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
double initCalib[9] = {
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0 };
SSG_planeCalibPara planePara;
for (int i = 0; i < 9; i++)
planePara.planeCalib[i] = initCalib[i];
planePara.planeHeight = -1.0;
//ȡ<><C8A1><EFBFBD><EFBFBD>
std::vector<cv::Point3f> Points3ds;
for (int line = 0; line < lineNum; line++)
{
for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
{
SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
if (pt3D->pt3D.z < 1e-4)
continue;
bool isValid = false;
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int m = 0, m_max = (int)ROIs.size(); m < m_max; m++)
source code
2025-06-08 10:46:41 +08:00
{
if ((pt3D->pt3D.x >= ROIs[m].xRange.min) && (pt3D->pt3D.x <= ROIs[m].xRange.max) &&
(pt3D->pt3D.y >= ROIs[m].yRange.min) && (pt3D->pt3D.y <= ROIs[m].yRange.max) &&
(pt3D->pt3D.z >= ROIs[m].zRange.min) && (pt3D->pt3D.y <= ROIs[m].zRange.max))
{
isValid = true;
break;
}
}
if (false == isValid)
continue;
cv::Point3f a_vldPt;
解决编译中的warning
2025-07-22 22:52:57 +08:00
a_vldPt.x = (float)pt3D->pt3D.x;
a_vldPt.y = (float)pt3D->pt3D.y;
a_vldPt.z = (float)pt3D->pt3D.z;
source code
2025-06-08 10:46:41 +08:00
Points3ds.push_back(a_vldPt);
}
}
//ƽ<><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<double> planceFunc;
vzCaculateLaserPlane(Points3ds, planceFunc);
#if 1 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ת<EFBFBD><D7AA>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]);
Vector3 b = Vector3(0, 0, -1.0);
Quaternion quanPara = rotationBetweenVectors(a, b);
RotationMatrix rMatrix;
quaternionToMatrix(quanPara, rMatrix.data);
//<2F><><EFBFBD>㷴<EFBFBD><E3B7B4><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
Quaternion invQuanPara = rotationBetweenVectors(b, a);
RotationMatrix invMatrix;
quaternionToMatrix(invQuanPara, invMatrix.data);
#else //<2F><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3>
eulerPra.roll = eulerPra.roll;
eulerPra.pitch = eulerPra.pitch;
eulerPra.yaw = eulerPra.yaw;
RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll);
#endif
planePara.planeCalib[0] = rMatrix.data[0][0];
planePara.planeCalib[1] = rMatrix.data[0][1];
planePara.planeCalib[2] = rMatrix.data[0][2];
planePara.planeCalib[3] = rMatrix.data[1][0];
planePara.planeCalib[4] = rMatrix.data[1][1];
planePara.planeCalib[5] = rMatrix.data[1][2];
planePara.planeCalib[6] = rMatrix.data[2][0];
planePara.planeCalib[7] = rMatrix.data[2][1];
planePara.planeCalib[8] = rMatrix.data[2][2];
planePara.invRMatrix[0] = invMatrix.data[0][0];
planePara.invRMatrix[1] = invMatrix.data[0][1];
planePara.invRMatrix[2] = invMatrix.data[0][2];
planePara.invRMatrix[3] = invMatrix.data[1][0];
planePara.invRMatrix[4] = invMatrix.data[1][1];
planePara.invRMatrix[5] = invMatrix.data[1][2];
planePara.invRMatrix[6] = invMatrix.data[2][0];
planePara.invRMatrix[7] = invMatrix.data[2][1];
planePara.invRMatrix[8] = invMatrix.data[2][2];
#if 0 //test: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij˻<C4B3><CBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>λ<EFBFBD><CEBB>
double testMatrix[3][3];
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
testMatrix[i][j] = 0;
for (int m = 0; m < 3; m++)
testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j];
}
}
#endif
//<2F><><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD>ת<EFBFBD><D7AA>
SVzNLRangeD calibZRange = { 0, -1 };
解决编译中的warning
2025-07-22 22:52:57 +08:00
for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++)
source code
2025-06-08 10:46:41 +08:00
{
cv::Point3f a_calibPt;
解决编译中的warning
2025-07-22 22:52:57 +08:00
a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]);
a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]);
a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]);
source code
2025-06-08 10:46:41 +08:00
//z
if (calibZRange.max < calibZRange.min)
{
calibZRange.min = a_calibPt.z;
calibZRange.max = a_calibPt.z;
}
else
{
if (calibZRange.min > a_calibPt.z)
calibZRange.min = a_calibPt.z;
if (calibZRange.max < a_calibPt.z)
calibZRange.max = a_calibPt.z;
}
}
planePara.planeHeight = calibZRange.min;
return planePara;
}
// <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>ǣ<EFBFBD>Z-Y-X˳<58><CBB3><EFBFBD><EFBFBD>
SSG_EulerAngles rotationMatrixToEulerZYX(const double R[3][3]) {
SSG_EulerAngles angles;
// <20><><EFBFBD>㸩<EFBFBD><E3B8A9><EFBFBD>ǣ<EFBFBD>pitch<63><68><EFBFBD><EFBFBD>
angles.pitch = asin(-R[2][0]); // asin<69><6E><EFBFBD>ػ<EFBFBD><D8BB><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>cos<6F>Ƚӽ<C8BD>0<EFBFBD><30>
const double epsilon = 1e-6;
if (abs(cos(angles.pitch)) > epsilon) {
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>yaw<61><77>roll
angles.yaw = atan2(R[1][0], R[0][0]);
angles.roll = atan2(R[2][1], R[2][2]);
}
else {
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC>roll=0<><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>yaw
angles.roll = 0.0;
angles.yaw = atan2(-R[0][1], R[1][1]);
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD>Ƕ<EFBFBD>
const double rad2deg = 180.0 / M_PI;
angles.yaw *= rad2deg;
angles.pitch *= rad2deg;
angles.roll *= rad2deg;
return angles;
}
// <20><>ŷ<EFBFBD><C5B7><EFBFBD>Ǽ<EFBFBD><C7BC><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Z-Y-X˳<58><CBB3><EFBFBD><EFBFBD>
void eulerToRotationMatrixZYX(const SSG_EulerAngles& angles, double R[3][3]) {
// <20><><EFBFBD>Ƕ<EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
const double deg2rad = M_PI / 180.0;
const double yaw = angles.yaw * deg2rad;
const double pitch = angles.pitch * deg2rad;
const double roll = angles.roll * deg2rad;
// Ԥ<><D4A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǻ<EFBFBD><C7BA><EFBFBD>ֵ
const double cy = cos(yaw), sy = sin(yaw);
const double cp = cos(pitch), sp = sin(pitch);
const double cr = cos(roll), sr = sin(roll);
#if 0
// <20><>Z<EFBFBD><5A><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
double Rz[3][3] = {
{cy, -sy, 0},
{sy, cy, 0},
{0, 0, 1}
};
// <20><>Y<EFBFBD><59><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
double Ry[3][3] = {
{cp, 0, sp},
{0, 1, 0},
{-sp, 0, cp}
};
// <20><>X<EFBFBD><58><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
double Rx[3][3] = {
{1, 0, 0},
{0, cr, -sr},
{0, sr, cr}
};
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>R = Rz * Ry * Rx
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
// <20>ȼ<EFBFBD><C8BC><EFBFBD> Rz * Ry
double temp[3][3] = { 0 };
for (int k = 0; k < 3; ++k) {
temp[i][j] += Rz[i][k] * Ry[k][j];
}
// <20><><EFBFBD><EFBFBD> Rx <20><><EFBFBD><EFBFBD>
R[i][j] = 0;
for (int k = 0; k < 3; ++k) {
R[i][j] += temp[i][k] * Rx[k][j];
}
}
}
#endif
// <20>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>Ӽ<EFBFBD><D3BC>㹫ʽ<E3B9AB><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
R[0][0] = cy * cp;
R[0][1] = cy * sp * sr - sy * cr;
R[0][2] = cy * sp * cr + sy * sr;
R[1][0] = sy * cp;
R[1][1] = sy * sp * sr + cy * cr;
R[1][2] = sy * sp * cr - cy * sr;
R[2][0] = -sp;
R[2][1] = cp * sr;
R[2][2] = cp * cr;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD>3D<33><44><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD><EFBFBD><EFBFBD>ת(û<><C3BB>ƽ<EFBFBD><C6BD>)<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>״̬
///camPoseRΪ3x3<78><33><EFBFBD><EFBFBD>
void lineDataRT(SVzNL3DLaserLine* a_line, const double* camPoseR, double groundH)
{
for (int i = 0; i < a_line->nPositionCnt; i++)
{
SVzNL3DPoint a_pt = a_line->p3DPosition[i].pt3D;
if (a_pt.z < 1e-4)
continue;
double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2];
double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5];
double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8];
if ((groundH > 0) && (z > groundH)) //ȥ<><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
z = 0;
a_pt.x = x;
a_pt.y = y;
a_pt.z = z;
a_line->p3DPosition[i].pt3D = a_pt;
}
return;
添加了拆包算法中编织袋正反识别和朝向识别算法，以及相应的测试程序
2025-07-15 21:39:09 +08:00
}
void lineDataRT_vector(std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, double groundH)
{
for (int i = 0; i < a_line.size(); i++)
{
SVzNL3DPoint a_pt = a_line[i].pt3D;
if (a_pt.z < 1e-4)
continue;
double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2];
double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5];
double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8];
if ((groundH > 0) && (z > groundH)) //ȥ<><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
z = 0;
a_pt.x = x;
a_pt.y = y;
a_pt.z = z;
a_line[i].pt3D = a_pt;
}
return;
}
void lineDataRT_RGBD(SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, double groundH)
{
for (int i = 0; i < a_line->nPointCnt; i++)
{
SVzNLPointXYZRGBA a_pt = a_line->p3DPoint[i];
if (a_pt.z < 1e-4)
continue;
double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2];
double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5];
double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8];
if ((groundH > 0) && (z > groundH)) //ȥ<><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
z = 0;
解决编译中的warning
2025-07-22 22:52:57 +08:00
a_pt.x = (float)x;
a_pt.y = (float)y;
a_pt.z = (float)z;
添加了拆包算法中编织袋正反识别和朝向识别算法，以及相应的测试程序
2025-07-15 21:39:09 +08:00
a_line->p3DPoint[i] = a_pt;
}
return;
source code
2025-06-08 10:46:41 +08:00
}
Reference in New Issue Copy Permalink