1845 lines
45 KiB
C++
1845 lines
45 KiB
C++
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include <vector>
|
||
#include <corecrt_math_defines.h>
|
||
#include <cmath>
|
||
#include <unordered_map>
|
||
|
||
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;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
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
|
||
}
|
||
|
||
#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 <20><>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD>ע<EFBFBD><D7A2><EFBFBD><EFBFBD>
|
||
* @param image <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵͼ<D6B5><CDBC><EFBFBD><EFBFBD>0<EFBFBD><30>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0Ϊǰ<CEAA><C7B0>
|
||
* @param labels <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ<EFBFBD><C7A9><EFBFBD><EFBFBD>
|
||
* @param connectivity <20><>ͨ<EFBFBD>ԣ<EFBFBD>4<EFBFBD><34>8<EFBFBD><38>
|
||
*/
|
||
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;
|
||
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;
|
||
//<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);
|
||
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++)
|
||
{
|
||
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);
|
||
}
|
||
}
|
||
}
|
||
//ƽ<><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 };
|
||
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);
|
||
|
||
#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 };
|
||
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;
|
||
}
|
||
|
||
//<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;
|
||
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);
|
||
}
|
||
}
|
||
|
||
//ƽ<><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;
|
||
}
|
||
|
||
// <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;
|
||
}
|
||
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;
|
||
a_pt.x = (float)x;
|
||
a_pt.y = (float)y;
|
||
a_pt.z = (float)z;
|
||
a_line->p3DPoint[i] = a_pt;
|
||
}
|
||
return;
|
||
} |