algoLib/sourceCode/SG_baseFunc.cpp

#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