Merge branch 'main' of http://gitea.mnutil.com/jerryzeng/algoLib

sourceCode/SG_baseAlgo_Export.h

@@ -666,6 +666,17 @@ SG_APISHARED_EXPORT void wd_gridPointClustering(
     SVzNL3DRangeD& clusterRoi  //roi3D
 );
 
+//对扫描数据的聚类
+SG_APISHARED_EXPORT void wd_gridPointClustering_2(
+    std::vector<std::vector<SVzNL3DPosition>>& gridData,
+    std::vector<std::vector<SSG_intPair>>& pointMaskInfo, //防止聚类时重复选中
+    int clusterCheckWin, //搜索窗口
+    SSG_treeGrowParam growParam,//聚类条件
+    int clusterID, //当前Cluster的ID
+    std::vector< SVzNL2DPoint>& a_cluster,  //result
+    SVzNL3DRangeD& clusterRoi  //roi3D
+);
+
 //使用聚类方法完成8连通连通域分析
 SG_APISHARED_EXPORT void wd_gridPointClustering_labelling(
     std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
@@ -688,6 +699,19 @@ SG_APISHARED_EXPORT void pointClout2DProjection(
     cv::Mat& backIndexing  //标记坐标索引，用于回找3D坐标
 );
 
+//对栅格化数据进行XY平面上的投影量化，Z值保留，并对量化产生的空白点进行插值
+SG_APISHARED_EXPORT void pointClout2DQuantization(
+    std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
+    SVzNLRangeD x_range,
+    SVzNLRangeD y_range,
+    double scale,
+    double cuttingGrndZ,
+    int edgeSkip,
+    double inerPolateDistTh,  //插值阈值。大于此阈值的不进行量化插值
+    std::vector<std::vector<double>> quantiData,  //量化数据，初始化为一个极大值1e+6
+    std::vector<std::vector<SVzNL2DPoint>> backIndexing //标记坐标索引，用于回找3D坐标
+);
+
 //分水岭算法
 SG_APISHARED_EXPORT void watershed(SWD_waterShedImage& img);
 // 根据输入的种子点进行分水岭算法
@@ -716,3 +740,16 @@ SG_APISHARED_EXPORT void caculateRT(
 SG_APISHARED_EXPORT 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前后的点
+
+//计算点旋转平移后的位置
+SG_APISHARED_EXPORT void pointRT_2(const cv::Mat& R, const cv::Mat& T,
+    const cv::Point3d& pt, cv::Point3d& rtPt);  //RT前后的点
+
+SG_APISHARED_EXPORT void pointRotate(const cv::Mat& R,
+    const cv::Point3d& pt, cv::Point3d& rtPt); //Rotate前后的点
+
+//从欧拉角计算旋转矩阵
+SG_APISHARED_EXPORT void WD_EulerRpyToRotation(double rpy[3], double matrix3d[9]);
+
+//从欧拉角计算方向矢量
+SG_APISHARED_EXPORT void WD_EulerRpyToDirVectors(double rpy[3], std::vector<cv::Point3d>& dirVectors);

sourceCode/SG_baseFunc.cpp

@@ -2966,7 +2966,7 @@ void lineDataRT_RGBD(SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, doub
 	return;
 }
 
-//对栅格化数据进行XY平面上的投影量化，并对量化产生的空白点进行插值
+//对栅格化数据进行XY平面上的投影二值量化，并对量化产生的空白点进行插值
 void pointClout2DProjection(
 	std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
 	SVzNLRangeD x_range,
@@ -3063,6 +3063,137 @@ void pointClout2DProjection(
 	}
 }
 
+#if 0
+//对栅格化数据进行XY平面上的投影量化，Z值保留，并对量化产生的空白点进行插值
+void pointClout2DQuantization(
+	std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
+	SVzNLRangeD x_range,
+	SVzNLRangeD y_range,
+	double scale,
+	double cuttingGrndZ,
+	int edgeSkip,
+	double inerPolateDistTh,  //插值阈值。大于此阈值的不进行量化插值
+	std::vector<std::vector<double>> 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);
+	for (int i = 0; i < x_cols; i++)
+	{
+		quantiData[i].resize(y_rows);
+		std::fill(quantiData[i].begin(), quantiData[i].end(), 0);
+		backIndexing[i].resize(y_rows);
+		std::fill(backIndexing[i].begin(), backIndexing[i].end(), 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;
+			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;
+
+			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] = 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] = 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];
+			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] = inter_z;
+						}
+					}
+				}
+				pre_x = x;
+				pre_value = value;
+			}
+		}
+	}
+}
+#endif
+
 //对空间两组对应点计算旋转平移矩阵
 // Eigen库实现
 void caculateRT(
@@ -3146,6 +3277,101 @@ void pointRT(const cv::Mat& R, const cv::Mat& T,
 	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;
+}
+
 
 /**
  * @brief 空间直线最小二乘拟合

sourceCode/SG_clustering.cpp

@@ -67,6 +67,7 @@ void sg_pointClustering(
 	return;
 }
 
+//对特征点的聚类
 void wd_gridPointClustering(
 	std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
 	std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
@@ -141,6 +142,85 @@ void wd_gridPointClustering(
 	return;
 }
 
+//对扫描数据的聚类
+//SSG_intPair成员变量定义: 
+//		data_0: flag
+//		data_1: valid point flag
+//		idx: cluster ID
+void wd_gridPointClustering_2(
+	std::vector<std::vector<SVzNL3DPosition>>& gridData,
+	std::vector<std::vector<SSG_intPair>>& pointMaskInfo, //防止聚类时重复选中
+	int clusterCheckWin, //搜索窗口
+	SSG_treeGrowParam growParam,//聚类条件
+	int clusterID, //当前Cluster的ID
+	std::vector< SVzNL2DPoint>& a_cluster,  //result
+	SVzNL3DRangeD& clusterRoi  //roi3D
+)
+{
+	int i = 0;
+	int lineNum = (int)gridData.size();
+	int linePtNum = (int)gridData[0].size();
+	pointMaskInfo[a_cluster[0].x][a_cluster[0].y].data_0 = 1;  //防止第一个被重复添加
+	while (1)
+	{
+		if (i >= a_cluster.size())
+			break;
+
+		SVzNL2DPoint a_seedPos = a_cluster[i];
+		if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
+			int kkk = 1;
+
+		SSG_intPair& a_seed = pointMaskInfo[a_seedPos.x][a_seedPos.y];
+		SVzNL3DPosition& seedValue = gridData[a_seedPos.x][a_seedPos.y];
+		if (0 == a_seed.idx)  //clusterID == 0, 未被处理，搜索邻域
+		{
+			for (int i = -clusterCheckWin; i <= clusterCheckWin; i++)
+			{
+				for (int j = -clusterCheckWin; j <= clusterCheckWin; j++)
+				{
+					int y = j + a_seedPos.y;
+					int x = i + a_seedPos.x;
+					if ((x == 390) && (y == 949))
+						int kkk = 1;
+
+					if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
+					{
+						SSG_intPair& chk_seed = pointMaskInfo[x][y];
+						if ((chk_seed.data_1 == 0) || (chk_seed.idx > 0))  //idx:clusterID, data_1:valid point
+							continue;
+
+						SVzNL3DPosition& chkValue = gridData[x][y];
+						double y_diff = abs(seedValue.pt3D.y - chkValue.pt3D.y);
+						double z_diff = abs(seedValue.pt3D.z - chkValue.pt3D.z);
+						double x_diff = abs(seedValue.pt3D.x - chkValue.pt3D.x);
+						if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
+							(x_diff < growParam.maxSkipDistance))
+						{
+							if (0 == chk_seed.data_0)//防止被重复添加
+							{
+								chk_seed.data_0 = 1;
+								SVzNL2DPoint new_seed = { x, y };
+								a_cluster.push_back(new_seed);
+							}
+						}
+					}
+
+				}
+			}
+		}
+		a_seed.idx = clusterID;
+		//更新ROI
+		clusterRoi.xRange.min = clusterRoi.xRange.min > seedValue.pt3D.x ? seedValue.pt3D.x : clusterRoi.xRange.min;
+		clusterRoi.xRange.max = clusterRoi.xRange.max < seedValue.pt3D.x ? seedValue.pt3D.x : clusterRoi.xRange.max;
+		clusterRoi.yRange.min = clusterRoi.yRange.min > seedValue.pt3D.y ? seedValue.pt3D.y : clusterRoi.yRange.min;
+		clusterRoi.yRange.max = clusterRoi.yRange.max < seedValue.pt3D.y ? seedValue.pt3D.y : clusterRoi.yRange.max;
+		clusterRoi.zRange.min = clusterRoi.zRange.min > seedValue.pt3D.z ? seedValue.pt3D.z : clusterRoi.zRange.min;
+		clusterRoi.zRange.max = clusterRoi.zRange.max < seedValue.pt3D.z ? seedValue.pt3D.z : clusterRoi.zRange.max;
+		i++;
+	}
+	return;
+}
+
 //使用聚类方法完成8连通连通域分析
 void wd_gridPointClustering_labelling(
 	std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
@@ -202,3 +282,66 @@ void wd_gridPointClustering_labelling(
 	}
 	return;
 }
+#if 0
+//使用聚类方法完成8连通连通域分析
+void wd_gridPointClustering_labelling_2(
+	std::vector<std::vector<double>>& srcData,
+	std::vector<std::vector<SSG_intPair>>& labelMask,
+	int clusterID, //当前Cluster的ID
+	std::vector< SVzNL2DPoint>& a_cluster,  //result
+	SVzNLRect& clusterRoi  //roi2D
+)
+{
+	int i = 0;
+	int lineNum = (int)srcData.size();
+	int linePtNum = (int)srcData[0].size();
+	while (1)
+	{
+		if (i >= a_cluster.size())
+			break;
+
+		SVzNL2DPoint a_seedPos = a_cluster[i];
+		if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
+			int kkk = 1;
+
+		SSG_featureClusteringInfo& a_seed = featureMask[a_seedPos.x][a_seedPos.y];
+		if (0 == a_seed.clusterID)  //clusterID == 0, 未被处理，搜索邻域
+		{
+			//8连通
+			for (int i = -1; i <= 1; i++)
+			{
+				for (int j = -1; j <= 1; j++)
+				{
+					int y = j + a_seedPos.y;
+					int x = i + a_seedPos.x;
+					if ((x == 390) && (y == 949))
+						int kkk = 1;
+
+					if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
+					{
+						SSG_featureClusteringInfo& chk_seed = featureMask[x][y];
+						if ((chk_seed.featurType == 0) || (chk_seed.clusterID > 0))  //只检查未聚类的特征点
+							continue;
+
+						if (0 == chk_seed.flag)//防止被重复添加
+						{
+							chk_seed.flag = 1;
+							SVzNL2DPoint new_seed = { x, y };
+							a_cluster.push_back(new_seed);
+						}
+					}
+
+				}
+			}
+		}
+		a_seed.clusterID = clusterID;
+		//更新ROI
+		clusterRoi.left = clusterRoi.left > a_seedPos.x ? a_seedPos.x : clusterRoi.left;
+		clusterRoi.right = clusterRoi.right < a_seedPos.x ? a_seedPos.x : clusterRoi.right;
+		clusterRoi.top = clusterRoi.top > a_seedPos.y ? a_seedPos.y : clusterRoi.top;
+		clusterRoi.bottom = clusterRoi.bottom < a_seedPos.y ? a_seedPos.y : clusterRoi.bottom;
+		i++;
+	}
+	return;
+}
+#endif

workpieceHolePositioning_test/workpieceHolePositioning_test.cpp

@@ -12,6 +12,7 @@
 #include <opencv2/opencv.hpp>
 #include <Windows.h>
 #include <limits>
+#include "SG_baseAlgo_Export.h"
 
 typedef struct
 {
@@ -432,7 +433,8 @@ SVzNL3DPoint _pointRT(SVzNL3DPoint& origin, const double* R, const double* T)
 }
 
 #define TEST_COMPUTE_CALIB_PARA		0
-#define TEST_COMPUTE_HOLE			1
+#define TEST_COMPUTE_HOLE			0
+#define TEST_COMPUTE_RT				1
 #define TEST_GROUP 1
 int main()
 {
@@ -448,9 +450,96 @@ int main()
 	const char* ver = wd_workpieceHolePositioningVersion();
 	printf("ver:%s\n", ver);
 
+#if TEST_COMPUTE_RT
+	std::vector<cv::Point3d> pts_eye;
+	pts_eye.resize(6);
+	std::vector<cv::Point3d> pts_robot;
+	pts_robot.resize(6);
+	pts_eye[0] = { 187.22, -99.43, 1797.70 }; //, R = 0.6413, P = 0.0302, Y = -91.1494
+	pts_eye[1] = { 186.50, -140.52, 1797.69 }; // R = 0.6413, P = 0.0302, Y = -88.8130
+	pts_eye[2] = { 256.73, -93.55, 1797.83 }; //, R = 0.6413, P = 0.0302, Y = -89.3432
+	pts_eye[3] = { 236.69, -48.82, 1797.77 }; // R = 0.6413, P = 0.0302, Y = -89.5285
+	pts_eye[4] = { 173.62, 13.92, 1797.30 };// R = 0.6413, P = 0.0302, Y = -89.6450
+	pts_eye[5] = { 232.11, -153.38, 1797.88 };// R = 0.6413, P = 0.0302, Y = -82.0278
+
+	//A:绕z, B：绕y， C:绕x
+	pts_robot[0] = { 55.830, -984.472, 97.687 }; // A:0.018 B : -0.638 C : -0.292
+	pts_robot[1] = { 95.469, -985.345, 97.782 }; //  A:-1.032 B : -0.354 C : 1.001
+	pts_robot[2] = { 47.908, -1053.709, 97.802 }; // A:-1.032 B : -0.356 C : 0.998
+	pts_robot[3] = { 3.646, -1033.152, 96.494 }; // A:-1.028 B : -0.367 C : 0.442
+	pts_robot[4] = { -58.843, -969.793, 95.297 }; // A:-1.029 B : -0.367 C : 0.442
+	pts_robot[5] = { 108.031, -1029.803, 98.333 }; // A:-9.208 B : -0.367 C : 0.442
+
+	//使用前6组数据
+	std::vector<cv::Point3d> test_pts_eye;
+	std::vector<cv::Point3d> test_pts_robot;
+	for (int i = 0; i < 5; i++)
+	{
+		test_pts_eye.push_back(pts_eye[i]);
+		test_pts_robot.push_back(pts_robot[i]);
+	}
+	//对空间两组对应点计算旋转平移矩阵
+	// Eigen库实现
+	cv::Mat R, T;
+	cv::Point3d C_eye, C_robot;
+	caculateRT(
+		test_pts_eye,
+		test_pts_robot,
+		R, T,
+		C_eye, C_robot);
+
+	std::cout << "方向向量转换结果：" << std::endl;
+	std::cout << std::fixed << std::setprecision(6); // 固定小数位数为6
+	std::cout << R << std::endl;
+	std::cout << T << std::endl;
+
+	//验算6轴姿态
+	std::vector<std::vector< cv::Point3d>>  pose_eye;
+	pose_eye.resize(6);
+	for (int i = 0; i < 6; i++)
+		pose_eye[i].resize(3);
+	pose_eye[0][0] = { -0.020, -1.000, -0.011 }; pose_eye[0][1] = { 1.000, -0.020, -0.001 }; pose_eye[0][2] = { 0.001, -0.011, 1.000 };
+	pose_eye[1][0] = { 0.021,-1.000,-0.011 }; pose_eye[1][1] = { 1.000,0.021,-0.000 }; pose_eye[1][2] = { 0.001,-0.011,1.000 };
+	pose_eye[2][0] = { 0.011,-1.000,-0.011 }; pose_eye[2][1] = { 1.000,0.011,-0.000 }; pose_eye[2][2] = { 0.001,-0.011,1.000 };
+	pose_eye[3][0] = { 0.008,-1.000,-0.011 }; pose_eye[3][1] = { 1.000,0.008,-0.000 }; pose_eye[3][2] = { 0.001,-0.011,1.000 };
+	pose_eye[4][0] = { 0.006,-1.000,-0.011 }; pose_eye[4][1] = { 1.000,0.006,-0.000 }; pose_eye[4][2] = { 0.001,-0.011,1.000 };
+	pose_eye[5][0] = { 0.139,-0.990,-0.011 }; pose_eye[5][1] = { 0.990,0.139,0.001 }; pose_eye[5][2] = { 0.001,-0.011,1.000 };
+
+	for (int i = 0; i < 6; i++)
+	{
+		cv::Point3d rtPt;
+		pointRT_2(R, T, pts_eye[i], rtPt);  //RT前后的点
+
+		std::vector<cv::Point3d> dirVectors_eye = pose_eye[i];
+		dirVectors_eye[1] = { -dirVectors_eye[1].x, -dirVectors_eye[1].y, -dirVectors_eye[1].z };
+		dirVectors_eye[2] = { -dirVectors_eye[2].x, -dirVectors_eye[2].y, -dirVectors_eye[2].z };
+		std::vector<cv::Point3d> dirVectors_robot;
+		for (int j = 0; j < 3; j++)
+		{
+			cv::Point3d rt_pt;
+			pointRotate(R, dirVectors_eye[j], rt_pt);
+			dirVectors_robot.push_back(rt_pt);
+		}
+		//生成旋转矩阵
+		double R_pose[3][3];
+		R_pose[0][0] = dirVectors_robot[0].x;
+		R_pose[0][1] = dirVectors_robot[1].x;
+		R_pose[0][2] = dirVectors_robot[2].x;
+		R_pose[1][0] = dirVectors_robot[0].y;
+		R_pose[1][1] = dirVectors_robot[1].y;
+		R_pose[1][2] = dirVectors_robot[2].y;
+		R_pose[2][0] = dirVectors_robot[0].z;
+		R_pose[2][1] = dirVectors_robot[1].z;
+		R_pose[2][2] = dirVectors_robot[2].z;
+		SSG_EulerAngles test_rpy = rotationMatrixToEulerZYX(R_pose);
+		std::cout << i << ":" << std::endl;
+		std::cout << rtPt.x << "," << rtPt.y << "," << rtPt.z << std::endl;
+		std::cout << test_rpy.roll << "," << test_rpy.pitch << "," << test_rpy.yaw << std::endl;
+	}
+#endif
 #if TEST_COMPUTE_CALIB_PARA
 	char _calib_datafile[256];
-	sprintf_s(_calib_datafile, "%sLaserData_ground.txt", dataPath[0]);
+	sprintf_s(_calib_datafile, "%sLaserData_ground2.txt", dataPath[0]);
 	int lineNum = 0;
 	float lineV = 0.0f;
 	int dataCalib = 0;
@@ -480,7 +569,7 @@ int main()
 		sprintf_s(_out_file, "%sscanData_ground_calib_verify.txt", dataPath[0]);
 		int headNullLines = 0;
 		_outputScanDataFile_vector(_out_file, scanData, false, &headNullLines);
-
+#if 0
 		for (int fidx = fileIdx[0].nMin; fidx <= fileIdx[0].nMax; fidx++)
 		{
 			//fidx =4;
@@ -500,6 +589,7 @@ int main()
 			int headNullLines = 0;
 			_outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines);
 		}
+#endif
 		printf("%s: calib done!\n", _calib_datafile);
 	}
 #endif

workpieceHolePositioning_test/workpieceHolePositioning_test.vcxproj

@@ -126,7 +126,7 @@
       <SubSystem>Console</SubSystem>
       <GenerateDebugInformation>true</GenerateDebugInformation>
       <AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
-      <AdditionalDependencies>opencv_world320d.lib;workpieceHolePositioning.lib;%(AdditionalDependencies)</AdditionalDependencies>
+      <AdditionalDependencies>opencv_world320d.lib;baseAlgorithm.lib;workpieceHolePositioning.lib;%(AdditionalDependencies)</AdditionalDependencies>
     </Link>
   </ItemDefinitionGroup>
   <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
@@ -145,7 +145,7 @@
       <OptimizeReferences>true</OptimizeReferences>
       <GenerateDebugInformation>true</GenerateDebugInformation>
       <AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
-      <AdditionalDependencies>opencv_world320.lib;workpieceHolePositioning.lib;%(AdditionalDependencies)</AdditionalDependencies>
+      <AdditionalDependencies>opencv_world320.lib;baseAlgorithm.lib;workpieceHolePositioning.lib;%(AdditionalDependencies)</AdditionalDependencies>
     </Link>
   </ItemDefinitionGroup>
   <ItemGroup>