diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index c698ca8..b39b7f1 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/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>& gridData, + std::vector>& 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>& 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>& gridScanData, + SVzNLRangeD x_range, + SVzNLRangeD y_range, + double scale, + double cuttingGrndZ, + int edgeSkip, + double inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值 + std::vector> quantiData, //量化数据，初始化为一个极大值1e+6 + std::vector> 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& dirVectors); diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index f9af922..53a0b64 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/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>& gridScanData, SVzNLRangeD x_range, @@ -3063,6 +3063,137 @@ void pointClout2DProjection( } } +#if 0 +//对栅格化数据进行XY平面上的投影量化，Z值保留，并对量化产生的空白点进行插值 +void pointClout2DQuantization( + std::vector< std::vector>& gridScanData, + SVzNLRangeD x_range, + SVzNLRangeD y_range, + double scale, + double cuttingGrndZ, + int edgeSkip, + double inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值 + std::vector> quantiData, //量化数据，初始化为一个极大值1e+6 + std::vector> 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 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(i, j); + } + } + Eigen::Vector3d _T = Eigen::Vector3d(T.at(0, 0), T.at(1, 0), T.at(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(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& 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 空间直线最小二乘拟合 diff --git a/sourceCode/SG_clustering.cpp b/sourceCode/SG_clustering.cpp index 8da9b91..536783a 100644 --- a/sourceCode/SG_clustering.cpp +++ b/sourceCode/SG_clustering.cpp @@ -67,6 +67,7 @@ void sg_pointClustering( return; } +//对特征点的聚类 void wd_gridPointClustering( std::vector>& featureMask, std::vector>& 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>& gridData, + std::vector>& 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>& featureMask, @@ -201,4 +281,67 @@ void wd_gridPointClustering_labelling( i++; } return; -} \ No newline at end of file +} +#if 0 +//使用聚类方法完成8连通连通域分析 +void wd_gridPointClustering_labelling_2( + std::vector>& srcData, + std::vector>& 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 \ No newline at end of file diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index 30f9dbe..07418fd 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -12,6 +12,7 @@ #include #include #include +#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 pts_eye; + pts_eye.resize(6); + std::vector 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:缁晍, B锛氱粫y锛 C:缁晉 + 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 test_pts_eye; + std::vector 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> 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 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 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 diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.vcxproj b/workpieceHolePositioning_test/workpieceHolePositioning_test.vcxproj index 76e8f5b..95d95e7 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.vcxproj +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.vcxproj @@ -126,7 +126,7 @@ Console true ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) - opencv_world320d.lib;workpieceHolePositioning.lib;%(AdditionalDependencies) + opencv_world320d.lib;baseAlgorithm.lib;workpieceHolePositioning.lib;%(AdditionalDependencies) @@ -145,7 +145,7 @@ true true ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) - opencv_world320.lib;workpieceHolePositioning.lib;%(AdditionalDependencies) + opencv_world320.lib;baseAlgorithm.lib;workpieceHolePositioning.lib;%(AdditionalDependencies)