#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include <vector>
#ifdef  __WIN32
#include <corecrt_math_defines.h>
#endif //  __WIN32

#include <cmath>
#include <unordered_map>
#include <Eigen/dense>

const double EPS = 1e-10;

SVzNL3DRangeD sg_getScanDataROI(
//计算扫描ROI
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum)
{
	SVzNL3DRangeD roi;
	roi.xRange = { 0, -1 };
	roi.yRange = { 0, -1 };
	roi.zRange = { 0, -1 };

	for (int line = 0; line < lineNum; line++)
	{
		for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
			if (pt3D->pt3D.z < 1e-4)
				continue;

			if (roi.xRange.max < roi.xRange.min)
			{
				roi.xRange.min = pt3D->pt3D.x;
				roi.xRange.max = pt3D->pt3D.x;
			}
			else
			{
				if (roi.xRange.min > pt3D->pt3D.x)
					roi.xRange.min = pt3D->pt3D.x;
				if (roi.xRange.max < pt3D->pt3D.x)
					roi.xRange.max = pt3D->pt3D.x;
			}
			//y
			if (roi.yRange.max < roi.yRange.min)
			{
				roi.yRange.min = pt3D->pt3D.y;
				roi.yRange.max = pt3D->pt3D.y;
			}
			else
			{
				if (roi.yRange.min > pt3D->pt3D.y)
					roi.yRange.min = pt3D->pt3D.y;
				if (roi.yRange.max < pt3D->pt3D.y)
					roi.yRange.max = pt3D->pt3D.y;
			}
			//z
			if (roi.zRange.max < roi.zRange.min)
			{
				roi.zRange.min = pt3D->pt3D.z;
				roi.zRange.max = pt3D->pt3D.z;
			}
			else
			{
				if (roi.zRange.min > pt3D->pt3D.z)
					roi.zRange.min = pt3D->pt3D.z;
				if (roi.zRange.max < pt3D->pt3D.z)
					roi.zRange.max = pt3D->pt3D.z;
			}
		}
	}
	return roi;
}

//计算扫描ROI: vecotr格式
SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	SVzNL3DRangeD roi;
	roi.xRange = { 0, -1 };
	roi.yRange = { 0, -1 };
	roi.zRange = { 0, -1 };

	int lineNum = (int)scanLines.size();
	for (int line = 0; line < lineNum; line++)
	{
		int nPositionCnt = (int)scanLines[line].size();
		for (int i = 0; i < nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &scanLines[line][i];
			if (pt3D->pt3D.z < 1e-4)
				continue;

			if (roi.xRange.max < roi.xRange.min)
			{
				roi.xRange.min = pt3D->pt3D.x;
				roi.xRange.max = pt3D->pt3D.x;
			}
			else
			{
				if (roi.xRange.min > pt3D->pt3D.x)
					roi.xRange.min = pt3D->pt3D.x;
				if (roi.xRange.max < pt3D->pt3D.x)
					roi.xRange.max = pt3D->pt3D.x;
			}
			//y
			if (roi.yRange.max < roi.yRange.min)
			{
				roi.yRange.min = pt3D->pt3D.y;
				roi.yRange.max = pt3D->pt3D.y;
			}
			else
			{
				if (roi.yRange.min > pt3D->pt3D.y)
					roi.yRange.min = pt3D->pt3D.y;
				if (roi.yRange.max < pt3D->pt3D.y)
					roi.yRange.max = pt3D->pt3D.y;
			}
			//z
			if (roi.zRange.max < roi.zRange.min)
			{
				roi.zRange.min = pt3D->pt3D.z;
				roi.zRange.max = pt3D->pt3D.z;
			}
			else
			{
				if (roi.zRange.min > pt3D->pt3D.z)
					roi.zRange.min = pt3D->pt3D.z;
				if (roi.zRange.max < pt3D->pt3D.z)
					roi.zRange.max = pt3D->pt3D.z;
			}
		}
	}
	return roi;
}

//计算点云ROI: vecotr格式
SVzNL3DRangeD wd_getPointCloudROI(std::vector<SVzNL3DPoint>& scanData)
{
	SVzNL3DRangeD roi;
	roi.xRange = { 0, -1 };
	roi.yRange = { 0, -1 };
	roi.zRange = { 0, -1 };

	int nPositionCnt = (int)scanData.size();
	for (int i = 0; i < nPositionCnt; i++)
	{
		SVzNL3DPoint& pt3D = scanData[i];
		if (pt3D.z < 1e-4)
			continue;

		if (roi.xRange.max < roi.xRange.min)
		{
			roi.xRange.min = pt3D.x;
			roi.xRange.max = pt3D.x;
		}
		else
		{
			if (roi.xRange.min > pt3D.x)
				roi.xRange.min = pt3D.x;
			if (roi.xRange.max < pt3D.x)
				roi.xRange.max = pt3D.x;
		}
		//y
		if (roi.yRange.max < roi.yRange.min)
		{
			roi.yRange.min = pt3D.y;
			roi.yRange.max = pt3D.y;
		}
		else
		{
			if (roi.yRange.min > pt3D.y)
				roi.yRange.min = pt3D.y;
			if (roi.yRange.max < pt3D.y)
				roi.yRange.max = pt3D.y;
		}
		//z
		if (roi.zRange.max < roi.zRange.min)
		{
			roi.zRange.min = pt3D.z;
			roi.zRange.max = pt3D.z;
		}
		else
		{
			if (roi.zRange.min > pt3D.z)
				roi.zRange.min = pt3D.z;
			if (roi.zRange.max < pt3D.z)
				roi.zRange.max = pt3D.z;
		}
	}

	return roi;
}

//计算点云的ROI和scale: vecotr格式
SWD_pointCloudPara wd_getPointCloudPara(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	SWD_pointCloudPara para;
	para.xRange = { 0, -1 };
	para.yRange = { 0, -1 };
	para.zRange = { 0, -1 };
	para.scale_x = -1;  //初始值
	para.scale_y = -1;
	int lineNum = (int)scanLines.size();
	double x_scale = 0;
	int x_scale_cnt = 0;
	double y_scale = 0;
	double y_scale_cnt = 0;
	for (int line = 0; line < lineNum; line++)
	{
		int nPositionCnt = (int)scanLines[line].size();
		for (int i = 0; i < nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &scanLines[line][i];
			if (pt3D->pt3D.z < 1e-4)
				continue;

			if (i > 0)
			{
				if (scanLines[line][i - 1].pt3D.z > 1e-4)
				{
					y_scale += abs(pt3D->pt3D.y - scanLines[line][i - 1].pt3D.y);
					y_scale_cnt++;
				}
			}
			if (line > 0)
			{
				if (scanLines[line - 1][i].pt3D.z > 1e-4)
				{
					x_scale += abs(pt3D->pt3D.x - scanLines[line-1][i].pt3D.x);
					x_scale_cnt++;
				}
			}

			if (para.xRange.max < para.xRange.min)
			{
				para.xRange.min = pt3D->pt3D.x;
				para.xRange.max = pt3D->pt3D.x;
			}
			else
			{
				if (para.xRange.min > pt3D->pt3D.x)
					para.xRange.min = pt3D->pt3D.x;
				if (para.xRange.max < pt3D->pt3D.x)
					para.xRange.max = pt3D->pt3D.x;
			}
			//y
			if (para.yRange.max < para.yRange.min)
			{
				para.yRange.min = pt3D->pt3D.y;
				para.yRange.max = pt3D->pt3D.y;
			}
			else
			{
				if (para.yRange.min > pt3D->pt3D.y)
					para.yRange.min = pt3D->pt3D.y;
				if (para.yRange.max < pt3D->pt3D.y)
					para.yRange.max = pt3D->pt3D.y;
			}
			//z
			if (para.zRange.max < para.zRange.min)
			{
				para.zRange.min = pt3D->pt3D.z;
				para.zRange.max = pt3D->pt3D.z;
			}
			else
			{
				if (para.zRange.min > pt3D->pt3D.z)
					para.zRange.min = pt3D->pt3D.z;
				if (para.zRange.max < pt3D->pt3D.z)
					para.zRange.max = pt3D->pt3D.z;
			}
		}
	}
	if (x_scale_cnt > 0)
		para.scale_x = x_scale / (double)x_scale_cnt;
	if (y_scale_cnt > 0)
		para.scale_y = y_scale / (double)y_scale_cnt;
	return para;
}

//计算Z均值
double computeMeanZ(std::vector< SVzNL3DPoint>& pts)
{
	int ptNum = (int)pts.size();
	int vldNum = 0;
	double sumZ = 0;
	for (int i = 0; i < ptNum; i++)
	{
		if (pts[i].z > 1e-4)
		{
			sumZ += pts[i].z;
			vldNum++;
		}
	}
	if (vldNum > 0)
		return (sumZ / vldNum);
	else
		return 0;
}

SVzNL3DPoint computeLineCrossPt_abs(double a1, double b1, double c1, double a2, double b2, double c2)
{
	SVzNL3DPoint crossPt;
	crossPt.x = (c2 * b1 - c1 * b2) / (a1 * b2 - a2 * b1);
	crossPt.y = (c2 * a1 - c1 * a2) / (b1 * a2 - b2 * a1);
	crossPt.z = 0;
	return crossPt;
}

//计算角度差值，在0-180度范围
double computeAngleDiff(double theta1, double theta2)
{
	double diff = theta1 - theta2;
	if (diff < 0)
		diff += 360;
	if (diff > 180)
		diff = 360 - diff;

	return diff;
}

void compute2ptLine(SVzNL3DPoint pt1, SVzNL3DPoint pt2, double* _a, double* _b, double* _c)
{
	*_a = pt2.y - pt1.y;
	*_b = pt1.x - pt2.x;
	*_c = pt2.x * pt1.y - pt1.x * pt2.y;
	return;
}

void compute2ptLine_2(double x1, double y1, double x2, double y2, double* _a, double* _b, double* _c)
{
	*_a = y2 - y1;
	*_b = x1 - x2;
	*_c = x2 * y1 - x1 * y2;
	return;
}

//旋转45度后的直线方程
void rotateLine45Deg(
	double _a, double _b, double _c,
	double x0, double y0,
	double* r_a, double* r_b, double* r_c)
{
	// 旋转后直线的系数（基于数学推导）
	*r_a = _a + _b;
	*r_b = _b - _a;
	*r_c = -(*r_a) * x0 - (*r_b) * y0;
	return;
}

double getLineAngle(const double _a, const double _b, const double _c)
{
	if (_a == 0)
		return 0;
	else if (_b == 0)
		return 90;
	else
	{
		double k = _a / _b;
		double theta = atan(-k) + PI / 2;
		theta = (theta * 180.0) / PI;
		return theta;
	}
}

//计算两点的2D距离
double compute2DLen(SVzNL3DPoint pt1, SVzNL3DPoint pt2)
{
	double len = sqrt(pow(pt1.x - pt2.x, 2) + pow(pt1.y - pt2.y, 2));
	return len;
}

//计算XY平面面的三角形顶角(p0的张角)
double computeXOYVertexAngle(SVzNL3DPoint p0, SVzNL3DPoint p1, SVzNL3DPoint p2)
{
	double len_c = compute2DLen(p1, p2);
	double len_a = compute2DLen(p0, p1);
	double len_b = compute2DLen(p0, p2);
	double cosAngle = (pow(len_a, 2) + pow(len_b, 2) - pow(len_c, 2)) / (2 * len_a * len_b);
	double angle = acos(cosAngle);
	angle = angle * 180 / M_PI;
	if (angle < 0)
		angle = angle + 180;
	return angle;
}


// 计算向量的模长
double vecNorm(const SVzNL2DPointD& v) {
	return sqrt(v.x * v.x + v.y * v.y);
}

// 向量归一化（单位向量），返回是否成功（零向量返回false）
bool vecNormalize(SVzNL2DPointD& v) {
	double norm = vecNorm(v);
	if (norm < EPS) { // 零向量，无法归一化
		return false;
	}
	v.x /= norm;
	v.y /= norm;
	return true;
}

// 计算两个向量的点积
double vecDot(const SVzNL2DPointD& a, const SVzNL2DPointD& b) {
	return a.x * b.x + a.y * b.y;
}

// 计算两个向量的2D叉积（标量值）
double vecCross(const SVzNL2DPointD& a, const SVzNL2DPointD& b) {
	return a.x * b.y - a.y * b.x;
}
/**
 * @brief 计算从向量a到向量b的**有方向旋转角**（范围：-π ~ π）
 * @param a 源向量
 * @param b 目标向量
 * @param rotAngle 输出：旋转角（弧度），逆时针为正，顺时针为负
 * @return true：计算成功，false：零向量（失败）
 */
bool calcRotateAngle(const SVzNL2DPointD& a, const SVzNL2DPointD& b, double& rotAngle) {
	SVzNL2DPointD aNorm = a;
	SVzNL2DPointD bNorm = b;
	// 归一化两个向量，零向量直接返回失败
	if (!vecNormalize(aNorm) || !vecNormalize(bNorm)) {
		std::cerr << "Error: 输入为零向量，无法计算旋转角！" << std::endl;
		return false;
	}
	// 计算点积并钳位（避免浮点精度导致超出[-1,1]）
	double dot = vecDot(aNorm, bNorm);
	if (dot < -1.0 + EPS)
		dot = -1.0 + EPS;
	if (dot > 1.0 - EPS)
		dot = 1.0 - EPS;

	// 点积求无方向夹角（0 ~ π）
	double angle = acos(dot);
	// 叉积判断旋转方向
	double cross = vecCross(aNorm, bNorm);
	if (cross < -EPS) { // 顺时针，角度取负
		rotAngle = -angle;
	}
	else { // 逆时针/共线，角度取正
		rotAngle = angle;
	}
	return true;
}

double computePtDistToLine(double x0, double y0, double a, double b, double c)
{
	double tmp = sqrt(pow(a, 2) + pow(b, 2));
	double dist = abs(a * x0 + b * y0 + c) / tmp;
	return dist;
}

//计算垂足点，直线方程：y = kx + b
SVzNL2DPointD sx_getFootPoint(double x0, double y0, double k, double b)
{
	double A = k;
	double B = -1;
	double C = b;
	SVzNL2DPointD foot;
	foot.x = (B * B * x0 - A * B * y0 - A * C) / (A * A + B * B);
	foot.y = (-A * B * x0 + A * A * y0 - B * C) / (A * A + B * B);
	return foot;
}

//计算垂足点，直线方程：ax+by+c = 0
SVzNL2DPointD sx_getFootPoint_abc(double x0, double y0, double A, double B, double C)
{
	SVzNL2DPointD foot;
	foot.x = (B * B * x0 - A * B * y0 - A * C) / (A * A + B * B);
	foot.y = (-A * B * x0 + A * A * y0 - B * C) / (A * A + B * B);
	return foot;
}
#if 0
void icvprCcaByTwoPass(const cv::Mat& binImg, cv::Mat& lableImg)
{
	// connected component analysis (4-component)
	// use two-pass algorithm
	// 1. first pass: label each foreground pixel with a label
	// 2. second pass: visit each labeled pixel and merge neighbor labels
	// 
	// foreground pixel: binImg(x,y) = 1
	// background pixel: binImg(x,y) = 0


	if (binImg.empty() ||
		binImg.type() != CV_8UC1)
	{
		return;
	}

	// 1. first pass

	lableImg.release();
	binImg.convertTo(lableImg, CV_32SC1);

	int label = 1;  // start by 2
	std::vector<int> labelSet;
	labelSet.push_back(0);   // background: 0
	labelSet.push_back(1);   // foreground: 1

	int rows = binImg.rows - 1;
	int cols = binImg.cols - 1;
	for (int i = 1; i < rows; i++)
	{
		int* data_preRow = lableImg.ptr<int>(i - 1);
		int* data_curRow = lableImg.ptr<int>(i);
		for (int j = 1; j < cols; j++)
		{
			if (data_curRow[j] == 1)
			{
				std::vector<int> neighborLabels;
				neighborLabels.reserve(2);
				int leftPixel = data_curRow[j - 1];
				int upPixel = data_preRow[j];
				if (leftPixel > 1)
				{
					neighborLabels.push_back(leftPixel);
				}
				if (upPixel > 1)
				{
					neighborLabels.push_back(upPixel);
				}

				if (neighborLabels.empty())
				{
					labelSet.push_back(++label);  // assign to a new label
					data_curRow[j] = label;
					labelSet[label] = label;
				}
				else
				{
					std::sort(neighborLabels.begin(), neighborLabels.end());
					int smallestLabel = neighborLabels[0];
					data_curRow[j] = smallestLabel;

					// save equivalence
					for (size_t k = 1; k < neighborLabels.size(); k++)
					{
						int tempLabel = neighborLabels[k];
						int& oldSmallestLabel = labelSet[tempLabel];
						if (oldSmallestLabel > smallestLabel)
						{
							labelSet[oldSmallestLabel] = smallestLabel;
							oldSmallestLabel = smallestLabel;
						}
						else if (oldSmallestLabel < smallestLabel)
						{
							labelSet[smallestLabel] = oldSmallestLabel;
						}
					}
				}
			}
		}
	}

	// update equivalent labels
	// assigned with the smallest label in each equivalent label set
	for (size_t i = 2; i < labelSet.size(); i++)
	{
		int curLabel = labelSet[i];
		int preLabel = labelSet[curLabel];
		while (preLabel != curLabel)
		{
			curLabel = preLabel;
			preLabel = labelSet[preLabel];
		}
		labelSet[i] = curLabel;
	}


	// 2. second pass
	for (int i = 0; i < rows; i++)
	{
		int* data = lableImg.ptr<int>(i);
		for (int j = 0; j < cols; j++)
		{
			int& pixelLabel = data[j];
			pixelLabel = labelSet[pixelLabel];
		}
	}
}
#endif


#if 0
//Bresenham算法
void line(int x0, int y0, int x1, int y1, TGAImage& image, TGAColor color) {
	bool steep = false;
	if (std::abs(x1 - x0) < std::abs(y1 - y0)) {
		std::swap(x0, y0);
		std::swap(x1, y1);
		steep = true;
	}
	if (x0 > x1) {
		std::swap(x0, x1);
		std::swap(y0, y1);
	}
	int dx = x1 - x0;
	int dy = y1 - y0;
	int deltaY = std::abs(dy << 1);
	int middle = dx;
	int y = y0;
	for (int x = x0; x <= x1; ++x) {
		if (steep) {
			image.set(y, x, color);
		}
		else {
			image.set(x, y, color);
		}
		deltaY += std::abs(dy << 1);
		if (deltaY >= middle) {
			y += (y1 > y0 ? 1 : -1);
			middle += std::abs(dx << 1);
		}
	}
}
#endif

//Bresenham算法
void drawLine(
	int x0, 
	int y0, 
	int x1, 
	int y1, 
	std::vector<SVzNL2DPoint>& pts)
{
	// 计算dx和dy的绝对值
	int dx = abs(x1 - x0);
	int dy = abs(y1 - y0);

	// 确定步进方向
	int sx = (x0 < x1) ? 1 : -1; // x方向步进
	int sy = (y0 < y1) ? 1 : -1; // y方向步进

	// 初始化误差变量，结合dx和dy的符号
	int err = dx - dy;

	while (true) {
		SVzNL2DPoint a_pt = { x0, y0 };
		pts.push_back(a_pt);

		// 到达终点时退出循环
		if (x0 == x1 && y0 == y1) break;

		int e2 = 2 * err; // 当前误差的两倍

		// 根据误差决定步进方向
		if (e2 > -dy) {   // 误差倾向于x方向步进
			err -= dy;
			x0 += sx;
		}
		if (e2 < dx) {    // 误差倾向于y方向步进
			err += dx;
			y0 += sy;
		}
	}
}

/// <summary>
/// 两步法标注
/// </summary>
/// <param name="bwImg"> 目标点为“1”， 空白点为“0”</param>
/// <param name="labImg"> 标注结果。每个点为rgnID, ID从2开始 </param>
/// <param name="labelRgns"></param>
#if 0
void SG_TwoPassLabel(
	const cv::Mat& bwImg, 
	cv::Mat& labImg, 
	std::vector<SSG_Region>& labelRgns,
	int connectivity)
{
	assert(bwImg.type() == CV_8UC1);
	bwImg.convertTo(labImg, CV_32SC1);
	int rows = bwImg.rows - 1;
	int cols = bwImg.cols - 1;

	//二值图像像素值为0或1，为了不冲突，label从2开始
	int label = 2;
	std::vector<int> labelSet;
	labelSet.push_back(0);
	labelSet.push_back(1);

	//第一次扫描
	int* data_prev = (int*)labImg.data;
	int* data_cur = (int*)(labImg.data + labImg.step);
	int left, up;//指针指向的像素点的左方点和上方点
	int neighborLabels[2];
	for (int i = 1; i < rows; i++)// 忽略第一行和第一列,其实可以将labImg的宽高加1，然后在初始化为0就可以了
	{
		data_cur++;
		data_prev++;
		for (int j = 1; j < cols; j++, data_cur++, data_prev++)
		{
			if ((i == 1409) && (j == 432))
				int kkk = 1;
			if (*data_cur != 1)//当前点不为1，扫描下一个点
				continue;
			left = *(data_cur - 1);
			up = *data_prev;

			int count = 0;
			for (int curLabel : {left, up})
			{
				if (curLabel > 1)
					neighborLabels[count++] = curLabel;
			}
			if (!count)//赋予一个新的label
			{
				labelSet.push_back(label);
				*data_cur = label;
				label++;
				continue;
			}
			//将当前点标记设为左点和上点label的最小值
			int smallestLabel = neighborLabels[0];
			if (count == 2 && neighborLabels[1] < smallestLabel)
				smallestLabel = neighborLabels[1];
			*data_cur = smallestLabel;
			//设置等价表，这里可能有点难理解
			//左点有可能比上点小，也有可能比上点大，两种情况都要考虑,例如
			//0 0 1 0 1 0       x x 2 x 3 x
			//1 1 1 1 1 1  ->   4 4 2 2 2 2
			//要将labelSet中3的位置设置为2
			for (int k = 0; k < count; k++)
			{
				int neiLabel = neighborLabels[k];
				int oldSmallestLabel = labelSet[neiLabel];

				if (oldSmallestLabel > smallestLabel)
				{
					if ((oldSmallestLabel == 117) && (smallestLabel == 113))
						int kkk = 1;
					labelSet[oldSmallestLabel] = smallestLabel;
				}
				else if (oldSmallestLabel < smallestLabel)
				{
					if ((smallestLabel == 117) && (oldSmallestLabel == 113))
						int kkk = 1;
					if (labelSet[smallestLabel] != oldSmallestLabel)
					{
					}
					labelSet[smallestLabel] = oldSmallestLabel;
				}
			}
		}
		data_cur++;
		data_prev++;
	}
	//上面一步中,有的labelSet的位置还未设为最小值，例如
	//0 0 1 0 1      x x 2 x 3
	//0 1 1 1 1  ->  x 4 2 2 2
	//1 1 1 0 1      5 4 2 x 2
	//上面这波操作中，把labelSet[4]设为2，但labelSet[5]仍为4
	//这里可以将labelSet[5]设为2
	for (size_t i = 2; i < labelSet.size(); i++)
	{
		int curLabel = labelSet[i];
		int prelabel = labelSet[curLabel];
		while (prelabel != curLabel)
		{
			curLabel = prelabel;
			prelabel = labelSet[prelabel];
		}
		labelSet[i] = curLabel;
	}
	//第二次扫描，用labelSet进行更新，最后一列
	std::vector<SSG_Region*> labelInfo;
	labelInfo.resize(labelSet.size(), nullptr);

	data_cur = (int*)labImg.data;
	for (int i = 0; i < labImg.rows; i++)
	{
		for (int j = 0; j < labImg.cols; j++)
		{
			*data_cur = labelSet[*data_cur];
			if (*data_cur > 1) //有效label
			{
				//统计Region信息
				SSG_Region* info_cur = (SSG_Region*)labelInfo[*data_cur];
				if (nullptr == info_cur)
				{
					SSG_Region new_rgn = { {j,j,i,i}, 1, *data_cur };
					labelRgns.push_back(new_rgn); //push_back()后，vector中内存单元可能会被改动
					for (int m = 0; m < labelRgns.size(); m++)
					{
						info_cur = &labelRgns[m];
						labelInfo[info_cur->labelID] = info_cur;
					}
				}
				else
				{
					assert(*data_cur == info_cur->labelID);
					if (info_cur->roi.left > j)
						info_cur->roi.left = j;
					if (info_cur->roi.right < j)
						info_cur->roi.right = j;
					if (info_cur->roi.top > i)
						info_cur->roi.top = i;
					if (info_cur->roi.bottom < i)
						info_cur->roi.bottom = i;
					info_cur->ptCounter++;
				}
			}
			data_cur++;
		}
	}

	return;
}
#else
// 查找函数（带路径压缩）
int find(int x, std::vector<int>& parent) {
	if (parent[x] != x) {
		parent[x] = find(parent[x], parent);
	}
	return parent[x];
}

// 合并函数（按秩合并到较小根）
void unionSet(int x, int y, std::vector<int>& parent) {
	int rootX = find(x, parent);
	int rootY = find(y, parent);
	if (rootX != rootY) {
		if (rootX < rootY) {
			parent[rootY] = rootX;
		}
		else {
			parent[rootX] = rootY;
		}
	}
}

/**
 * @brief 连通域标注函数
 * @param image 输入二值图像，0表示背景，非0为前景
 * @param labels 输出标签矩阵
 * @param connectivity 连通性（4或8）
 */
void SG_TwoPassLabel(
	const cv::Mat& bwImg, 
	cv::Mat& labImg, 
	std::vector<SSG_Region>& labelRgns,
	int connectivity) 
{
	assert(bwImg.type() == CV_8UC1);
	bwImg.convertTo(labImg, CV_32SC1);

	if (bwImg.rows == 0)
		return;

	int rows = bwImg.rows - 1;
	int cols = bwImg.cols - 1;

	// 初始化并查集（最大可能标签数为像素总数）
	int max_label = rows * cols;
	std::vector<int> parent(max_label + 1);
	for (int i = 0; i <= max_label; ++i) {
		parent[i] = i;
	}

	//第一次扫描
	int label_cnt = 2; // 当前最大标签,二值图像像素值为0或1，为了不冲突，label从2开始
	int* data_prev = (int*)labImg.data;
	int* data_cur = (int*)(labImg.data + labImg.step);
	// 第一遍扫描：临时标签分配
	for (int i = 1; i < rows; i++) 
	{
		data_cur++;
		data_prev++;
		for (int j = 1; j < cols; j++, data_cur++, data_prev++)
		{
			if (*data_cur != 1)//当前点不为1，扫描下一个点
				continue;
			int left = *(data_cur - 1);
			int up = *data_prev;
			int up_left = *(data_prev-1);
			int up_right = *(data_prev + 1);
			std::vector<int> neighbors;
			auto add_neighbor = [&](int neiLabel) {
				if (neiLabel != 0) {
					neighbors.push_back(find(neiLabel, parent));
				}
			};

			// 检查已处理邻域
			if(up > 1)
				add_neighbor(up);        // 上
			if( (left > 1) && (left != up))
				add_neighbor(left);        // 左

			if (connectivity == 8) 
			{
				if( (up_left > 1) && (up_left != up) && (up_left != left))
					add_neighbor(up_left); // 左上
				if( (up_right > 1) && (up_right != up) && (up_right != left) && (up_right != up_left))
					add_neighbor(up_right); // 右上
			}

			if (neighbors.empty()) { // 新连通域
				*data_cur = label_cnt++;
			}
			else { // 合并邻域
				int min_root = *std::min_element(neighbors.begin(), neighbors.end());
				*data_cur = min_root;
				for (int root : neighbors) 
				{
					if (root != min_root) 
					{
						unionSet(root, min_root, parent);
					}
				}
			}
		}
		data_cur++;
		data_prev++;
	}

	for (int i = 2; i < label_cnt; i++)
		parent[i] = find(parent[i], parent);

	data_cur = (int*)labImg.data;
	for (int i = 0; i < labImg.rows; i++)
	{
		for (int j = 0; j < labImg.cols; j++)
		{
			if (*data_cur > 1)
			{
				*data_cur = parent[*data_cur];
			}
			data_cur++;
		}
	}

	std::vector<SSG_Region*> labelInfo;
	labelInfo.resize(label_cnt, nullptr);
	// （可选）重新映射为连续标签
	std::unordered_map<int, int> label_map;
	int new_label = 2;
	data_cur = (int*)labImg.data;
	for (int i = 0; i < labImg.rows; i++)
	{
		for (int j = 0; j < labImg.cols; j++)
		{
			if (j == 69)
				int kkk = 1;
			int lbl = *data_cur;
			if (lbl > 1)
			{
				if (label_map.find(lbl) == label_map.end())
				{
					label_map[lbl] = new_label++;
				}
				*data_cur = label_map[lbl];
				//统计Region信息
				SSG_Region* info_cur = (SSG_Region*)labelInfo[*data_cur];
				if (nullptr == info_cur)
				{
					SSG_Region new_rgn = { {j,j,i,i}, 1, *data_cur };
					labelRgns.push_back(new_rgn); //push_back()后，vector中内存单元可能会被改动
					for (int m = 0; m < labelRgns.size(); m++)
					{
						info_cur = &labelRgns[m];
						labelInfo[info_cur->labelID] = info_cur;
					}
				}
				else
				{
					assert(*data_cur == info_cur->labelID);
					if (info_cur->roi.left > j)
						info_cur->roi.left = j;
					if (info_cur->roi.right < j)
						info_cur->roi.right = j;
					if (info_cur->roi.top > i)
						info_cur->roi.top = i;
					if (info_cur->roi.bottom < i)
						info_cur->roi.bottom = i;
					info_cur->ptCounter++;
				}
			}
			data_cur++;
		}
	}
}
#endif


// 函数：从平面法向量计算欧拉角（ZYX顺序）
SSG_EulerAngles planeNormalToEuler(double A, double B, double C) {
	SSG_EulerAngles angles = { 0, 0, 0 };

	// 1. 归一化法向量
	double length = std::sqrt(A * A + B * B + C * C);
	if (length < 1e-7)
		return angles;
	double nx = A / length;
	double ny = B / length;
	double nz = C / length;

	// 2. 计算俯仰角（绕Y轴）
	angles.pitch = std::asin(nx) * (180.0 / M_PI); // 转为度数

	// 3. 计算Roll（绕X轴）
	const double cos_pitch = std::sqrt(1 - nx * nx);  // 等价于cos(pitch)
	if (cos_pitch > 1e-7) {
		// 当cos_pitch非零时，用atan2计算Roll
		angles.roll = std::asin(-ny/ cos_pitch) * (180.0 / M_PI);
	}
	else {
		// 当Pitch接近±π/2时，Roll无法确定，设为0
		angles.roll = 0.0;
	}

	// 4. 假设yaw为0（绕Z轴）
	angles.yaw= 0.0;

	return angles;
}

// 定义3x3旋转矩阵结构体
struct RotationMatrix {
	double data[3][3]; // 行优先存储 [row][col]
};

// 将角度转换为弧度
inline double degreesToRadians(double degrees) {
	return degrees * M_PI / 180.0;
}

// 从欧拉角计算旋转矩阵 (ZYX顺序: 偏航Z -> 俯仰Y -> 横滚X)
RotationMatrix eulerToRotationMatrix(double yaw_deg, double pitch_deg, double roll_deg) {
	RotationMatrix R;

	// 角度转弧度
	double yaw = degreesToRadians(yaw_deg);
	double pitch = degreesToRadians(pitch_deg);
	double roll = degreesToRadians(roll_deg);

	// 预计算三角函数
	double cy = cos(yaw);
	double sy = sin(yaw);
	double cp = cos(pitch);
	double sp = sin(pitch);
	double cr = cos(roll);
	double sr = sin(roll);

	// 计算旋转矩阵元素（ZYX顺序 = Rz * Ry * Rx）
	R.data[0][0] = cy * cp;
	R.data[0][1] = cy * sp * sr - sy * cr;
	R.data[0][2] = cy * sp * cr + sy * sr;

	R.data[1][0] = sy * cp;
	R.data[1][1] = sy * sp * sr + cy * cr;
	R.data[1][2] = sy * sp * cr - cy * sr;

	R.data[2][0] = -sp;
	R.data[2][1] = cp * sr;
	R.data[2][2] = cp * cr;

	return R;
}



// 定义三维向量结构体
struct Vector3 {
	double x, y, z;
	Vector3(double x_, double y_, double z_) : x(x_), y(y_), z(z_) {}
};

// 定义四元数结构体
struct Quaternion {
	double w, x, y, z;
	Quaternion(double w_, double x_, double y_, double z_)
		: w(w_), x(x_), y(y_), z(z_) {}
};

// 计算两个向量的旋转四元数
Quaternion rotationBetweenVectors(const Vector3& a, const Vector3& b) {
	// 归一化输入向量
	const double eps = 1e-7;
	double a_len = std::sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
	double b_len = std::sqrt(b.x * b.x + b.y * b.y + b.z * b.z);

	if (a_len < eps || b_len < eps) {
		// 零向量无法定义旋转，返回单位四元数
		return Quaternion(1.0, 0.0, 0.0, 0.0);
	}

	Vector3 a_norm(a.x / a_len, a.y / a_len, a.z / a_len);
	Vector3 b_norm(b.x / b_len, b.y / b_len, b.z / b_len);

	double cos_theta = a_norm.x * b_norm.x + a_norm.y * b_norm.y + a_norm.z * b_norm.z;

	// 处理共线情况
	if (cos_theta > 1.0 - eps) {
		// 向量方向相同，无需旋转
		return Quaternion(1.0, 0.0, 0.0, 0.0);
	}
	else if (cos_theta < -1.0 + eps) {
		// 向量方向相反，绕任意垂直轴旋转180度
		Vector3 axis(1.0, 0.0, 0.0); // 默认选择X轴
		if (std::abs(a_norm.y) < eps && std::abs(a_norm.z) < eps) {
			// 如果a接近X轴，则选择Y轴作为旋转轴
			axis = Vector3(0.0, 1.0, 0.0);
		}
		return Quaternion(0.0, axis.x, axis.y, axis.z); // 180度旋转
	}

	// 计算旋转轴和半角
	Vector3 axis = Vector3(
		a_norm.y * b_norm.z - a_norm.z * b_norm.y,
		a_norm.z * b_norm.x - a_norm.x * b_norm.z,
		a_norm.x * b_norm.y - a_norm.y * b_norm.x
	);

	double axis_len = std::sqrt(axis.x * axis.x + axis.y * axis.y + axis.z * axis.z);
	if (axis_len < eps) { // 防止除零
		return Quaternion(1.0, 0.0, 0.0, 0.0);
	}
	axis.x /= axis_len;
	axis.y /= axis_len;
	axis.z /= axis_len;

	double half_cos = std::sqrt(0.5 * (1.0 + cos_theta));
	double half_sin = std::sqrt(0.5 * (1.0 - cos_theta));

	return Quaternion(
		half_cos,
		half_sin * axis.x,
		half_sin * axis.y,
		half_sin * axis.z
	);
}

void quaternionToMatrix(const Quaternion& q, double mat[3][3]) {
	double xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z;
	double xy = q.x * q.y, xz = q.x * q.z, yz = q.y * q.z;
	double wx = q.w * q.x, wy = q.w * q.y, wz = q.w * q.z;

	mat[0][0] = 1 - 2 * (yy + zz);
	mat[0][1] = 2 * (xy - wz);
	mat[0][2] = 2 * (xz + wy);

	mat[1][0] = 2 * (xy + wz);
	mat[1][1] = 1 - 2 * (xx + zz);
	mat[1][2] = 2 * (yz - wx);

	mat[2][0] = 2 * (xz - wy);
	mat[2][1] = 2 * (yz + wx);
	mat[2][2] = 1 - 2 * (xx + yy);
}

//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara sg_getPlaneCalibPara(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum)
{
	//设置初始结果
	double initCalib[9]= {
		1.0, 0.0, 0.0,
		0.0, 1.0, 0.0,
		0.0, 0.0, 1.0 };
	SSG_planeCalibPara planePara;
	for (int i = 0; i < 9; i++)
		planePara.planeCalib[i] = initCalib[i];
	planePara.planeHeight = -1.0;

	//统计z范围
	SVzNLRangeD zRange = { 0, -1 }; //< Z范围
	for (int line = 0; line < lineNum; line++)
	{
		for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
			if (pt3D->pt3D.z < 1e-4)
				continue;
			//z
			if (zRange.max < zRange.min)
			{
				zRange.min = pt3D->pt3D.z;
				zRange.max = pt3D->pt3D.z;
			}
			else
			{
				if (zRange.min > pt3D->pt3D.z)
					zRange.min = pt3D->pt3D.z;
				if (zRange.max < pt3D->pt3D.z)
					zRange.max = pt3D->pt3D.z;
			}
		}
	}

	//在Z方向进行统计，取第一个极值
	//以mm为单位，简化量化
	int zHistSize = (int)(zRange.max - zRange.min) + 1;
	if (zHistSize == 0)
		return planePara;

	std::vector<int> zHist;
	zHist.resize(zHistSize);
	int totalPntSize = 0;
	for (int line = 0; line < lineNum; line++)
	{
		for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
			if (pt3D->pt3D.z < 1e-4)
				continue;

			totalPntSize++;
			int histPos = (int)(pt3D->pt3D.z - zRange.min);
			zHist[histPos] ++;
		}
	}
	std::vector<int> zSumHist;
	zSumHist.resize(zHistSize);
	bool isSame = true;
	//以厘米为单位进行累加
	for (int i = 0; i < zHistSize; i++)
	{
		int sumValue = 0;
		for (int j = i - 5; j <= i + 5; j++)
		{
			if ((j >= 0) && (j < zHistSize))
				sumValue += zHist[j];
		}
		zSumHist[i] = sumValue;
		if (i > 0)
		{
			if (sumValue != zSumHist[i - 1])
				isSame = false;
		}
	}
	if(true == isSame)
	{
		//不进行累加（如果累加，累加值相等）
		for (int i = 0; i < zHistSize; i++)
			zSumHist[i] = zHist[i];
	}

	//寻找极值
	int _state = 0;
	int pre_i = -1;
	int sEdgePtIdx = -1;
	int eEdgePtIdx = -1;
	int pre_data = -1;
	std::vector< SSG_intPair> pkTop;
	std::vector< SSG_intPair> pkBtm;
	std::vector<int> pkBtmBackIndexing;
	pkBtmBackIndexing.resize(zHistSize);
	for (int i = 0; i < zHistSize; i++)
		pkBtmBackIndexing[i] = -1;


	for (int i = 0; i < zHistSize; i++)
	{
		int curr_data = zSumHist[i];
		if (pre_data < 0)
		{
			sEdgePtIdx = i;
			eEdgePtIdx = i;
			pre_data = curr_data;
			pre_i = i;
			continue;
		}

		eEdgePtIdx = i;
		double z_diff = curr_data - pre_data;
		switch (_state)
		{
		case 0:  //初态
			if (z_diff < 0) //下降
			{
				_state = 2;
			}
			else if (z_diff > 0) //上升
			{
				_state = 1;
			}
			break;
		case 1: //上升
			if (z_diff < 0)   //下降
			{
				pkTop.push_back({pre_i, pre_data});
				_state = 2;
			}
			else  if(i == (zHistSize-1))
				pkTop.push_back({ i, curr_data });
			break;
		case 2: //下降
			if (z_diff > 0)   //  上升
			{
				int pkBtmIdx = (int)pkBtm.size();
				pkBtmBackIndexing[pre_i] = pkBtmIdx;
				pkBtm.push_back({ pre_i, pre_data });
				_state = 1;
			}
			else if (i == (zHistSize - 1))
			{
				int pkBtmIdx = (int)pkBtm.size();
				pkBtmBackIndexing[i] = pkBtmIdx;
				pkBtm.push_back({ i, curr_data });
			}
			break;
		default:
			_state = 0;
			break;
		}
		pre_data = curr_data;
		pre_i = i;
	}
	//寻找第一个超过总点数1/3的极值点
	if (pkTop.size() < 1)
		return planePara;

	int pntSizeTh = totalPntSize / 10;
	SSG_intPair* vldPeak = NULL;
	for (int i = 0, i_max = (int)pkTop.size(); i < i_max; i++)
	{
		if (pkTop[i].data_1 > pntSizeTh)
		{
			vldPeak = &pkTop[i];
			break;
		}
	}
	if (NULL == vldPeak)
		return planePara;

	//寻找开始和结束位置
	//向前向后寻找
	int preBtmIdx = -1;
	for (int j = vldPeak->data_0 - 1; j >= 0; j--)
	{
		if (pkBtmBackIndexing[j] >= 0)
		{
			int idx = pkBtmBackIndexing[j];
			if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2))
			{
				preBtmIdx = j;
				break;
			}
		}
	}
	int postBtmIdx = -1;
	for (int j = vldPeak->data_0 + 1; j <zHistSize; j++)
	{
		if (pkBtmBackIndexing[j] >= 0)
		{
			int idx = pkBtmBackIndexing[j];
			if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2))
			{
				postBtmIdx = j;
				break;
			}
		}
	}

	SVzNLRangeD topZRange;
	if (preBtmIdx < 0)
		topZRange.min = zRange.min;
	else
		topZRange.min = (float)preBtmIdx + zRange.min;
	if (postBtmIdx < 0)
		topZRange.max = zRange.max;
	else
		topZRange.max = (float)postBtmIdx + zRange.min;

	//取数据
	std::vector<cv::Point3f> Points3ds;
	for (int line = 0; line < lineNum; line++)
	{
		for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
			if (pt3D->pt3D.z < 1e-4)
				continue;

			if ((pt3D->pt3D.z >= topZRange.min) && (pt3D->pt3D.z <= topZRange.max))
			{
				cv::Point3f a_vldPt;
				a_vldPt.x = (float)pt3D->pt3D.x;
				a_vldPt.y = (float)pt3D->pt3D.y;
				a_vldPt.z = (float)pt3D->pt3D.z;
				Points3ds.push_back(a_vldPt);
			}
		}
	}
	//平面拟合
	std::vector<double> planceFunc;
	//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
	vzCaculateLaserPlane(Points3ds, planceFunc);

#if 1 //两个向量的旋转旋转，使用四元数法，
	Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]);
	Vector3 b = Vector3(0, 0, -1.0);
	Quaternion quanPara = rotationBetweenVectors(a, b);

	RotationMatrix rMatrix;
	quaternionToMatrix(quanPara, rMatrix.data);
	//计算反向旋转矩阵
	Quaternion invQuanPara = rotationBetweenVectors(b, a);
	RotationMatrix invMatrix;
	quaternionToMatrix(invQuanPara, invMatrix.data);
#else  //根据平面的法向量计算欧拉角，进而计算旋转矩阵
	//参数计算
	SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
	//反射进行校正
	eulerPra.roll = eulerPra.roll;
	eulerPra.pitch = eulerPra.pitch;
	eulerPra.yaw = eulerPra.yaw;
	RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll);
#endif

	planePara.planeCalib[0] = rMatrix.data[0][0];
	planePara.planeCalib[1] = rMatrix.data[0][1];
	planePara.planeCalib[2] = rMatrix.data[0][2];
	planePara.planeCalib[3] = rMatrix.data[1][0];
	planePara.planeCalib[4] = rMatrix.data[1][1];
	planePara.planeCalib[5] = rMatrix.data[1][2];
	planePara.planeCalib[6] = rMatrix.data[2][0];
	planePara.planeCalib[7] = rMatrix.data[2][1];
	planePara.planeCalib[8] = rMatrix.data[2][2];

	planePara.invRMatrix[0] = invMatrix.data[0][0];
	planePara.invRMatrix[1] = invMatrix.data[0][1];
	planePara.invRMatrix[2] = invMatrix.data[0][2];
	planePara.invRMatrix[3] = invMatrix.data[1][0];
	planePara.invRMatrix[4] = invMatrix.data[1][1];
	planePara.invRMatrix[5] = invMatrix.data[1][2];
	planePara.invRMatrix[6] = invMatrix.data[2][0];
	planePara.invRMatrix[7] = invMatrix.data[2][1];
	planePara.invRMatrix[8] = invMatrix.data[2][2];

#if 0  //test: 两个矩阵的乘积必须是单位阵
	double testMatrix[3][3];
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			testMatrix[i][j] = 0;
			for (int m = 0; m < 3; m++)
				testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j];
		}
	}
#endif
	//数据进行转换
	SVzNLRangeD calibZRange = { 0, -1 };
	topZRange = { 0, -1 };
	for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++)
	{
		//z
		if (topZRange.max < topZRange.min)
		{
			topZRange.min = Points3ds[i].z;
			topZRange.max = Points3ds[i].z;
		}
		else
		{
			if (topZRange.min > Points3ds[i].z)
				topZRange.min = Points3ds[i].z;
			if (topZRange.max < Points3ds[i].z)
				topZRange.max = Points3ds[i].z;
		}
		cv::Point3f a_calibPt;
		a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]);
		a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]);
		a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]);
		//z
		if (calibZRange.max < calibZRange.min)
		{
			calibZRange.min = a_calibPt.z;
			calibZRange.max = a_calibPt.z;
		}
		else
		{
			if (calibZRange.min > a_calibPt.z)
				calibZRange.min = a_calibPt.z;
			if (calibZRange.max < a_calibPt.z)
				calibZRange.max = a_calibPt.z;
		}
	}
	planePara.planeHeight = calibZRange.min;

	return planePara;
}

SSG_planeCalibPara adjustPlaneToXYPlane(double plane_A, double plane_B, double plane_C)
{
	SSG_planeCalibPara calibPara;

	//两个向量的旋转旋转，使用四元数法，
	Vector3 a = Vector3(plane_A, plane_B, plane_C);
	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);


	calibPara.planeCalib[0] = rMatrix.data[0][0];
	calibPara.planeCalib[1] = rMatrix.data[0][1];
	calibPara.planeCalib[2] = rMatrix.data[0][2];
	calibPara.planeCalib[3] = rMatrix.data[1][0];
	calibPara.planeCalib[4] = rMatrix.data[1][1];
	calibPara.planeCalib[5] = rMatrix.data[1][2];
	calibPara.planeCalib[6] = rMatrix.data[2][0];
	calibPara.planeCalib[7] = rMatrix.data[2][1];
	calibPara.planeCalib[8] = rMatrix.data[2][2];

	calibPara.invRMatrix[0] = invMatrix.data[0][0];
	calibPara.invRMatrix[1] = invMatrix.data[0][1];
	calibPara.invRMatrix[2] = invMatrix.data[0][2];
	calibPara.invRMatrix[3] = invMatrix.data[1][0];
	calibPara.invRMatrix[4] = invMatrix.data[1][1];
	calibPara.invRMatrix[5] = invMatrix.data[1][2];
	calibPara.invRMatrix[6] = invMatrix.data[2][0];
	calibPara.invRMatrix[7] = invMatrix.data[2][1];
	calibPara.invRMatrix[8] = invMatrix.data[2][2];

	return calibPara;
}

//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara sg_getPlaneCalibPara2(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	//设置初始结果
	double initCalib[9] = {
		1.0, 0.0, 0.0,
		0.0, 1.0, 0.0,
		0.0, 0.0, 1.0 };
	SSG_planeCalibPara planePara;
	for (int i = 0; i < 9; i++)
		planePara.planeCalib[i] = initCalib[i];
	planePara.planeHeight = -1.0;

	int lineNum = (int)scanLines.size();
	//统计z范围
	SVzNLRangeD zRange = { 0, -1 }; //< Z范围
	for (int line = 0; line < lineNum; line++)
	{
		int nPositionCnt = (int)scanLines[line].size();
		for (int i = 0; i < nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &scanLines[line][i];
			if (pt3D->pt3D.z < 1e-4)
				continue;
			//z
			if (zRange.max < zRange.min)
			{
				zRange.min = pt3D->pt3D.z;
				zRange.max = pt3D->pt3D.z;
			}
			else
			{
				if (zRange.min > pt3D->pt3D.z)
					zRange.min = pt3D->pt3D.z;
				if (zRange.max < pt3D->pt3D.z)
					zRange.max = pt3D->pt3D.z;
			}
		}
	}

	//在Z方向进行统计，取第一个极值
	//以mm为单位，简化量化
	int zHistSize = (int)(zRange.max - zRange.min) + 1;
	if (zHistSize == 0)
		return planePara;

	std::vector<int> zHist;
	zHist.resize(zHistSize);
	int totalPntSize = 0;
	for (int line = 0; line < lineNum; line++)
	{
		int nPositionCnt = (int)scanLines[line].size();
		for (int i = 0; i < nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &scanLines[line][i];
			if (pt3D->pt3D.z < 1e-4)
				continue;

			totalPntSize++;
			int histPos = (int)(pt3D->pt3D.z - zRange.min);
			zHist[histPos] ++;
		}
	}
	std::vector<int> zSumHist;
	zSumHist.resize(zHistSize);
	bool isSame = true;
	//以厘米为单位进行累加
	for (int i = 0; i < zHistSize; i++)
	{
		int sumValue = 0;
		for (int j = i - 5; j <= i + 5; j++)
		{
			if ((j >= 0) && (j < zHistSize))
				sumValue += zHist[j];
		}
		zSumHist[i] = sumValue;
		if (i > 0)
		{
			if (sumValue != zSumHist[i - 1])
				isSame = false;
		}
	}
	if (true == isSame)
	{
		//不进行累加（如果累加，累加值相等）
		for (int i = 0; i < zHistSize; i++)
			zSumHist[i] = zHist[i];
	}

	//寻找极值
	int _state = 0;
	int pre_i = -1;
	int sEdgePtIdx = -1;
	int eEdgePtIdx = -1;
	int pre_data = -1;
	std::vector< SSG_intPair> pkTop;
	std::vector< SSG_intPair> pkBtm;
	std::vector<int> pkBtmBackIndexing;
	pkBtmBackIndexing.resize(zHistSize);
	for (int i = 0; i < zHistSize; i++)
		pkBtmBackIndexing[i] = -1;


	for (int i = 0; i < zHistSize; i++)
	{
		int curr_data = zSumHist[i];
		if (pre_data < 0)
		{
			sEdgePtIdx = i;
			eEdgePtIdx = i;
			pre_data = curr_data;
			pre_i = i;
			continue;
		}

		eEdgePtIdx = i;
		double z_diff = curr_data - pre_data;
		switch (_state)
		{
		case 0:  //初态
			if (z_diff < 0) //下降
			{
				_state = 2;
			}
			else if (z_diff > 0) //上升
			{
				_state = 1;
			}
			break;
		case 1: //上升
			if (z_diff < 0)   //下降
			{
				pkTop.push_back({ pre_i, pre_data });
				_state = 2;
			}
			else  if (i == (zHistSize - 1))
				pkTop.push_back({ i, curr_data });
			break;
		case 2: //下降
			if (z_diff > 0)   //  上升
			{
				int pkBtmIdx = (int)pkBtm.size();
				pkBtmBackIndexing[pre_i] = pkBtmIdx;
				pkBtm.push_back({ pre_i, pre_data });
				_state = 1;
			}
			else if (i == (zHistSize - 1))
			{
				int pkBtmIdx = (int)pkBtm.size();
				pkBtmBackIndexing[i] = pkBtmIdx;
				pkBtm.push_back({ i, curr_data });
			}
			break;
		default:
			_state = 0;
			break;
		}
		pre_data = curr_data;
		pre_i = i;
	}
	//寻找第一个超过总点数1/3的极值点
	if (pkTop.size() < 1)
		return planePara;

	int pntSizeTh = totalPntSize / 10;
	SSG_intPair* vldPeak = NULL;
	for (int i = 0, i_max = (int)pkTop.size(); i < i_max; i++)
	{
		if (pkTop[i].data_1 > pntSizeTh)
		{
			vldPeak = &pkTop[i];
			break;
		}
	}
	if (NULL == vldPeak)
		return planePara;

	//寻找开始和结束位置
	//向前向后寻找
	int preBtmIdx = -1;
	for (int j = vldPeak->data_0 - 1; j >= 0; j--)
	{
		if (pkBtmBackIndexing[j] >= 0)
		{
			int idx = pkBtmBackIndexing[j];
			if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2))
			{
				preBtmIdx = j;
				break;
			}
		}
	}
	int postBtmIdx = -1;
	for (int j = vldPeak->data_0 + 1; j < zHistSize; j++)
	{
		if (pkBtmBackIndexing[j] >= 0)
		{
			int idx = pkBtmBackIndexing[j];
			if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2))
			{
				postBtmIdx = j;
				break;
			}
		}
	}

	SVzNLRangeD topZRange;
	if (preBtmIdx < 0)
		topZRange.min = zRange.min;
	else
		topZRange.min = (float)preBtmIdx + zRange.min;
	if (postBtmIdx < 0)
		topZRange.max = zRange.max;
	else
		topZRange.max = (float)postBtmIdx + zRange.min;

	//取数据
	std::vector<cv::Point3f> Points3ds;
	for (int line = 0; line < lineNum; line++)
	{
		int nPositionCnt = (int)scanLines[line].size();
		for (int i = 0; i < nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &scanLines[line][i];
			if (pt3D->pt3D.z < 1e-4)
				continue;

			if ((pt3D->pt3D.z >= topZRange.min) && (pt3D->pt3D.z <= topZRange.max))
			{
				cv::Point3f a_vldPt;
				a_vldPt.x = (float)pt3D->pt3D.x;
				a_vldPt.y = (float)pt3D->pt3D.y;
				a_vldPt.z = (float)pt3D->pt3D.z;
				Points3ds.push_back(a_vldPt);
			}
		}
	}
	//平面拟合
	std::vector<double> planceFunc;
	vzCaculateLaserPlane(Points3ds, planceFunc);

#if 1 //两个向量的旋转旋转，使用四元数法，
	Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]);
	Vector3 b = Vector3(0, 0, -1.0);
	Quaternion quanPara = rotationBetweenVectors(a, b);

	RotationMatrix rMatrix;
	quaternionToMatrix(quanPara, rMatrix.data);
	//计算反向旋转矩阵
	Quaternion invQuanPara = rotationBetweenVectors(b, a);
	RotationMatrix invMatrix;
	quaternionToMatrix(invQuanPara, invMatrix.data);
#else  //根据平面的法向量计算欧拉角，进而计算旋转矩阵
	//参数计算
	SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
	//反射进行校正
	eulerPra.roll = eulerPra.roll;
	eulerPra.pitch = eulerPra.pitch;
	eulerPra.yaw = eulerPra.yaw;
	RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll);
#endif

	planePara.planeCalib[0] = rMatrix.data[0][0];
	planePara.planeCalib[1] = rMatrix.data[0][1];
	planePara.planeCalib[2] = rMatrix.data[0][2];
	planePara.planeCalib[3] = rMatrix.data[1][0];
	planePara.planeCalib[4] = rMatrix.data[1][1];
	planePara.planeCalib[5] = rMatrix.data[1][2];
	planePara.planeCalib[6] = rMatrix.data[2][0];
	planePara.planeCalib[7] = rMatrix.data[2][1];
	planePara.planeCalib[8] = rMatrix.data[2][2];

	planePara.invRMatrix[0] = invMatrix.data[0][0];
	planePara.invRMatrix[1] = invMatrix.data[0][1];
	planePara.invRMatrix[2] = invMatrix.data[0][2];
	planePara.invRMatrix[3] = invMatrix.data[1][0];
	planePara.invRMatrix[4] = invMatrix.data[1][1];
	planePara.invRMatrix[5] = invMatrix.data[1][2];
	planePara.invRMatrix[6] = invMatrix.data[2][0];
	planePara.invRMatrix[7] = invMatrix.data[2][1];
	planePara.invRMatrix[8] = invMatrix.data[2][2];

#if 0  //test: 两个矩阵的乘积必须是单位阵
	double testMatrix[3][3];
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			testMatrix[i][j] = 0;
			for (int m = 0; m < 3; m++)
				testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j];
		}
	}
#endif
	//数据进行转换
	SVzNLRangeD calibZRange = { 0, -1 };
	topZRange = { 0, -1 };
	double sumMeanZ = 0;
	int sumSize = 0;
	for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++)
	{
		//z
		if (topZRange.max < topZRange.min)
		{
			topZRange.min = Points3ds[i].z;
			topZRange.max = Points3ds[i].z;
		}
		else
		{
			if (topZRange.min > Points3ds[i].z)
				topZRange.min = Points3ds[i].z;
			if (topZRange.max < Points3ds[i].z)
				topZRange.max = Points3ds[i].z;
		}
		cv::Point3f a_calibPt;
		a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]);
		a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]);
		a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]);
		//z
		if (calibZRange.max < calibZRange.min)
		{
			calibZRange.min = a_calibPt.z;
			calibZRange.max = a_calibPt.z;
			sumMeanZ += a_calibPt.z;
			sumSize++;
		}
		else
		{
			if (calibZRange.min > a_calibPt.z)
				calibZRange.min = a_calibPt.z;
			if (calibZRange.max < a_calibPt.z)
				calibZRange.max = a_calibPt.z;
			sumMeanZ += a_calibPt.z;
			sumSize++;
		}
	}
	if (sumSize > 0)
		sumMeanZ = sumMeanZ / (double)sumSize;
	planePara.planeHeight = sumMeanZ; // calibZRange.min;

	return planePara;
}

//针对孔板计算一个平面调平参数：提取不经过孔的扫描线进行平面拟合
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara sg_getHolePlaneCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	//设置初始结果
	double initCalib[9] = {
		1.0, 0.0, 0.0,
		0.0, 1.0, 0.0,
		0.0, 0.0, 1.0 };
	SSG_planeCalibPara planePara;
	for (int i = 0; i < 9; i++)
		planePara.planeCalib[i] = initCalib[i];
	planePara.planeHeight = -1.0;

	SSG_lineSegParam segParam;
	segParam.segGapTh_y = 1.0;
	segParam.segGapTh_z = 1.0;
	int lineNum = (int)scanLines.size();
	std::vector<int> validLines;
	for (int line = 0; line < lineNum; line++)
	{
		//去除零点
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(scanLines[line], segParam, segs);
		if (segs.size() == 1)
			validLines.push_back(line);
	}

	//取数据
	std::vector<cv::Point3f> Points3ds;
	for (int vline = 0; vline < (int)validLines.size(); vline++)
	{
		int line = validLines[vline];
		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;

			cv::Point3f a_vldPt;
			a_vldPt.x = (float)pt3D->pt3D.x;
			a_vldPt.y = (float)pt3D->pt3D.y;
			a_vldPt.z = (float)pt3D->pt3D.z;
			Points3ds.push_back(a_vldPt);
		}
	}
	//平面拟合
	std::vector<double> planceFunc;
	vzCaculateLaserPlane(Points3ds, planceFunc);

#if 1 //两个向量的旋转旋转，使用四元数法，
	Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]);
	Vector3 b = Vector3(0, 0, -1.0);
	Quaternion quanPara = rotationBetweenVectors(a, b);

	RotationMatrix rMatrix;
	quaternionToMatrix(quanPara, rMatrix.data);
	//计算反向旋转矩阵
	Quaternion invQuanPara = rotationBetweenVectors(b, a);
	RotationMatrix invMatrix;
	quaternionToMatrix(invQuanPara, invMatrix.data);
#else  //根据平面的法向量计算欧拉角，进而计算旋转矩阵
	//参数计算
	SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
	//反射进行校正
	eulerPra.roll = eulerPra.roll;
	eulerPra.pitch = eulerPra.pitch;
	eulerPra.yaw = eulerPra.yaw;
	RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll);
#endif

	planePara.planeCalib[0] = rMatrix.data[0][0];
	planePara.planeCalib[1] = rMatrix.data[0][1];
	planePara.planeCalib[2] = rMatrix.data[0][2];
	planePara.planeCalib[3] = rMatrix.data[1][0];
	planePara.planeCalib[4] = rMatrix.data[1][1];
	planePara.planeCalib[5] = rMatrix.data[1][2];
	planePara.planeCalib[6] = rMatrix.data[2][0];
	planePara.planeCalib[7] = rMatrix.data[2][1];
	planePara.planeCalib[8] = rMatrix.data[2][2];

	planePara.invRMatrix[0] = invMatrix.data[0][0];
	planePara.invRMatrix[1] = invMatrix.data[0][1];
	planePara.invRMatrix[2] = invMatrix.data[0][2];
	planePara.invRMatrix[3] = invMatrix.data[1][0];
	planePara.invRMatrix[4] = invMatrix.data[1][1];
	planePara.invRMatrix[5] = invMatrix.data[1][2];
	planePara.invRMatrix[6] = invMatrix.data[2][0];
	planePara.invRMatrix[7] = invMatrix.data[2][1];
	planePara.invRMatrix[8] = invMatrix.data[2][2];

#if 0  //test: 两个矩阵的乘积必须是单位阵
	double testMatrix[3][3];
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			testMatrix[i][j] = 0;
			for (int m = 0; m < 3; m++)
				testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j];
		}
	}
#endif
	//数据进行转换
	SVzNLRangeD calibZRange = { 0, -1 };
	SVzNLRangeD topZRange = { 0, -1 };
	double sumMeanZ = 0;
	int sumSize = 0;
	for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++)
	{
		//z
		if (topZRange.max < topZRange.min)
		{
			topZRange.min = Points3ds[i].z;
			topZRange.max = Points3ds[i].z;
		}
		else
		{
			if (topZRange.min > Points3ds[i].z)
				topZRange.min = Points3ds[i].z;
			if (topZRange.max < Points3ds[i].z)
				topZRange.max = Points3ds[i].z;
		}
		cv::Point3f a_calibPt;
		a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]);
		a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]);
		a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]);
		//z
		if (calibZRange.max < calibZRange.min)
		{
			calibZRange.min = a_calibPt.z;
			calibZRange.max = a_calibPt.z;
			sumMeanZ += a_calibPt.z;
			sumSize++;
		}
		else
		{
			if (calibZRange.min > a_calibPt.z)
				calibZRange.min = a_calibPt.z;
			if (calibZRange.max < a_calibPt.z)
				calibZRange.max = a_calibPt.z;
			sumMeanZ += a_calibPt.z;
			sumSize++;
		}
	}
	if (sumSize > 0)
		sumMeanZ = sumMeanZ / (double)sumSize;
	planePara.planeHeight = sumMeanZ; // calibZRange.min;

	return planePara;
}

//水平安装相机垂直扫描模式地面调平
SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	//设置初始结果
	double initCalib[9] = {
		1.0, 0.0, 0.0,
		0.0, 1.0, 0.0,
		0.0, 0.0, 1.0 };
	SSG_planeCalibPara planePara;
	for (int i = 0; i < 9; i++)
		planePara.planeCalib[i] = initCalib[i];
	planePara.planeHeight = -1.0;

	//提取地面直线段
	SSG_lineSegParam lineSegPara;
	lineSegPara.distScale = 2.0;
	lineSegPara.segGapTh_y = 5.0;  //y方向间隔大于5mm认为是分段
	lineSegPara.segGapTh_z = 10.0; //z方向间隔大于10mm认为是分段

	std::vector<cv::Point3f> groundPts;
	int lineNum = (int)scanLines.size();
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
		int dataSize = (int)lineData.size();
		//去除零点
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(lineData, lineSegPara, segs);
		if (segs.size() == 0)
			continue;

		//对最后一段进行处理
		SSG_RUN lastSeg = segs.back();
		//直线分割
		std::vector< SSG_RUN> segmentationLines;
		split(lastSeg, lineData, lineSegPara.distScale, segmentationLines);
		//检查最后一段的直线段的斜率
		SSG_RUN lastLine = segmentationLines.back();
		//计算斜率
		int startIdx = lastLine.start;
		int endIdx = lastLine.start + lastLine.len - 1;
		double dy = abs(lineData[endIdx].pt3D.y - lineData[startIdx].pt3D.y) + 1e-8;  //加扰，防止dy为0
		double dz = lineData[startIdx].pt3D.z - lineData[endIdx].pt3D.z;
		if (dz > 0)
		{
			double tan_k = dz / dy;
			if (tan_k > tan(PI / 3))  //大于60度，合格
			{
				for (int i = startIdx; i <= endIdx; i++)
				{
					if (lineData[i].pt3D.z > 1e-4)
					{
						lineData[i].nPointIdx = 1;
						cv::Point3f a_pt = cv::Point3f(lineData[i].pt3D.x, lineData[i].pt3D.y, lineData[i].pt3D.z);
						groundPts.push_back(a_pt);
					}
				}
			}
		}
	}

	//平面拟合
	std::vector<double> planceFunc;
	vzCaculateLaserPlane(groundPts, planceFunc);

#if 1 //两个向量的旋转旋转，使用四元数法，
	Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]);
	Vector3 b1 = Vector3(0, 1.0, 0);
	Vector3 b2 = Vector3(0, -1.0, 0);
	Quaternion quanPara_1 = rotationBetweenVectors(a, b1);
	Quaternion quanPara_2 = rotationBetweenVectors(a, b2);
	RotationMatrix rMatrix_1;
	quaternionToMatrix(quanPara_1, rMatrix_1.data);
	RotationMatrix rMatrix_2;
	quaternionToMatrix(quanPara_2, rMatrix_2.data);
	//计算反向旋转矩阵

	Quaternion invQuanPara_1 = rotationBetweenVectors(b1, a);
	Quaternion invQuanPara_2 = rotationBetweenVectors(b2, a);
	RotationMatrix invMatrix_1;
	quaternionToMatrix(invQuanPara_1, invMatrix_1.data);
	RotationMatrix invMatrix_2;
	quaternionToMatrix(invQuanPara_2, invMatrix_2.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_1.data[0][0];
	planePara.planeCalib[1] = rMatrix_1.data[0][1];
	planePara.planeCalib[2] = rMatrix_1.data[0][2];
	planePara.planeCalib[3] = rMatrix_1.data[1][0];
	planePara.planeCalib[4] = rMatrix_1.data[1][1];
	planePara.planeCalib[5] = rMatrix_1.data[1][2];
	planePara.planeCalib[6] = rMatrix_1.data[2][0];
	planePara.planeCalib[7] = rMatrix_1.data[2][1];
	planePara.planeCalib[8] = rMatrix_1.data[2][2];

	planePara.invRMatrix[0] = invMatrix_1.data[0][0];
	planePara.invRMatrix[1] = invMatrix_1.data[0][1];
	planePara.invRMatrix[2] = invMatrix_1.data[0][2];
	planePara.invRMatrix[3] = invMatrix_1.data[1][0];
	planePara.invRMatrix[4] = invMatrix_1.data[1][1];
	planePara.invRMatrix[5] = invMatrix_1.data[1][2];
	planePara.invRMatrix[6] = invMatrix_1.data[2][0];
	planePara.invRMatrix[7] = invMatrix_1.data[2][1];
	planePara.invRMatrix[8] = invMatrix_1.data[2][2];

	//数据进行转换
	SVzNLRangeD calibYRange = { 0, -1 };
	SVzNLRangeD topYRange = { 0, -1 };
	double sumMeanY = 0;
	int sumSize = 0;
	for (int i = 0, i_max = (int)groundPts.size(); i < i_max; i++)
	{

		cv::Point3f a_calibPt;
		a_calibPt.x = (float)(groundPts[i].x * planePara.planeCalib[0] + groundPts[i].y * planePara.planeCalib[1] + groundPts[i].z * planePara.planeCalib[2]);
		a_calibPt.y = (float)(groundPts[i].x * planePara.planeCalib[3] + groundPts[i].y * planePara.planeCalib[4] + groundPts[i].z * planePara.planeCalib[5]);
		a_calibPt.z = (float)(groundPts[i].x * planePara.planeCalib[6] + groundPts[i].y * planePara.planeCalib[7] + groundPts[i].z * planePara.planeCalib[8]);
		//z
		if (calibYRange.max < calibYRange.min)
		{
			calibYRange.min = a_calibPt.y;
			calibYRange.max = a_calibPt.y;
			sumMeanY += a_calibPt.y;
			sumSize++;
		}
		else
		{
			if (calibYRange.min > a_calibPt.y)
				calibYRange.min = a_calibPt.y;
			if (calibYRange.max < a_calibPt.y)
				calibYRange.max = a_calibPt.y;
			sumMeanY += a_calibPt.y;
			sumSize++;
		}
	}
	if (sumSize > 0)
		sumMeanY = sumMeanY / (double)sumSize;

	if (sumMeanY < 0)
	{
		planePara.planeCalib[0] = rMatrix_2.data[0][0];
		planePara.planeCalib[1] = rMatrix_2.data[0][1];
		planePara.planeCalib[2] = rMatrix_2.data[0][2];
		planePara.planeCalib[3] = rMatrix_2.data[1][0];
		planePara.planeCalib[4] = rMatrix_2.data[1][1];
		planePara.planeCalib[5] = rMatrix_2.data[1][2];
		planePara.planeCalib[6] = rMatrix_2.data[2][0];
		planePara.planeCalib[7] = rMatrix_2.data[2][1];
		planePara.planeCalib[8] = rMatrix_2.data[2][2];

		planePara.invRMatrix[0] = invMatrix_2.data[0][0];
		planePara.invRMatrix[1] = invMatrix_2.data[0][1];
		planePara.invRMatrix[2] = invMatrix_2.data[0][2];
		planePara.invRMatrix[3] = invMatrix_2.data[1][0];
		planePara.invRMatrix[4] = invMatrix_2.data[1][1];
		planePara.invRMatrix[5] = invMatrix_2.data[1][2];
		planePara.invRMatrix[6] = invMatrix_2.data[2][0];
		planePara.invRMatrix[7] = invMatrix_2.data[2][1];
		planePara.invRMatrix[8] = invMatrix_2.data[2][2];

		//数据进行转换
		calibYRange = { 0, -1 };
		topYRange = { 0, -1 };
		sumMeanY = 0;
		sumSize = 0;
		for (int i = 0, i_max = (int)groundPts.size(); i < i_max; i++)
		{
			cv::Point3f a_calibPt;
			a_calibPt.x = (float)(groundPts[i].x * planePara.planeCalib[0] + groundPts[i].y * planePara.planeCalib[1] + groundPts[i].z * planePara.planeCalib[2]);
			a_calibPt.y = (float)(groundPts[i].x * planePara.planeCalib[3] + groundPts[i].y * planePara.planeCalib[4] + groundPts[i].z * planePara.planeCalib[5]);
			a_calibPt.z = (float)(groundPts[i].x * planePara.planeCalib[6] + groundPts[i].y * planePara.planeCalib[7] + groundPts[i].z * planePara.planeCalib[8]);
			//z
			if (calibYRange.max < calibYRange.min)
			{
				calibYRange.min = a_calibPt.y;
				calibYRange.max = a_calibPt.y;
				sumMeanY += a_calibPt.y;
				sumSize++;
			}
			else
			{
				if (calibYRange.min > a_calibPt.y)
					calibYRange.min = a_calibPt.y;
				if (calibYRange.max < a_calibPt.y)
					calibYRange.max = a_calibPt.y;
				sumMeanY += a_calibPt.y;
				sumSize++;
			}
		}
		if (sumSize > 0)
			sumMeanY = sumMeanY / (double)sumSize;
	}
	planePara.planeHeight = sumMeanY; // calibZRange.min;
	return planePara;
}

SSG_planeCalibPara wd_computeRTMatrix(SVzNL3DPoint& vector1, SVzNL3DPoint& vector2)
{
	Vector3 a = Vector3(vector1.x, vector1.y, vector1.z);
	Vector3 b = Vector3(vector2.x, vector2.y, vector2.z);
	Quaternion quanPara = rotationBetweenVectors(a, b);

	RotationMatrix rMatrix;
	quaternionToMatrix(quanPara, rMatrix.data);
	//计算反向旋转矩阵
	Quaternion invQuanPara = rotationBetweenVectors(b, a);
	RotationMatrix invMatrix;
	quaternionToMatrix(invQuanPara, invMatrix.data);

	SSG_planeCalibPara calibPara;
	calibPara.planeCalib[0] = rMatrix.data[0][0];
	calibPara.planeCalib[1] = rMatrix.data[0][1];
	calibPara.planeCalib[2] = rMatrix.data[0][2];
	calibPara.planeCalib[3] = rMatrix.data[1][0];
	calibPara.planeCalib[4] = rMatrix.data[1][1];
	calibPara.planeCalib[5] = rMatrix.data[1][2];
	calibPara.planeCalib[6] = rMatrix.data[2][0];
	calibPara.planeCalib[7] = rMatrix.data[2][1];
	calibPara.planeCalib[8] = rMatrix.data[2][2];

	calibPara.invRMatrix[0] = invMatrix.data[0][0];
	calibPara.invRMatrix[1] = invMatrix.data[0][1];
	calibPara.invRMatrix[2] = invMatrix.data[0][2];
	calibPara.invRMatrix[3] = invMatrix.data[1][0];
	calibPara.invRMatrix[4] = invMatrix.data[1][1];
	calibPara.invRMatrix[5] = invMatrix.data[1][2];
	calibPara.invRMatrix[6] = invMatrix.data[2][0];
	calibPara.invRMatrix[7] = invMatrix.data[2][1];
	calibPara.invRMatrix[8] = invMatrix.data[2][2];

	calibPara.planeHeight = 0;
	return calibPara;
}

SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	SVzNL3DRangeD roi)
{
	//设置初始结果
	double initCalib[9] = {
		1.0, 0.0, 0.0,
		0.0, 1.0, 0.0,
		0.0, 0.0, 1.0 };
	SSG_planeCalibPara planePara;
	for (int i = 0; i < 9; i++)
		planePara.planeCalib[i] = initCalib[i];
	planePara.planeHeight = -1.0;

	int lineNum = (int)scanLines.size();
	//取数据
	std::vector<cv::Point3f> Points3ds;
	for (int line = 0; line < lineNum; line++)
	{
		int nPositionCnt = (int)scanLines[line].size();
		for (int i = 0; i < nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &scanLines[line][i];

			if (pt3D->pt3D.z < 1e-4)
				continue;

			bool isValid = false;

			if ((pt3D->pt3D.x >= roi.xRange.min) && (pt3D->pt3D.x <= roi.xRange.max) &&
				(pt3D->pt3D.y >= roi.yRange.min) && (pt3D->pt3D.y <= roi.yRange.max) &&
				(pt3D->pt3D.z >= roi.zRange.min) && (pt3D->pt3D.y <= roi.zRange.max))
			{
				cv::Point3f a_vldPt;
				a_vldPt.x = (float)pt3D->pt3D.x;
				a_vldPt.y = (float)pt3D->pt3D.y;
				a_vldPt.z = (float)pt3D->pt3D.z;
				Points3ds.push_back(a_vldPt);
			}
		}
	}

	//平面拟合
	std::vector<double> planceFunc;
	vzCaculateLaserPlane(Points3ds, planceFunc);

#if 1 //两个向量的旋转旋转，使用四元数法，
	Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]);
	Vector3 b = Vector3(0, 0, -1.0);
	Quaternion quanPara = rotationBetweenVectors(a, b);

	RotationMatrix rMatrix;
	quaternionToMatrix(quanPara, rMatrix.data);
	//计算反向旋转矩阵
	Quaternion invQuanPara = rotationBetweenVectors(b, a);
	RotationMatrix invMatrix;
	quaternionToMatrix(invQuanPara, invMatrix.data);
#else  //根据平面的法向量计算欧拉角，进而计算旋转矩阵
	//参数计算
	SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
	//反射进行校正
	eulerPra.roll = eulerPra.roll;
	eulerPra.pitch = eulerPra.pitch;
	eulerPra.yaw = eulerPra.yaw;
	RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll);
#endif

	planePara.planeCalib[0] = rMatrix.data[0][0];
	planePara.planeCalib[1] = rMatrix.data[0][1];
	planePara.planeCalib[2] = rMatrix.data[0][2];
	planePara.planeCalib[3] = rMatrix.data[1][0];
	planePara.planeCalib[4] = rMatrix.data[1][1];
	planePara.planeCalib[5] = rMatrix.data[1][2];
	planePara.planeCalib[6] = rMatrix.data[2][0];
	planePara.planeCalib[7] = rMatrix.data[2][1];
	planePara.planeCalib[8] = rMatrix.data[2][2];

	planePara.invRMatrix[0] = invMatrix.data[0][0];
	planePara.invRMatrix[1] = invMatrix.data[0][1];
	planePara.invRMatrix[2] = invMatrix.data[0][2];
	planePara.invRMatrix[3] = invMatrix.data[1][0];
	planePara.invRMatrix[4] = invMatrix.data[1][1];
	planePara.invRMatrix[5] = invMatrix.data[1][2];
	planePara.invRMatrix[6] = invMatrix.data[2][0];
	planePara.invRMatrix[7] = invMatrix.data[2][1];
	planePara.invRMatrix[8] = invMatrix.data[2][2];

#if 0  //test: 两个矩阵的乘积必须是单位阵
	double testMatrix[3][3];
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			testMatrix[i][j] = 0;
			for (int m = 0; m < 3; m++)
				testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j];
		}
	}
#endif
	//数据进行转换
	SVzNLRangeD calibZRange = { 0, -1 };
	for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++)
	{
		cv::Point3f a_calibPt;
		a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]);
		a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]);
		a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]);
		//z
		if (calibZRange.max < calibZRange.min)
		{
			calibZRange.min = a_calibPt.z;
			calibZRange.max = a_calibPt.z;
		}
		else
		{
			if (calibZRange.min > a_calibPt.z)
				calibZRange.min = a_calibPt.z;
			if (calibZRange.max < a_calibPt.z)
				calibZRange.max = a_calibPt.z;
		}
	}
	planePara.planeHeight = calibZRange.min;

	return planePara;
}

//计算一个平面调平参数。
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara sg_getPlaneCalibPara_ROIs(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	std::vector<SVzNL3DRangeD>& ROIs)
{
	//设置初始结果
	double initCalib[9] = {
		1.0, 0.0, 0.0,
		0.0, 1.0, 0.0,
		0.0, 0.0, 1.0 };
	SSG_planeCalibPara planePara;
	for (int i = 0; i < 9; i++)
		planePara.planeCalib[i] = initCalib[i];
	planePara.planeHeight = -1.0;

	//取数据
	std::vector<cv::Point3f> Points3ds;
	for (int line = 0; line < lineNum; line++)
	{
		for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];

			if (pt3D->pt3D.z < 1e-4)
				continue;

			bool isValid = false;
			for (int m = 0, m_max = (int)ROIs.size(); m < m_max; m++)
			{
				if ((pt3D->pt3D.x >= ROIs[m].xRange.min) && (pt3D->pt3D.x <= ROIs[m].xRange.max) &&
					(pt3D->pt3D.y >= ROIs[m].yRange.min) && (pt3D->pt3D.y <= ROIs[m].yRange.max) &&
					(pt3D->pt3D.z >= ROIs[m].zRange.min) && (pt3D->pt3D.y <= ROIs[m].zRange.max))
				{
					isValid = true;
					break;
				}
			}
			if (false == isValid)
				continue;

			cv::Point3f a_vldPt;
			a_vldPt.x = (float)pt3D->pt3D.x;
			a_vldPt.y = (float)pt3D->pt3D.y;
			a_vldPt.z = (float)pt3D->pt3D.z;
			Points3ds.push_back(a_vldPt);
		}
	}

	//平面拟合
	std::vector<double> planceFunc;
	vzCaculateLaserPlane(Points3ds, planceFunc);

#if 1 //两个向量的旋转旋转，使用四元数法，
	Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]);
	Vector3 b = Vector3(0, 0, -1.0);
	Quaternion quanPara = rotationBetweenVectors(a, b);

	RotationMatrix rMatrix;
	quaternionToMatrix(quanPara, rMatrix.data);
	//计算反向旋转矩阵
	Quaternion invQuanPara = rotationBetweenVectors(b, a);
	RotationMatrix invMatrix;
	quaternionToMatrix(invQuanPara, invMatrix.data);
#else  //根据平面的法向量计算欧拉角，进而计算旋转矩阵
	//参数计算
	SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
	//反射进行校正
	eulerPra.roll = eulerPra.roll;
	eulerPra.pitch = eulerPra.pitch;
	eulerPra.yaw = eulerPra.yaw;
	RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll);
#endif

	planePara.planeCalib[0] = rMatrix.data[0][0];
	planePara.planeCalib[1] = rMatrix.data[0][1];
	planePara.planeCalib[2] = rMatrix.data[0][2];
	planePara.planeCalib[3] = rMatrix.data[1][0];
	planePara.planeCalib[4] = rMatrix.data[1][1];
	planePara.planeCalib[5] = rMatrix.data[1][2];
	planePara.planeCalib[6] = rMatrix.data[2][0];
	planePara.planeCalib[7] = rMatrix.data[2][1];
	planePara.planeCalib[8] = rMatrix.data[2][2];

	planePara.invRMatrix[0] = invMatrix.data[0][0];
	planePara.invRMatrix[1] = invMatrix.data[0][1];
	planePara.invRMatrix[2] = invMatrix.data[0][2];
	planePara.invRMatrix[3] = invMatrix.data[1][0];
	planePara.invRMatrix[4] = invMatrix.data[1][1];
	planePara.invRMatrix[5] = invMatrix.data[1][2];
	planePara.invRMatrix[6] = invMatrix.data[2][0];
	planePara.invRMatrix[7] = invMatrix.data[2][1];
	planePara.invRMatrix[8] = invMatrix.data[2][2];

#if 0  //test: 两个矩阵的乘积必须是单位阵
	double testMatrix[3][3];
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			testMatrix[i][j] = 0;
			for (int m = 0; m < 3; m++)
				testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j];
		}
	}
#endif
	//数据进行转换
	SVzNLRangeD calibZRange = { 0, -1 };
	for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++)
	{
		cv::Point3f a_calibPt;
		a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]);
		a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]);
		a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]);
		//z
		if (calibZRange.max < calibZRange.min)
		{
			calibZRange.min = a_calibPt.z;
			calibZRange.max = a_calibPt.z;
		}
		else
		{
			if (calibZRange.min > a_calibPt.z)
				calibZRange.min = a_calibPt.z;
			if (calibZRange.max < a_calibPt.z)
				calibZRange.max = a_calibPt.z;
		}
	}
	planePara.planeHeight = calibZRange.min;

	return planePara;
}

// 从旋转矩阵计算欧拉角（Z-Y-X顺序）
SSG_EulerAngles rotationMatrixToEulerZYX(const double R[3][3]) {
	SSG_EulerAngles angles;

	// 计算俯仰角（pitch）θ
	angles.pitch = asin(-R[2][0]);  // asin返回弧度

	// 检查万向节锁（cosθ接近0）
	const double epsilon = 1e-6;
	if (abs(cos(angles.pitch)) > epsilon) {
		// 无万向节锁，正常计算yaw和roll
		angles.yaw = atan2(R[1][0], R[0][0]);
		angles.roll = atan2(R[2][1], R[2][2]);
	}
	else {
		// 万向节锁，约定roll=0，计算yaw
		angles.roll = 0.0;
		angles.yaw = atan2(-R[0][1], R[1][1]);
	}

	// 将弧度转换为角度
	const double rad2deg = 180.0 / M_PI;
	angles.yaw *= rad2deg;
	angles.pitch *= rad2deg;
	angles.roll *= rad2deg;

	return angles;
}

// 从欧拉角计算旋转矩阵（Z-Y-X顺序）
void eulerToRotationMatrixZYX(const SSG_EulerAngles& angles, double R[3][3]) {
	// 将角度转换为弧度
	const double deg2rad = M_PI / 180.0;
	const double yaw = angles.yaw * deg2rad;
	const double pitch = angles.pitch * deg2rad;
	const double roll = angles.roll * deg2rad;

	// 预计算三角函数值
	const double cy = cos(yaw), sy = sin(yaw);
	const double cp = cos(pitch), sp = sin(pitch);
	const double cr = cos(roll), sr = sin(roll);

#if 0
	// 绕Z轴旋转矩阵
	double Rz[3][3] = {
		{cy, -sy, 0},
		{sy,  cy, 0},
		{0,   0,  1}
	};

	// 绕Y轴旋转矩阵
	double Ry[3][3] = {
		{cp,  0, sp},
		{0,   1,  0},
		{-sp, 0, cp}
	};

	// 绕X轴旋转矩阵
	double Rx[3][3] = {
		{1,  0,   0},
		{0, cr, -sr},
		{0, sr,  cr}
	};


	// 矩阵相乘顺序：R = Rz * Ry * Rx
	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 3; ++j) {
			// 先计算 Rz * Ry
			double temp[3][3] = { 0 };
			for (int k = 0; k < 3; ++k) {
				temp[i][j] += Rz[i][k] * Ry[k][j];
			}

			// 再与 Rx 相乘
			R[i][j] = 0;
			for (int k = 0; k < 3; ++k) {
				R[i][j] += temp[i][k] * Rx[k][j];
			}
		}
	}
#endif

	// 优化后的直接计算公式（避免中间矩阵）
	R[0][0] = cy * cp;
	R[0][1] = cy * sp * sr - sy * cr;
	R[0][2] = cy * sp * cr + sy * sr;
	R[1][0] = sy * cp;
	R[1][1] = sy * sp * sr + cy * cr;
	R[1][2] = sy * sp * cr - cy * sr;
	R[2][0] = -sp;
	R[2][1] = cp * sr;
	R[2][2] = cp * cr;
}

//根据相机姿态对相机采集的3D数据进行旋转(没有平移)，将数据调整为俯视状态
///camPoseR为3x3矩阵
void lineDataRT(SVzNL3DLaserLine* a_line, const double* camPoseR, double groundH)
{
	for (int i = 0; i < a_line->nPositionCnt; i++)
	{
		SVzNL3DPoint a_pt = a_line->p3DPosition[i].pt3D;
		if (a_pt.z < 1e-4)
			continue;
		double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2];
		double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5];
		double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8];
		if ((groundH > 0) && (z > groundH))  //去除地面
			z = 0;
		a_pt.x = x;
		a_pt.y = y;
		a_pt.z = z;
		a_line->p3DPosition[i].pt3D = a_pt;
	}
	return;
}
void lineDataRT_vector(std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, double groundH)
{
	for (int i = 0; i < (int)a_line.size(); i++)
	{
		SVzNL3DPoint a_pt = a_line[i].pt3D;
		if (a_pt.z < 1e-4)
			continue;
		double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2];
		double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5];
		double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8];
		if ((groundH > 0) && (z > groundH))  //去除地面
			z = 0;
		a_pt.x = x;
		a_pt.y = y;
		a_pt.z = z;
		a_line[i].pt3D = a_pt;
	}
	return;
}

void HCamera_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) && (y >= groundH))  //去除地面
			z = 0;
		a_pt.x = x;
		a_pt.y = y;
		a_pt.z = z;
		a_line[i].pt3D = a_pt;
	}
	return;
}


void lineDataRT_RGBD(SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, double groundH)
{
	for (int i = 0; i < a_line->nPointCnt; i++)
	{
		SVzNLPointXYZRGBA a_pt = a_line->p3DPoint[i];
		if (a_pt.z < 1e-4)
			continue;
		double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2];
		double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5];
		double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8];
		if ((groundH > 0) && (z > groundH))  //去除地面
			z = 0;
		a_pt.x = (float)x;
		a_pt.y = (float)y;
		a_pt.z = (float)z;
		a_line->p3DPoint[i] = a_pt;
	}
	return;
}

//对栅格化数据进行XY平面上的投影二值量化，并对量化产生的空白点进行插值
void pointClout2DProjection(
	std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
	SVzNLRangeD x_range,
	SVzNLRangeD y_range,
	double scale,
	double cuttingGrndZ,
	int edgeSkip,
	double inerPolateDistTh,  //插值阈值。大于此阈值的不进行量化插值
	cv::Mat& projectionData,//投影量化数据，初始化为一个极大值1e+6
	cv::Mat& backIndexing  //标记坐标索引，用于回找3D坐标
)
{
	int lineNum = (int)gridScanData.size();
	if (lineNum == 0)
		return;
	int nPointCnt = (int)gridScanData[0].size();
	for (int line = 0; line < lineNum; line++)
	{
		int pre_x = -1, pre_y = -1;
		SVzNL3DPosition* prePt = NULL;
		for (int i = 0; i < nPointCnt; i++)
		{
			SVzNL3DPosition* pt3D = &gridScanData[line][i];
			if (pt3D->pt3D.z < 1e-4)
				continue;
			if ((cuttingGrndZ > 0) && (pt3D->pt3D.z > cuttingGrndZ))
				continue;
			double x = pt3D->pt3D.x;
			double y = pt3D->pt3D.y;
			int px = (int)(x - x_range.min)/scale + edgeSkip;
			int py = (int)(y - y_range.min)/scale + edgeSkip;

			cv::Vec2i v2i_exist = backIndexing.at<cv::Vec2i>(py, px);
#if 0
			if ((v2i_exist[0] > 0) || (v2i_exist[1] > 0))  //多个点重复投影到同一个点上，只保留一个有效点
			{
				pt3D->pt3D.z = 0; //invalidate
			}
			else
#endif
			{
				cv::Vec2i v2i = { line, i };
				backIndexing.at<cv::Vec2i>(py, px) = v2i;
				projectionData.at<float>(py, px) = 1e+6;
				//垂直插值
				if (prePt)
				{
					//计算距离，超过一定距离则不插值
					double dist = sqrt(pow(pt3D->pt3D.x - prePt->pt3D.x, 2) +
						pow(pt3D->pt3D.y - prePt->pt3D.y, 2) +
						pow(pt3D->pt3D.z - prePt->pt3D.z, 2));
					if (dist < inerPolateDistTh)
					{
						std::vector<SVzNL2DPoint> interPts;
						drawLine(
							pre_x,
							pre_y,
							px,
							py,
							interPts);
						for (int m = 0, m_max = (int)interPts.size(); m < m_max; m++)
							projectionData.at<float>(interPts[m].y, interPts[m].x) = 1e+6;
					}
				}
				prePt = pt3D;
				pre_x = px;
				pre_y = py;
			}
		}
	}
	//水平插值
	int pixWin = (int)(inerPolateDistTh / scale);
	for (int y = 0; y < projectionData.rows; y++)
	{
		int pre_x = -1;
		for (int x = 0; x < projectionData.cols; x++)
		{
			double value = projectionData.at<float>(y, x);
			if (value > 1e-4)
			{
				if (pre_x >= 0)
				{
					//插值
					int x_diff = x - pre_x;
					if ((x_diff > 1) && (x_diff < pixWin))
					{
						for (int m = pre_x + 1; m < x; m++)
							projectionData.at<float>(y, m) = 1e+6;
					}
				}
				pre_x = x;
			}
		}
	}
}

//对栅格化数据进行XY平面上的投影量化，Z值保留，并对量化产生的空白点进行插值
void pointCloud2DQuantization(
	std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
	SVzNLRangeD x_range,
	SVzNLRangeD y_range,
	double scale,
	int edgeSkip,
	double inerPolateDistTh,  //插值阈值。大于此阈值的不进行量化插值
	std::vector<std::vector<SVzNL3DPoint>>& quantiData,  //量化数据，初始化为一个极大值1e+6
	std::vector<std::vector<SVzNL2DPoint>>& backIndexing //标记坐标索引，用于回找3D坐标
)
{
	int lineNum = (int)gridScanData.size();
	if (lineNum == 0)
		return;

	//计算量化大小并初始化
	int x_cols = (int)((x_range.max - x_range.min) / scale) + 1 + edgeSkip * 2;
	int y_rows = (int)((y_range.max - y_range.min) / scale) + 1 + edgeSkip * 2;
	quantiData.resize(x_cols);
	backIndexing.resize(x_cols);
	double quantiXStart = x_range.min - edgeSkip * scale;
	double quantiYStart = y_range.min - edgeSkip * scale;
	for (int i = 0; i < x_cols; i++)
	{
		quantiData[i].resize(y_rows);
		for (int j = 0; j < y_rows; j++)
			quantiData[i][j] = {i * scale + quantiXStart + scale/2, j * scale + quantiYStart + scale / 2 , 0};
		backIndexing[i].resize(y_rows);
		std::fill(backIndexing[i].begin(), backIndexing[i].end(), SVzNL2DPoint{0,0});
	}

	int nPointCnt = (int)gridScanData[0].size();
	for (int line = 0; line < lineNum; line++)
	{
		int pre_x = -1, pre_y = -1;
		SVzNL3DPosition* prePt = NULL;
		for (int i = 0; i < nPointCnt; i++)
		{
			SVzNL3DPosition* pt3D = &gridScanData[line][i];
			if (pt3D->pt3D.z < 1e-4)
				continue;
			double x = pt3D->pt3D.x;
			double y = pt3D->pt3D.y;
			int px = (int)(x - x_range.min) / scale + edgeSkip;
			int py = (int)(y - y_range.min) / scale + edgeSkip;

			SVzNL2DPoint indexing_exist = backIndexing[px][py];  //按列存储，和扫描线方向一致
#if 0
			if ((v2i_exist[0] > 0) || (v2i_exist[1] > 0))  //多个点重复投影到同一个点上，只保留一个有效点
			{
				pt3D->pt3D.z = 0; //invalidate
			}
			else
#endif
			{
				SVzNL2DPoint v2i = { line, i };
				backIndexing[px][px] = v2i;
				quantiData[px][py].z = pt3D->pt3D.z;
				//垂直插值
				if (prePt)
				{
					//计算距离，超过一定距离则不插值
					double dist = sqrt(pow(pt3D->pt3D.x - prePt->pt3D.x, 2) +
						pow(pt3D->pt3D.y - prePt->pt3D.y, 2) +
						pow(pt3D->pt3D.z - prePt->pt3D.z, 2));
					if (dist < inerPolateDistTh)
					{
						std::vector<SVzNL2DPoint> interPts;
						drawLine(
							pre_x,
							pre_y,
							px,
							py,
							interPts);
						for (int m = 0, m_max = (int)interPts.size(); m < m_max; m++)
						{
							double k1=1.0, k2=0.0;
							if (py != pre_y)
							{
								k1 = ((double)(interPts[m].y - pre_y)) / ((double)(py - pre_y));
								k2 = 1.0 - k1;
							}
							double inter_z = k1 * pt3D->pt3D.z + k2 * prePt->pt3D.z;
							quantiData[interPts[m].x][interPts[m].y].z = inter_z;
						}
					}
				}
				prePt = pt3D;
				pre_x = px;
				pre_y = py;
			}
		}
	}
	//水平插值
	int pixWin = (int)(inerPolateDistTh / scale);
	int cols = (int)quantiData[0].size();
	int rows = (int)quantiData.size();
	for (int y = 0; y < rows; y++)  //和激光扫描方向一致
	{
		int pre_x = -1;
		double pre_value = -1;
		for (int x = 0; x < cols; x++)
		{
			double value = quantiData[x][y].z;
			if (value > 1e-4)
			{
				if (pre_x >= 0)
				{
					//插值
					int x_diff = x - pre_x;
					if ((x_diff > 1) && (x_diff < pixWin))
					{
						for (int m = pre_x + 1; m < x; m++)
						{
							double k1 = ((double)(m - pre_x)) / ((double)x_diff);
							double k2 = 1.0 - k1;
							double inter_z = k1 * value + k2 * pre_value;
							quantiData[x][y].z = inter_z;
						}
					}
				}
				pre_x = x;
				pre_value = value;
			}
		}
	}
}

//对空间两组对应点计算旋转平移矩阵
// Eigen库实现
void caculateRT(
	const std::vector<cv::Point3d>& pts1, 
	const std::vector<cv::Point3d>& pts2, 
	cv::Mat& R, cv::Mat& T,
	cv::Point3d& C1, cv::Point3d& C2)
{
	//【1】 求中心点
	cv::Point3d p1, p2;
	int N = pts1.size();
	for (int i = 0; i < N; i++)
	{
		p1 += pts1[i];
		p2 += pts2[i];
	}
	p1 = cv::Point3d(cv::Vec3d(p1) / N);
	p2 = cv::Point3d(cv::Vec3d(p2) / N);
	C1 = p1;
	C2 = p2;
	// 【2】得到去中心坐标
	std::vector<cv::Point3d> q1(N), q2(N);
	for (int i = 0; i < N; i++)
	{
		q1[i] = pts1[i] - p1;
		q2[i] = pts2[i] - p2;
	}

	//【3】计算需要进行奇异值分解的 W = sum(qi * qi’转置) compute q1*q2^T
	Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
	for (int i = 0; i < N; i++)
		W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();

	// 【4】对  W 进行SVD 奇异值分解
	Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
	Eigen::Matrix3d U = svd.matrixU();
	Eigen::Matrix3d V = svd.matrixV();

	// 【5】计算旋转 和平移矩阵 R  和 t,  R= V *M* UT 
	double det = (U * V.transpose()).determinant();
	Eigen::Matrix3d M;
	M << 1, 0, 0, 0, 1, 0, 0, 0, det;

	Eigen::Matrix3d R_ = V * M * (U.transpose());

	// t =  p' - R * p
	Eigen::Vector3d t_ = Eigen::Vector3d(p2.x, p2.y, p2.z) - R_ * Eigen::Vector3d(p1.x, p1.y, p1.z);

	// 【6】格式转换 convert to cv::Mat
	R = (cv::Mat_<double>(3, 3) <<
		R_(0, 0), R_(0, 1), R_(0, 2),
		R_(1, 0), R_(1, 1), R_(1, 2),
		R_(2, 0), R_(2, 1), R_(2, 2)
		);

	T = (cv::Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
	return;
}

//计算点旋转平移后的位置
void pointRT(const cv::Mat& R, const cv::Mat& T,
	const cv::Point3d& originPt, const cv::Point3d& rtOriginPT,  //RT(旋转平移）前后的质心
	const cv::Point3d& pt, cv::Point3d& rtPt)  //RT前后的点
{
	Eigen::Matrix3d _R;
	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 3; ++j) {
			_R(i, j) = R.at<double>(i, j);
		}
	}
	Eigen::Vector3d _T = Eigen::Vector3d(T.at<double>(0, 0), T.at<double>(1, 0), T.at<double>(2, 0));

	Eigen::Vector3d vec_origin = Eigen::Vector3d(originPt.x, originPt.y, originPt.z);
	Eigen::Vector3d vec_rtOrigin = Eigen::Vector3d(rtOriginPT.x, rtOriginPT.y, rtOriginPT.z);
	Eigen::Vector3d vec_pt = Eigen::Vector3d(pt.x, pt.y, pt.z);
	Eigen::Vector3d result = _R * (vec_pt - vec_origin) + vec_rtOrigin;

	rtPt.x = result(0);
	rtPt.y = result(1);
	rtPt.z = result(2);
	return;
}

//计算点旋转平移后的位置
void pointRT_2(const cv::Mat& R, const cv::Mat& T,
	const cv::Point3d& pt, cv::Point3d& rtPt)  //RT前后的点
{
	Eigen::Matrix3d _R;
	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 3; ++j) {
			_R(i, j) = R.at<double>(i, j);
		}
	}
	Eigen::Vector3d _T = Eigen::Vector3d(T.at<double>(0, 0), T.at<double>(1, 0), T.at<double>(2, 0));

	Eigen::Vector3d vec_pt = Eigen::Vector3d(pt.x, pt.y, pt.z);
	Eigen::Vector3d result = _R * vec_pt + _T;

	rtPt.x = result(0);
	rtPt.y = result(1);
	rtPt.z = result(2);
	return;
}

//计算点旋转后的位置
void pointRotate(const cv::Mat& R, 
	const cv::Point3d& pt, cv::Point3d& rtPt)  //Rotate前后的点
{
	Eigen::Matrix3d _R;
	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 3; ++j) {
			_R(i, j) = R.at<double>(i, j);
		}
	}

	Eigen::Vector3d vec_pt = Eigen::Vector3d(pt.x, pt.y, pt.z);
	Eigen::Vector3d result = _R * vec_pt;

	rtPt.x = result(0);
	rtPt.y = result(1);
	rtPt.z = result(2);
	return;
}

void WD_EulerRpyToRotation(double rpy[3], double matrix3d[9]) {
	double cos0 = cos(rpy[0] * PI / 180);
	double sin0 = sin(rpy[0] * PI / 180);
	double cos1 = cos(rpy[1] * PI / 180);
	double sin1 = sin(rpy[1] * PI / 180);
	double cos2 = cos(rpy[2] * PI / 180);
	double sin2 = sin(rpy[2] * PI / 180);
	matrix3d[0] = cos2 * cos1;
	matrix3d[1] = cos2 * sin1 * sin0 - sin2 * cos0;
	matrix3d[2] = cos2 * sin1 * cos0 + sin2 * sin0;
	matrix3d[3] = sin2 * cos1;
	matrix3d[4] = sin2 * sin1 * sin0 + cos2 * cos0;
	matrix3d[5] = sin2 * sin1 * cos0 - cos2 * sin0;
	matrix3d[6] = -sin1;
	matrix3d[7] = cos1 * sin0;
	matrix3d[8] = cos1 * cos0;
	return;
}

void WD_EulerRpyToDirVectors(double rpy[3],std::vector<cv::Point3d>& dirVectors) {
	double cos0 = cos(rpy[0] * PI / 180);
	double sin0 = sin(rpy[0] * PI / 180);
	double cos1 = cos(rpy[1] * PI / 180);
	double sin1 = sin(rpy[1] * PI / 180);
	double cos2 = cos(rpy[2] * PI / 180);
	double sin2 = sin(rpy[2] * PI / 180);

	double matrix3d[9];
	matrix3d[0] = cos2 * cos1;
	matrix3d[1] = cos2 * sin1 * sin0 - sin2 * cos0;
	matrix3d[2] = cos2 * sin1 * cos0 + sin2 * sin0;
	matrix3d[3] = sin2 * cos1;
	matrix3d[4] = sin2 * sin1 * sin0 + cos2 * cos0;
	matrix3d[5] = sin2 * sin1 * cos0 - cos2 * sin0;
	matrix3d[6] = -sin1;
	matrix3d[7] = cos1 * sin0;
	matrix3d[8] = cos1 * cos0;

	cv::Point3d vx, vy, vz;
	vx.x = matrix3d[0];
	vy.x = matrix3d[1];
	vz.x = matrix3d[2];
	vx.y = matrix3d[3];
	vy.y = matrix3d[4];
	vz.y = matrix3d[5];
	vx.z = matrix3d[6];
	vy.z = matrix3d[7];
	vz.z = matrix3d[8];
	dirVectors.push_back(vx);
	dirVectors.push_back(vy);
	dirVectors.push_back(vz);
	return;
}



#if 0
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/SVD>

// Define a struct for 3D points
struct Point3D {
	double x, y, z;
};

// Function to perform 3D line fitting using SVD
void fitLine3D(const std::vector<Point3D>& points, Eigen::Vector3d& centroid, Eigen::Vector3d& direction) {
	int n = points.size();
	if (n < 2) {
		std::cerr << "Need at least 2 points to fit a line." << std::endl;
		return;
	}

	// 1. Calculate Centroid
	centroid.setZero();
	for (const auto& p : points) {
		centroid += Eigen::Vector3d(p.x, p.y, p.z);
	}
	centroid /= n;

	// 2. Center the data and build the data matrix
	Eigen::MatrixXd data_matrix(n, 3);
	for (int i = 0; i < n; ++i) {
		data_matrix.row(i) << points[i].x - centroid(0),
			points[i].y - centroid(1),
			points[i].z - centroid(2);
	}

	// 3. Apply SVD
	// We compute the SVD of the centered data matrix
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(data_matrix, Eigen::ComputeThinV);

	// 4. Extract the direction vector
	// The right singular vector corresponding to the largest singular value (first column of V)
	// gives the direction of the best-fit line.
	direction = svd.matrixV().col(0);
}

int main() {
	// Sample data points
	std::vector<Point3D> points = {
		{1.0, 2.0, 3.0},
		{2.0, 3.0, 4.0},
		{3.0, 4.0, 5.0},
		{4.0, 5.0, 6.0},
		{5.0, 6.0, 7.0}
	};

	Eigen::Vector3d centroid;
	Eigen::Vector3d direction;

	fitLine3D(points, centroid, direction);

	std::cout << "Centroid (point on the line): " << centroid.transpose() << std::endl;
	std::cout << "Direction vector of the line: " << direction.transpose() << std::endl;
	std::cout << "Equation of the line: P(t) = Centroid + t * Direction" << std::endl;

	return 0;
}

template<class Vector3>
std::pair < Vector3, Vector3 > best_line_from_points(const std::vector<Vector3>& c)
{
	// copy coordinates to  matrix in Eigen format
	size_t num_atoms = c.size();
	Eigen::Matrix< Vector3::Scalar, Eigen::Dynamic, Eigen::Dynamic > centers(num_atoms, 3);
	for (size_t i = 0; i < num_atoms; ++i) centers.row(i) = c[i];

	Vector3 origin = centers.colwise().mean();
	Eigen::MatrixXd centered = centers.rowwise() - origin.transpose();
	Eigen::MatrixXd cov = centered.adjoint() * centered;
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(cov);
	Vector3 axis = eig.eigenvectors().col(2).normalized();

	return std::make_pair(origin, axis);
}
#endif