diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 4aeb206..6a3c5a4 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -2280,15 +2280,22 @@ SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara( #if 1 //两个向量的旋转旋转,使用四元数法, Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]); - Vector3 b = Vector3(0, 1.0, 0); - Quaternion quanPara = rotationBetweenVectors(a, b); - - RotationMatrix rMatrix; - quaternionToMatrix(quanPara, rMatrix.data); + 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 = rotationBetweenVectors(b, a); - RotationMatrix invMatrix; - quaternionToMatrix(invQuanPara, invMatrix.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]); @@ -2299,38 +2306,26 @@ SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara( 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.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.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]; + 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]; -#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 calibYRange = { 0, -1 }; SVzNLRangeD topYRange = { 0, -1 }; @@ -2363,8 +2358,62 @@ SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara( } if (sumSize > 0) sumMeanY = sumMeanY / (double)sumSize; - planePara.planeHeight = sumMeanY; // calibZRange.min; + 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; } diff --git a/sourceCode/wheelArchHeigthMeasure.cpp b/sourceCode/wheelArchHeigthMeasure.cpp index 79be6fe..8b47c72 100644 --- a/sourceCode/wheelArchHeigthMeasure.cpp +++ b/sourceCode/wheelArchHeigthMeasure.cpp @@ -7,7 +7,8 @@ //version 1.0.0 : base version release to customer //version 1.1.0 : 添加了轮眉到地面高度输出 -std::string m_strVersion = "1.1.0"; +//version 1.2.0 : 修正了地面调平,防止法向量旋转180度 +std::string m_strVersion = "1.2.0"; const char* wd_wheelArchHeigthMeasureVersion(void) { return m_strVersion.c_str(); diff --git a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp index 02ecfb7..a19004f 100644 --- a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp +++ b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp @@ -2636,8 +2636,8 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine * scanData } #define TEST_CONVERT_TO_GRID 0 -#define TEST_COMPUTE_WHEEL_ARCH 0 -#define TEST_COMPUTE_CALIB_PARA 1 +#define TEST_COMPUTE_WHEEL_ARCH 1 +#define TEST_COMPUTE_CALIB_PARA 0 #define TEST_GROUP 1 int main() @@ -2683,7 +2683,7 @@ int main() #if TEST_COMPUTE_CALIB_PARA char _calib_datafile[256]; - sprintf_s(_calib_datafile, "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/ground_LaserData.txt"); + sprintf_s(_calib_datafile, "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/鐜板満鏁版嵁/ground_LaserData.txt"); std::vector< std::vector> scanLines; vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines); @@ -2704,10 +2704,10 @@ int main() #endif // char calibFile[250]; - sprintf_s(calibFile, "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/ground_calib_para.txt"); + sprintf_s(calibFile, "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/鐜板満鏁版嵁/ground_calib_para.txt"); _outputCalibPara(calibFile, calibPara); char _out_file[256]; - sprintf_s(_out_file, "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/LaserLine_grund_calib.txt"); + sprintf_s(_out_file, "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/鐜板満鏁版嵁/LaserLine_grund_calib.txt"); _outputScanDataFile_self(_out_file, scanLines, 0, 0, 0); printf("%s: calib done!\n", _calib_datafile); } @@ -2716,11 +2716,11 @@ int main() #if TEST_COMPUTE_WHEEL_ARCH const char* dataPath[TEST_GROUP] = { - "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/", //0 + "F:/ShangGu/椤圭洰/鍐犻挦_杞湁楂樺害娴嬮噺/娴嬭瘯鏁版嵁/鐜板満鏁版嵁/", //0 }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,1}, + {1,2}, }; SSG_planeCalibPara poseCalibPara; @@ -2752,7 +2752,7 @@ int main() for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { //fidx = 193; - sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx); + sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); std::vector> scanData; vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData); if (scanData.size() == 0) @@ -2819,8 +2819,6 @@ int main() _outputScanDataFile_RGBD_obj(_dbg_file, scanData, 0, 0, 0, wheelArcHeight); #endif printf("%s: height=%f, arcToGrund=%f, time=%d(ms)!\n", _scan_file, wheelArcHeight.archToCenterHeigth, wheelArcHeight.archToGroundHeigth, (int)(t2 - t1)); - - } } #endif