#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 * 3 / 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;
}