1380 lines
34 KiB
C++
1380 lines
34 KiB
C++
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include <vector>
|
||
#include <corecrt_math_defines.h>
|
||
#include <cmath>
|
||
|
||
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;
|
||
}
|
||
|
||
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
|
||
}
|
||
|
||
#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) = 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);
|
||
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;
|
||
}
|
||
|
||
//寻找极值
|
||
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;
|
||
}
|
||
break;
|
||
case 2: //下降
|
||
if (z_diff > 0) // 上升
|
||
{
|
||
int pkBtmIdx = pkBtm.size();
|
||
pkBtmBackIndexing[pre_i] = pkBtmIdx;
|
||
pkBtm.push_back({ pre_i, pre_data });
|
||
_state = 1;
|
||
}
|
||
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 = 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 = pt3D->pt3D.x;
|
||
a_vldPt.y = pt3D->pt3D.y;
|
||
a_vldPt.z = 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 = 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 = Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2];
|
||
a_calibPt.y = Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5];
|
||
a_calibPt.z = 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 = 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 = pt3D->pt3D.x;
|
||
a_vldPt.y = pt3D->pt3D.y;
|
||
a_vldPt.z = 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 = Points3ds.size(); i < i_max; i++)
|
||
{
|
||
cv::Point3f a_calibPt;
|
||
a_calibPt.x = Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2];
|
||
a_calibPt.y = Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5];
|
||
a_calibPt.z = 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;
|
||
} |