2166 lines
54 KiB
C++
2166 lines
54 KiB
C++
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include <vector>
|
||
#include <corecrt_math_defines.h>
|
||
#include <cmath>
|
||
#include <unordered_map>
|
||
|
||
//¼ÆËãɨÃèROI
|
||
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;
|
||
}
|
||
|
||
//¼ÆËãɨÃèROI: vecotr¸ñʽ
|
||
SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
SVzNL3DRangeD roi;
|
||
roi.xRange = { 0, -1 };
|
||
roi.yRange = { 0, -1 };
|
||
roi.zRange = { 0, -1 };
|
||
|
||
int lineNum = (int)scanLines.size();
|
||
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 (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;
|
||
}
|
||
|
||
//¼ÆËãµãÔƵÄROIºÍscale: vecotr¸ñʽ
|
||
SWD_pointCloudPara wd_getPointCloudPara(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
SWD_pointCloudPara para;
|
||
para.xRange = { 0, -1 };
|
||
para.yRange = { 0, -1 };
|
||
para.zRange = { 0, -1 };
|
||
para.scale_x = -1; //³õʼֵ
|
||
para.scale_y = -1;
|
||
int lineNum = (int)scanLines.size();
|
||
double x_scale = 0;
|
||
int x_scale_cnt = 0;
|
||
double y_scale = 0;
|
||
double y_scale_cnt = 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;
|
||
|
||
if (i > 0)
|
||
{
|
||
if (scanLines[line][i - 1].pt3D.z > 1e-4)
|
||
{
|
||
y_scale += abs(pt3D->pt3D.y - scanLines[line][i - 1].pt3D.y);
|
||
y_scale_cnt++;
|
||
}
|
||
}
|
||
if (line > 0)
|
||
{
|
||
if (scanLines[line - 1][i].pt3D.z > 1e-4)
|
||
{
|
||
x_scale += abs(pt3D->pt3D.x - scanLines[line-1][i].pt3D.x);
|
||
x_scale_cnt++;
|
||
}
|
||
}
|
||
|
||
if (para.xRange.max < para.xRange.min)
|
||
{
|
||
para.xRange.min = pt3D->pt3D.x;
|
||
para.xRange.max = pt3D->pt3D.x;
|
||
}
|
||
else
|
||
{
|
||
if (para.xRange.min > pt3D->pt3D.x)
|
||
para.xRange.min = pt3D->pt3D.x;
|
||
if (para.xRange.max < pt3D->pt3D.x)
|
||
para.xRange.max = pt3D->pt3D.x;
|
||
}
|
||
//y
|
||
if (para.yRange.max < para.yRange.min)
|
||
{
|
||
para.yRange.min = pt3D->pt3D.y;
|
||
para.yRange.max = pt3D->pt3D.y;
|
||
}
|
||
else
|
||
{
|
||
if (para.yRange.min > pt3D->pt3D.y)
|
||
para.yRange.min = pt3D->pt3D.y;
|
||
if (para.yRange.max < pt3D->pt3D.y)
|
||
para.yRange.max = pt3D->pt3D.y;
|
||
}
|
||
//z
|
||
if (para.zRange.max < para.zRange.min)
|
||
{
|
||
para.zRange.min = pt3D->pt3D.z;
|
||
para.zRange.max = pt3D->pt3D.z;
|
||
}
|
||
else
|
||
{
|
||
if (para.zRange.min > pt3D->pt3D.z)
|
||
para.zRange.min = pt3D->pt3D.z;
|
||
if (para.zRange.max < pt3D->pt3D.z)
|
||
para.zRange.max = pt3D->pt3D.z;
|
||
}
|
||
}
|
||
}
|
||
if (x_scale_cnt > 0)
|
||
para.scale_x = x_scale / (double)x_scale_cnt;
|
||
if (y_scale_cnt > 0)
|
||
para.scale_y = y_scale / (double)y_scale_cnt;
|
||
return para;
|
||
}
|
||
|
||
void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b)
|
||
{
|
||
//×îС¶þ³ËÄâºÏÖ±Ïß²ÎÊý
|
||
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µÄÀÛ¼ÓºÍ
|
||
y_sum += inliers[i].y; //yµÄÀÛ¼ÓºÍ
|
||
xx_sum += inliers[i].x * inliers[i].x; //xµÄƽ·½ÀÛ¼ÓºÍ
|
||
xy_sum += inliers[i].x * inliers[i].y; //x£¬yµÄÀÛ¼ÓºÍ
|
||
num++;
|
||
}
|
||
*_k = (num * xy_sum - x_sum * y_sum) / (num * xx_sum - x_sum * x_sum); //¸ù¾Ý¹«Ê½Çó½âk
|
||
*_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//¸ù¾Ý¹«Ê½Çó½âb
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
#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Ëã·¨
|
||
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Ëã·¨
|
||
void drawLine(
|
||
int x0,
|
||
int y0,
|
||
int x1,
|
||
int y1,
|
||
std::vector<SVzNL2DPoint>& pts)
|
||
{
|
||
// ¼ÆËãdxºÍdyµÄ¾ø¶ÔÖµ
|
||
int dx = abs(x1 - x0);
|
||
int dy = abs(y1 - y0);
|
||
|
||
// È·¶¨²½½ø·½Ïò
|
||
int sx = (x0 < x1) ? 1 : -1; // x·½Ïò²½½ø
|
||
int sy = (y0 < y1) ? 1 : -1; // y·½Ïò²½½ø
|
||
|
||
// ³õʼ»¯Îó²î±äÁ¿£¬½áºÏdxºÍdyµÄ·ûºÅ
|
||
int err = dx - dy;
|
||
|
||
while (true) {
|
||
SVzNL2DPoint a_pt = { x0, y0 };
|
||
pts.push_back(a_pt);
|
||
|
||
// µ½´ïÖÕµãʱÍ˳öÑ»·
|
||
if (x0 == x1 && y0 == y1) break;
|
||
|
||
int e2 = 2 * err; // µ±Ç°Îó²îµÄÁ½±¶
|
||
|
||
// ¸ù¾ÝÎó²î¾ö¶¨²½½ø·½Ïò
|
||
if (e2 > -dy) { // Îó²îÇãÏòÓÚx·½Ïò²½½ø
|
||
err -= dy;
|
||
x0 += sx;
|
||
}
|
||
if (e2 < dx) { // Îó²îÇãÏòÓÚy·½Ïò²½½ø
|
||
err += dx;
|
||
y0 += sy;
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Á½²½·¨±ê×¢
|
||
/// </summary>
|
||
/// <param name="bwImg"> Ä¿±êµãΪ¡°1¡±£¬ ¿Õ°×µãΪ¡°0¡±</param>
|
||
/// <param name="labImg"> ±ê×¢½á¹û¡£Ã¿¸öµãΪrgnID, ID´Ó2¿ªÊ¼ </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;
|
||
|
||
//¶þֵͼÏñÏñËØֵΪ0»ò1£¬ÎªÁ˲»³åÍ»£¬label´Ó2¿ªÊ¼
|
||
int label = 2;
|
||
std::vector<int> labelSet;
|
||
labelSet.push_back(0);
|
||
labelSet.push_back(1);
|
||
|
||
//µÚÒ»´ÎɨÃè
|
||
int* data_prev = (int*)labImg.data;
|
||
int* data_cur = (int*)(labImg.data + labImg.step);
|
||
int left, up;//Ö¸ÕëÖ¸ÏòµÄÏñËصãµÄ×ó·½µãºÍÉÏ·½µã
|
||
int neighborLabels[2];
|
||
for (int i = 1; i < rows; i++)// ºöÂÔµÚÒ»Ðк͵ÚÒ»ÁÐ,Æäʵ¿ÉÒÔ½«labImgµÄ¿í¸ß¼Ó1£¬È»ºóÔÚ³õʼ»¯Îª0¾Í¿ÉÒÔÁË
|
||
{
|
||
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)//µ±Ç°µã²»Îª1£¬É¨ÃèÏÂÒ»¸öµã
|
||
continue;
|
||
left = *(data_cur - 1);
|
||
up = *data_prev;
|
||
|
||
int count = 0;
|
||
for (int curLabel : {left, up})
|
||
{
|
||
if (curLabel > 1)
|
||
neighborLabels[count++] = curLabel;
|
||
}
|
||
if (!count)//¸³ÓèÒ»¸öеÄlabel
|
||
{
|
||
labelSet.push_back(label);
|
||
*data_cur = label;
|
||
label++;
|
||
continue;
|
||
}
|
||
//½«µ±Ç°µã±ê¼ÇÉèΪ×óµãºÍÉϵãlabelµÄ×îСֵ
|
||
int smallestLabel = neighborLabels[0];
|
||
if (count == 2 && neighborLabels[1] < smallestLabel)
|
||
smallestLabel = neighborLabels[1];
|
||
*data_cur = smallestLabel;
|
||
//ÉèÖÃµÈ¼Û±í£¬ÕâÀï¿ÉÄÜÓеãÄÑÀí½â
|
||
//×óµãÓпÉÄܱÈÉϵãС£¬Ò²ÓпÉÄܱÈÉϵã´ó£¬Á½ÖÖÇé¿ö¶¼Òª¿¼ÂÇ,ÀýÈç
|
||
//0 0 1 0 1 0 x x 2 x 3 x
|
||
//1 1 1 1 1 1 -> 4 4 2 2 2 2
|
||
//Òª½«labelSetÖÐ3µÄλÖÃÉèÖÃΪ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++;
|
||
}
|
||
//ÉÏÃæÒ»²½ÖÐ,ÓеÄlabelSetµÄλÖû¹Î´ÉèΪ×îСֵ£¬ÀýÈç
|
||
//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
|
||
//ÉÏÃæÕⲨ²Ù×÷ÖУ¬°ÑlabelSet[4]ÉèΪ2£¬µ«labelSet[5]ÈÔΪ4
|
||
//ÕâÀï¿ÉÒÔ½«labelSet[5]ÉèΪ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;
|
||
}
|
||
//µÚ¶þ´ÎɨÃ裬ÓÃlabelSet½øÐиüУ¬×îºóÒ»ÁÐ
|
||
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) //ÓÐЧlabel
|
||
{
|
||
//ͳ¼ÆRegionÐÅÏ¢
|
||
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()ºó£¬vectorÖÐÄÚ´æµ¥Ôª¿ÉÄܻᱻ¸Ä¶¯
|
||
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
|
||
// ²éÕÒº¯Êý£¨´ø·¾¶Ñ¹Ëõ£©
|
||
int find(int x, std::vector<int>& parent) {
|
||
if (parent[x] != x) {
|
||
parent[x] = find(parent[x], parent);
|
||
}
|
||
return parent[x];
|
||
}
|
||
|
||
// ºÏ²¢º¯Êý£¨°´ÖȺϲ¢µ½½ÏС¸ù£©
|
||
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 Á¬Í¨Óò±ê×¢º¯Êý
|
||
* @param image ÊäÈë¶þֵͼÏñ£¬0±íʾ±³¾°£¬·Ç0Ϊǰ¾°
|
||
* @param labels Êä³ö±êÇ©¾ØÕó
|
||
* @param connectivity Á¬Í¨ÐÔ£¨4»ò8£©
|
||
*/
|
||
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;
|
||
|
||
// ³õʼ»¯²¢²é¼¯£¨×î´ó¿ÉÄܱêÇ©ÊýΪÏñËØ×ÜÊý£©
|
||
int max_label = rows * cols;
|
||
std::vector<int> parent(max_label + 1);
|
||
for (int i = 0; i <= max_label; ++i) {
|
||
parent[i] = i;
|
||
}
|
||
|
||
//µÚÒ»´ÎɨÃè
|
||
int label_cnt = 2; // µ±Ç°×î´ó±êÇ©,¶þֵͼÏñÏñËØֵΪ0»ò1£¬ÎªÁ˲»³åÍ»£¬label´Ó2¿ªÊ¼
|
||
int* data_prev = (int*)labImg.data;
|
||
int* data_cur = (int*)(labImg.data + labImg.step);
|
||
// µÚÒ»±éɨÃ裺ÁÙʱ±êÇ©·ÖÅä
|
||
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)//µ±Ç°µã²»Îª1£¬É¨ÃèÏÂÒ»¸öµã
|
||
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));
|
||
}
|
||
};
|
||
|
||
// ¼ì²éÒÑ´¦ÀíÁÚÓò
|
||
if(up > 1)
|
||
add_neighbor(up); // ÉÏ
|
||
if( (left > 1) && (left != up))
|
||
add_neighbor(left); // ×ó
|
||
|
||
if (connectivity == 8)
|
||
{
|
||
if( (up_left > 1) && (up_left != up) && (up_left != left))
|
||
add_neighbor(up_left); // ×óÉÏ
|
||
if( (up_right > 1) && (up_right != up) && (up_right != left) && (up_right != up_left))
|
||
add_neighbor(up_right); // ÓÒÉÏ
|
||
}
|
||
|
||
if (neighbors.empty()) { // ÐÂÁ¬Í¨Óò
|
||
*data_cur = label_cnt++;
|
||
}
|
||
else { // ºÏ²¢ÁÚÓò
|
||
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);
|
||
// £¨¿ÉÑ¡£©ÖØÐÂÓ³ÉäΪÁ¬Ðø±êÇ©
|
||
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];
|
||
//ͳ¼ÆRegionÐÅÏ¢
|
||
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()ºó£¬vectorÖÐÄÚ´æµ¥Ôª¿ÉÄܻᱻ¸Ä¶¯
|
||
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
|
||
|
||
//¼ÆËãÃæ²ÎÊý: 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)
|
||
{
|
||
//×îС¶þ³Ë·¨ÄâºÏƽÃæ
|
||
//»ñÈ¡cv::MatµÄ×ø±êϵÒÔ×ÝÏòΪxÖᣬºáÏòΪyÖᣬ¶øcvPointµÈÔòÏà·´
|
||
//ϵÊý¾ØÕó
|
||
cv::Mat A = cv::Mat::zeros(3, 3, CV_64FC1);
|
||
//
|
||
cv::Mat B = cv::Mat::zeros(3, 1, CV_64FC1);
|
||
//½á¹û¾ØÕó
|
||
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;
|
||
A.at<double>(2, 2) = (double)((int)Points3ds.size());
|
||
B.at<double>(0, 0) = zixi;
|
||
B.at<double>(1, 0) = ziyi;
|
||
B.at<double>(2, 0) = zi;
|
||
//¼ÆËãƽÃæϵÊý
|
||
X = A.inv() * B;
|
||
//A
|
||
res.push_back(X.at<double>(0, 0));
|
||
//B
|
||
res.push_back(X.at<double>(1, 0));
|
||
//ZµÄϵÊýΪ-1
|
||
res.push_back(-1.0);
|
||
//C
|
||
res.push_back(X.at<double>(2, 0));
|
||
return;
|
||
}
|
||
|
||
// º¯Êý£º´ÓƽÃæ·¨ÏòÁ¿¼ÆËãÅ·À½Ç£¨ZYX˳Ðò£©
|
||
SSG_EulerAngles planeNormalToEuler(double A, double B, double C) {
|
||
SSG_EulerAngles angles = { 0, 0, 0 };
|
||
|
||
// 1. ¹éÒ»»¯·¨ÏòÁ¿
|
||
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. ¼ÆË㸩Ñö½Ç£¨ÈÆYÖᣩ
|
||
angles.pitch = std::asin(nx) * (180.0 / M_PI); // תΪ¶ÈÊý
|
||
|
||
// 3. ¼ÆËãRoll£¨ÈÆXÖᣩ
|
||
const double cos_pitch = std::sqrt(1 - nx * nx); // µÈ¼ÛÓÚcos(pitch)
|
||
if (cos_pitch > 1e-7) {
|
||
// µ±cos_pitch·ÇÁãʱ£¬ÓÃatan2¼ÆËãRoll
|
||
angles.roll = std::asin(-ny/ cos_pitch) * (180.0 / M_PI);
|
||
}
|
||
else {
|
||
// µ±Pitch½Ó½ü¡À¦Ð/2ʱ£¬RollÎÞ·¨È·¶¨£¬ÉèΪ0
|
||
angles.roll = 0.0;
|
||
}
|
||
|
||
// 4. ¼ÙÉèyawΪ0£¨ÈÆZÖᣩ
|
||
angles.yaw= 0.0;
|
||
|
||
return angles;
|
||
}
|
||
|
||
// ¶¨Òå3x3Ðýת¾ØÕó½á¹¹Ìå
|
||
struct RotationMatrix {
|
||
double data[3][3]; // ÐÐÓÅÏÈ´æ´¢ [row][col]
|
||
};
|
||
|
||
// ½«½Ç¶Èת»»Îª»¡¶È
|
||
inline double degreesToRadians(double degrees) {
|
||
return degrees * M_PI / 180.0;
|
||
}
|
||
|
||
// ´ÓÅ·À½Ç¼ÆËãÐýת¾ØÕó (ZYX˳Ðò: Æ«º½Z -> ¸©ÑöY -> ºá¹öX)
|
||
RotationMatrix eulerToRotationMatrix(double yaw_deg, double pitch_deg, double roll_deg) {
|
||
RotationMatrix R;
|
||
|
||
// ½Ç¶Èת»¡¶È
|
||
double yaw = degreesToRadians(yaw_deg);
|
||
double pitch = degreesToRadians(pitch_deg);
|
||
double roll = degreesToRadians(roll_deg);
|
||
|
||
// Ô¤¼ÆËãÈý½Çº¯Êý
|
||
double cy = cos(yaw);
|
||
double sy = sin(yaw);
|
||
double cp = cos(pitch);
|
||
double sp = sin(pitch);
|
||
double cr = cos(roll);
|
||
double sr = sin(roll);
|
||
|
||
// ¼ÆËãÐýת¾ØÕóÔªËØ£¨ZYX˳Ðò = Rz * Ry * Rx£©
|
||
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;
|
||
}
|
||
|
||
|
||
|
||
// ¶¨ÒåÈýάÏòÁ¿½á¹¹Ìå
|
||
struct Vector3 {
|
||
double x, y, z;
|
||
Vector3(double x_, double y_, double z_) : x(x_), y(y_), z(z_) {}
|
||
};
|
||
|
||
// ¶¨ÒåËÄÔªÊý½á¹¹Ìå
|
||
struct Quaternion {
|
||
double w, x, y, z;
|
||
Quaternion(double w_, double x_, double y_, double z_)
|
||
: w(w_), x(x_), y(y_), z(z_) {}
|
||
};
|
||
|
||
// ¼ÆËãÁ½¸öÏòÁ¿µÄÐýתËÄÔªÊý
|
||
Quaternion rotationBetweenVectors(const Vector3& a, const Vector3& b) {
|
||
// ¹éÒ»»¯ÊäÈëÏòÁ¿
|
||
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) {
|
||
// ÁãÏòÁ¿ÎÞ·¨¶¨ÒåÐýת£¬·µ»Øµ¥Î»ËÄÔªÊý
|
||
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;
|
||
|
||
// ´¦Àí¹²ÏßÇé¿ö
|
||
if (cos_theta > 1.0 - eps) {
|
||
// ÏòÁ¿·½ÏòÏàͬ£¬ÎÞÐèÐýת
|
||
return Quaternion(1.0, 0.0, 0.0, 0.0);
|
||
}
|
||
else if (cos_theta < -1.0 + eps) {
|
||
// ÏòÁ¿·½ÏòÏà·´£¬ÈÆÈÎÒâ´¹Ö±ÖáÐýת180¶È
|
||
Vector3 axis(1.0, 0.0, 0.0); // ĬÈÏÑ¡ÔñXÖá
|
||
if (std::abs(a_norm.y) < eps && std::abs(a_norm.z) < eps) {
|
||
// Èç¹ûa½Ó½üXÖᣬÔòÑ¡ÔñYÖá×÷ΪÐýתÖá
|
||
axis = Vector3(0.0, 1.0, 0.0);
|
||
}
|
||
return Quaternion(0.0, axis.x, axis.y, axis.z); // 180¶ÈÐýת
|
||
}
|
||
|
||
// ¼ÆËãÐýתÖáºÍ°ë½Ç
|
||
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) { // ·ÀÖ¹³ýÁã
|
||
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);
|
||
}
|
||
|
||
//¼ÆËãÒ»¸öƽÃæµ÷ƽ²ÎÊý¡£
|
||
//Êý¾ÝÊäÈëÖпÉÒÔÓÐÒ»¸öµØƽÃæºÍ²Î¿¼µ÷ƽƽÃ棬ÒÔ×î¸ßµÄƽÃæ½øÐе÷ƽ
|
||
//Ðýת¾ØÕóΪµ÷ƽ²ÎÊý£¬¼´½«Æ½Ãæ·¨Ïòµ÷ÕûΪ´¹Ö±ÏòÁ¿µÄ²ÎÊý
|
||
SSG_planeCalibPara sg_getPlaneCalibPara(
|
||
SVzNL3DLaserLine* laser3DPoints,
|
||
int lineNum)
|
||
{
|
||
//ÉèÖóõʼ½á¹û
|
||
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;
|
||
|
||
//ͳ¼Æz·¶Î§
|
||
SVzNLRangeD zRange = { 0, -1 }; //< Z·¶Î§
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
//ÔÚZ·½Ïò½øÐÐͳ¼Æ£¬È¡µÚÒ»¸ö¼«Öµ
|
||
//ÒÔmmΪµ¥Î»£¬¼ò»¯Á¿»¯
|
||
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);
|
||
bool isSame = true;
|
||
//ÒÔÀåÃ×Ϊµ¥Î»½øÐÐÀÛ¼Ó
|
||
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)
|
||
{
|
||
//²»½øÐÐÀÛ¼Ó£¨Èç¹ûÀÛ¼Ó£¬ÀÛ¼ÓÖµÏàµÈ£©
|
||
for (int i = 0; i < zHistSize; i++)
|
||
zSumHist[i] = zHist[i];
|
||
}
|
||
|
||
//Ñ°ÕÒ¼«Öµ
|
||
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: //³õ̬
|
||
if (z_diff < 0) //Ͻµ
|
||
{
|
||
_state = 2;
|
||
}
|
||
else if (z_diff > 0) //ÉÏÉý
|
||
{
|
||
_state = 1;
|
||
}
|
||
break;
|
||
case 1: //ÉÏÉý
|
||
if (z_diff < 0) //Ͻµ
|
||
{
|
||
pkTop.push_back({pre_i, pre_data});
|
||
_state = 2;
|
||
}
|
||
else if(i == (zHistSize-1))
|
||
pkTop.push_back({ i, curr_data });
|
||
break;
|
||
case 2: //Ͻµ
|
||
if (z_diff > 0) // ÉÏÉý
|
||
{
|
||
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;
|
||
}
|
||
//Ñ°ÕÒµÚÒ»¸ö³¬¹ý×ܵãÊý1/3µÄ¼«Öµµã
|
||
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;
|
||
|
||
//Ñ°ÕÒ¿ªÊ¼ºÍ½áÊøλÖÃ
|
||
//ÏòÇ°ÏòºóÑ°ÕÒ
|
||
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;
|
||
|
||
//È¡Êý¾Ý
|
||
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;
|
||
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);
|
||
}
|
||
}
|
||
}
|
||
//ƽÃæÄâºÏ
|
||
std::vector<double> planceFunc;
|
||
vzCaculateLaserPlane(Points3ds, planceFunc);
|
||
|
||
#if 1 //Á½¸öÏòÁ¿µÄÐýתÐýת£¬Ê¹ÓÃËÄÔªÊý·¨£¬
|
||
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);
|
||
//¼ÆËã·´ÏòÐýת¾ØÕó
|
||
Quaternion invQuanPara = rotationBetweenVectors(b, a);
|
||
RotationMatrix invMatrix;
|
||
quaternionToMatrix(invQuanPara, invMatrix.data);
|
||
#else //¸ù¾ÝƽÃæµÄ·¨ÏòÁ¿¼ÆËãÅ·À½Ç£¬½ø¶ø¼ÆËãÐýת¾ØÕó
|
||
//²ÎÊý¼ÆËã
|
||
SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
|
||
//·´Éä½øÐÐУÕý
|
||
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: Á½¸ö¾ØÕóµÄ³Ë»ý±ØÐëÊǵ¥Î»Õó
|
||
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
|
||
//Êý¾Ý½øÐÐת»»
|
||
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;
|
||
}
|
||
|
||
//¼ÆËãÒ»¸öƽÃæµ÷ƽ²ÎÊý¡£
|
||
//Êý¾ÝÊäÈëÖпÉÒÔÓÐÒ»¸öµØƽÃæºÍ²Î¿¼µ÷ƽƽÃ棬ÒÔ×î¸ßµÄƽÃæ½øÐе÷ƽ
|
||
//Ðýת¾ØÕóΪµ÷ƽ²ÎÊý£¬¼´½«Æ½Ãæ·¨Ïòµ÷ÕûΪ´¹Ö±ÏòÁ¿µÄ²ÎÊý
|
||
SSG_planeCalibPara sg_getPlaneCalibPara2(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
//ÉèÖóõʼ½á¹û
|
||
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();
|
||
//ͳ¼Æz·¶Î§
|
||
SVzNLRangeD zRange = { 0, -1 }; //< Z·¶Î§
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
//ÔÚZ·½Ïò½øÐÐͳ¼Æ£¬È¡µÚÒ»¸ö¼«Öµ
|
||
//ÒÔmmΪµ¥Î»£¬¼ò»¯Á¿»¯
|
||
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;
|
||
//ÒÔÀåÃ×Ϊµ¥Î»½øÐÐÀÛ¼Ó
|
||
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)
|
||
{
|
||
//²»½øÐÐÀÛ¼Ó£¨Èç¹ûÀÛ¼Ó£¬ÀÛ¼ÓÖµÏàµÈ£©
|
||
for (int i = 0; i < zHistSize; i++)
|
||
zSumHist[i] = zHist[i];
|
||
}
|
||
|
||
//Ñ°ÕÒ¼«Öµ
|
||
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: //³õ̬
|
||
if (z_diff < 0) //Ͻµ
|
||
{
|
||
_state = 2;
|
||
}
|
||
else if (z_diff > 0) //ÉÏÉý
|
||
{
|
||
_state = 1;
|
||
}
|
||
break;
|
||
case 1: //ÉÏÉý
|
||
if (z_diff < 0) //Ͻµ
|
||
{
|
||
pkTop.push_back({ pre_i, pre_data });
|
||
_state = 2;
|
||
}
|
||
else if (i == (zHistSize - 1))
|
||
pkTop.push_back({ i, curr_data });
|
||
break;
|
||
case 2: //Ͻµ
|
||
if (z_diff > 0) // ÉÏÉý
|
||
{
|
||
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;
|
||
}
|
||
//Ñ°ÕÒµÚÒ»¸ö³¬¹ý×ܵãÊý1/3µÄ¼«Öµµã
|
||
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;
|
||
|
||
//Ñ°ÕÒ¿ªÊ¼ºÍ½áÊøλÖÃ
|
||
//ÏòÇ°ÏòºóÑ°ÕÒ
|
||
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;
|
||
|
||
//È¡Êý¾Ý
|
||
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);
|
||
}
|
||
}
|
||
}
|
||
//ƽÃæÄâºÏ
|
||
std::vector<double> planceFunc;
|
||
vzCaculateLaserPlane(Points3ds, planceFunc);
|
||
|
||
#if 1 //Á½¸öÏòÁ¿µÄÐýתÐýת£¬Ê¹ÓÃËÄÔªÊý·¨£¬
|
||
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);
|
||
//¼ÆËã·´ÏòÐýת¾ØÕó
|
||
Quaternion invQuanPara = rotationBetweenVectors(b, a);
|
||
RotationMatrix invMatrix;
|
||
quaternionToMatrix(invQuanPara, invMatrix.data);
|
||
#else //¸ù¾ÝƽÃæµÄ·¨ÏòÁ¿¼ÆËãÅ·À½Ç£¬½ø¶ø¼ÆËãÐýת¾ØÕó
|
||
//²ÎÊý¼ÆËã
|
||
SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
|
||
//·´Éä½øÐÐУÕý
|
||
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: Á½¸ö¾ØÕóµÄ³Ë»ý±ØÐëÊǵ¥Î»Õó
|
||
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
|
||
//Êý¾Ý½øÐÐת»»
|
||
SVzNLRangeD calibZRange = { 0, -1 };
|
||
topZRange = { 0, -1 };
|
||
double sumMeanZ = 0;
|
||
int sumSize = 0;
|
||
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;
|
||
sumMeanZ += a_calibPt.z;
|
||
sumSize++;
|
||
}
|
||
else
|
||
{
|
||
if (calibZRange.min > a_calibPt.z)
|
||
calibZRange.min = a_calibPt.z;
|
||
if (calibZRange.max < a_calibPt.z)
|
||
calibZRange.max = a_calibPt.z;
|
||
sumMeanZ += a_calibPt.z;
|
||
sumSize++;
|
||
}
|
||
}
|
||
if (sumSize > 0)
|
||
sumMeanZ = sumMeanZ / (double)sumSize;
|
||
planePara.planeHeight = sumMeanZ; // calibZRange.min;
|
||
|
||
return planePara;
|
||
}
|
||
|
||
SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
SVzNL3DRangeD roi)
|
||
{
|
||
//ÉèÖóõʼ½á¹û
|
||
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();
|
||
//È¡Êý¾Ý
|
||
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);
|
||
}
|
||
}
|
||
}
|
||
|
||
//ƽÃæÄâºÏ
|
||
std::vector<double> planceFunc;
|
||
vzCaculateLaserPlane(Points3ds, planceFunc);
|
||
|
||
#if 1 //Á½¸öÏòÁ¿µÄÐýתÐýת£¬Ê¹ÓÃËÄÔªÊý·¨£¬
|
||
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);
|
||
//¼ÆËã·´ÏòÐýת¾ØÕó
|
||
Quaternion invQuanPara = rotationBetweenVectors(b, a);
|
||
RotationMatrix invMatrix;
|
||
quaternionToMatrix(invQuanPara, invMatrix.data);
|
||
#else //¸ù¾ÝƽÃæµÄ·¨ÏòÁ¿¼ÆËãÅ·À½Ç£¬½ø¶ø¼ÆËãÐýת¾ØÕó
|
||
//²ÎÊý¼ÆËã
|
||
SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
|
||
//·´Éä½øÐÐУÕý
|
||
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: Á½¸ö¾ØÕóµÄ³Ë»ý±ØÐëÊǵ¥Î»Õó
|
||
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
|
||
//Êý¾Ý½øÐÐת»»
|
||
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;
|
||
}
|
||
|
||
//¼ÆËãÒ»¸öƽÃæµ÷ƽ²ÎÊý¡£
|
||
//ÒÔÊý¾ÝÊäÈëÖÐROIÒÔÄڵĵã½øÐÐƽÃæÄâºÏ£¬¼ÆËãµ÷ƽ²ÎÊý
|
||
//Ðýת¾ØÕóΪµ÷ƽ²ÎÊý£¬¼´½«Æ½Ãæ·¨Ïòµ÷ÕûΪ´¹Ö±ÏòÁ¿µÄ²ÎÊý
|
||
SSG_planeCalibPara sg_getPlaneCalibPara_ROIs(
|
||
SVzNL3DLaserLine* laser3DPoints,
|
||
int lineNum,
|
||
std::vector<SVzNL3DRangeD>& ROIs)
|
||
{
|
||
//ÉèÖóõʼ½á¹û
|
||
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;
|
||
|
||
//È¡Êý¾Ý
|
||
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;
|
||
for (int m = 0, m_max = (int)ROIs.size(); m < m_max; m++)
|
||
{
|
||
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;
|
||
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);
|
||
}
|
||
}
|
||
|
||
//ƽÃæÄâºÏ
|
||
std::vector<double> planceFunc;
|
||
vzCaculateLaserPlane(Points3ds, planceFunc);
|
||
|
||
#if 1 //Á½¸öÏòÁ¿µÄÐýתÐýת£¬Ê¹ÓÃËÄÔªÊý·¨£¬
|
||
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);
|
||
//¼ÆËã·´ÏòÐýת¾ØÕó
|
||
Quaternion invQuanPara = rotationBetweenVectors(b, a);
|
||
RotationMatrix invMatrix;
|
||
quaternionToMatrix(invQuanPara, invMatrix.data);
|
||
#else //¸ù¾ÝƽÃæµÄ·¨ÏòÁ¿¼ÆËãÅ·À½Ç£¬½ø¶ø¼ÆËãÐýת¾ØÕó
|
||
//²ÎÊý¼ÆËã
|
||
SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
|
||
//·´Éä½øÐÐУÕý
|
||
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: Á½¸ö¾ØÕóµÄ³Ë»ý±ØÐëÊǵ¥Î»Õó
|
||
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
|
||
//Êý¾Ý½øÐÐת»»
|
||
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;
|
||
}
|
||
|
||
// ´ÓÐýת¾ØÕó¼ÆËãÅ·À½Ç£¨Z-Y-X˳Ðò£©
|
||
SSG_EulerAngles rotationMatrixToEulerZYX(const double R[3][3]) {
|
||
SSG_EulerAngles angles;
|
||
|
||
// ¼ÆË㸩Ñö½Ç£¨pitch£©¦È
|
||
angles.pitch = asin(-R[2][0]); // asin·µ»Ø»¡¶È
|
||
|
||
// ¼ì²éÍòÏò½ÚËø£¨cos¦È½Ó½ü0£©
|
||
const double epsilon = 1e-6;
|
||
if (abs(cos(angles.pitch)) > epsilon) {
|
||
// ÎÞÍòÏò½ÚËø£¬Õý³£¼ÆËãyawºÍroll
|
||
angles.yaw = atan2(R[1][0], R[0][0]);
|
||
angles.roll = atan2(R[2][1], R[2][2]);
|
||
}
|
||
else {
|
||
// ÍòÏò½ÚËø£¬Ô¼¶¨roll=0£¬¼ÆËãyaw
|
||
angles.roll = 0.0;
|
||
angles.yaw = atan2(-R[0][1], R[1][1]);
|
||
}
|
||
|
||
// ½«»¡¶Èת»»Îª½Ç¶È
|
||
const double rad2deg = 180.0 / M_PI;
|
||
angles.yaw *= rad2deg;
|
||
angles.pitch *= rad2deg;
|
||
angles.roll *= rad2deg;
|
||
|
||
return angles;
|
||
}
|
||
|
||
// ´ÓÅ·À½Ç¼ÆËãÐýת¾ØÕó£¨Z-Y-X˳Ðò£©
|
||
void eulerToRotationMatrixZYX(const SSG_EulerAngles& angles, double R[3][3]) {
|
||
// ½«½Ç¶Èת»»Îª»¡¶È
|
||
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;
|
||
|
||
// Ô¤¼ÆËãÈý½Çº¯ÊýÖµ
|
||
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
|
||
// ÈÆZÖáÐýת¾ØÕó
|
||
double Rz[3][3] = {
|
||
{cy, -sy, 0},
|
||
{sy, cy, 0},
|
||
{0, 0, 1}
|
||
};
|
||
|
||
// ÈÆYÖáÐýת¾ØÕó
|
||
double Ry[3][3] = {
|
||
{cp, 0, sp},
|
||
{0, 1, 0},
|
||
{-sp, 0, cp}
|
||
};
|
||
|
||
// ÈÆXÖáÐýת¾ØÕó
|
||
double Rx[3][3] = {
|
||
{1, 0, 0},
|
||
{0, cr, -sr},
|
||
{0, sr, cr}
|
||
};
|
||
|
||
|
||
// ¾ØÕóÏà³Ë˳Ðò£ºR = Rz * Ry * Rx
|
||
for (int i = 0; i < 3; ++i) {
|
||
for (int j = 0; j < 3; ++j) {
|
||
// ÏȼÆËã Rz * Ry
|
||
double temp[3][3] = { 0 };
|
||
for (int k = 0; k < 3; ++k) {
|
||
temp[i][j] += Rz[i][k] * Ry[k][j];
|
||
}
|
||
|
||
// ÔÙÓë Rx Ïà³Ë
|
||
R[i][j] = 0;
|
||
for (int k = 0; k < 3; ++k) {
|
||
R[i][j] += temp[i][k] * Rx[k][j];
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
|
||
// ÓÅ»¯ºóµÄÖ±½Ó¼ÆË㹫ʽ£¨±ÜÃâÖмä¾ØÕó£©
|
||
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;
|
||
}
|
||
|
||
//¸ù¾ÝÏà»ú×Ë̬¶ÔÏà»ú²É¼¯µÄ3DÊý¾Ý½øÐÐÐýת(ûÓÐƽÒÆ)£¬½«Êý¾Ýµ÷ÕûΪ¸©ÊÓ״̬
|
||
///camPoseRΪ3x3¾ØÕó
|
||
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)) //È¥³ýµØÃæ
|
||
z = 0;
|
||
a_pt.x = x;
|
||
a_pt.y = y;
|
||
a_pt.z = z;
|
||
a_line->p3DPosition[i].pt3D = a_pt;
|
||
}
|
||
return;
|
||
}
|
||
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)) //È¥³ýµØÃæ
|
||
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)) //È¥³ýµØÃæ
|
||
z = 0;
|
||
a_pt.x = (float)x;
|
||
a_pt.y = (float)y;
|
||
a_pt.z = (float)z;
|
||
a_line->p3DPoint[i] = a_pt;
|
||
}
|
||
return;
|
||
}
|
||
|
||
//¶ÔÕ¤¸ñ»¯Êý¾Ý½øÐÐXYƽÃæÉϵÄͶӰÁ¿»¯£¬²¢¶ÔÁ¿»¯²úÉúµÄ¿Õ°×µã½øÐвåÖµ
|
||
void pointClout2DProjection(
|
||
std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
|
||
SVzNLRangeD x_range,
|
||
SVzNLRangeD y_range,
|
||
double scale,
|
||
int edgeSkip,
|
||
double inerPolateDistTh, //²åÖµãÐÖµ¡£´óÓÚ´ËãÐÖµµÄ²»½øÐÐÁ¿»¯²åÖµ
|
||
cv::Mat& projectionData,//ͶӰÁ¿»¯Êý¾Ý£¬³õʼ»¯ÎªÒ»¸ö¼«´óÖµ1e+6
|
||
cv::Mat& backIndexing //±ê¼Ç×ø±êË÷Òý£¬ÓÃÓÚ»ØÕÒ3D×ø±ê
|
||
)
|
||
{
|
||
int lineNum = (int)gridScanData.size();
|
||
if (lineNum == 0)
|
||
return;
|
||
int nPointCnt = (int)gridScanData[0].size();
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
int pre_x = -1, pre_y = -1;
|
||
SVzNL3DPosition* prePt = NULL;
|
||
for (int i = 0; i < nPointCnt; i++)
|
||
{
|
||
SVzNL3DPosition* pt3D = &gridScanData[line][i];
|
||
if (pt3D->pt3D.z < 1e-4)
|
||
continue;
|
||
double x = pt3D->pt3D.x;
|
||
double y = pt3D->pt3D.y;
|
||
int px = (int)(x - x_range.min)/scale + edgeSkip;
|
||
int py = (int)(y - y_range.min)/scale + edgeSkip;
|
||
|
||
cv::Vec2i v2i_exist = backIndexing.at<cv::Vec2i>(py, px);
|
||
#if 0
|
||
if ((v2i_exist[0] > 0) || (v2i_exist[1] > 0)) //¶à¸öµãÖظ´Í¶Ó°µ½Í¬Ò»¸öµãÉÏ£¬Ö»±£ÁôÒ»¸öÓÐЧµã
|
||
{
|
||
pt3D->pt3D.z = 0; //invalidate
|
||
}
|
||
else
|
||
#endif
|
||
{
|
||
cv::Vec2i v2i = { line, i };
|
||
backIndexing.at<cv::Vec2i>(py, px) = v2i;
|
||
projectionData.at<float>(py, px) = 1e+6;
|
||
//´¹Ö±²åÖµ
|
||
if (prePt)
|
||
{
|
||
//¼ÆËã¾àÀ룬³¬¹ýÒ»¶¨¾àÀëÔò²»²åÖµ
|
||
double dist = sqrt(pow(pt3D->pt3D.x - prePt->pt3D.x, 2) +
|
||
pow(pt3D->pt3D.y - prePt->pt3D.y, 2) +
|
||
pow(pt3D->pt3D.z - prePt->pt3D.z, 2));
|
||
if (dist < inerPolateDistTh)
|
||
{
|
||
std::vector<SVzNL2DPoint> interPts;
|
||
drawLine(
|
||
pre_x,
|
||
pre_y,
|
||
px,
|
||
py,
|
||
interPts);
|
||
for (int m = 0, m_max = (int)interPts.size(); m < m_max; m++)
|
||
projectionData.at<float>(interPts[m].y, interPts[m].x) = 1e+6;
|
||
}
|
||
}
|
||
prePt = pt3D;
|
||
pre_x = px;
|
||
pre_y = py;
|
||
}
|
||
}
|
||
}
|
||
//ˮƽ²åÖµ
|
||
int pixWin = (int)(inerPolateDistTh / scale);
|
||
for (int y = 0; y < projectionData.rows; y++)
|
||
{
|
||
int pre_x = -1;
|
||
for (int x = 0; x < projectionData.cols; x++)
|
||
{
|
||
double value = projectionData.at<float>(y, x);
|
||
if (value > 1e-4)
|
||
{
|
||
if (pre_x >= 0)
|
||
{
|
||
//²åÖµ
|
||
int x_diff = x - pre_x;
|
||
if ((x_diff > 1) && (x_diff < pixWin))
|
||
{
|
||
for (int m = pre_x + 1; m < x; m++)
|
||
projectionData.at<float>(y, m) = 1e+6;
|
||
}
|
||
}
|
||
pre_x = x;
|
||
}
|
||
}
|
||
}
|
||
}
|