jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wheelArchHeigthMeasure version 1.2.0 :

Browse Source 
修正了地面调平，防止法向量旋转180度
This commit is contained in:
jerryzeng 2026-01-05 16:24:18 +08:00
parent 47e3a04bf0
commit 3ddb970acc
3 changed files with 98 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

127
sourceCode/SG_baseFunc.cpp
View File

@ -2280,15 +2280,22 @@ SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara(
#if 1 //两个向量的旋转旋转，使用四元数法， #if 1 //两个向量的旋转旋转，使用四元数法，
Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]); Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]);
Vector3 b = Vector3(0, 1.0, 0); Vector3 b1 = Vector3(0, 1.0, 0);
Quaternion quanPara = rotationBetweenVectors(a, b); Vector3 b2 = Vector3(0, -1.0, 0);
Quaternion quanPara_1 = rotationBetweenVectors(a, b1);
RotationMatrix rMatrix; Quaternion quanPara_2 = rotationBetweenVectors(a, b2);
quaternionToMatrix(quanPara, rMatrix.data); 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; Quaternion invQuanPara_1 = rotationBetweenVectors(b1, a);
quaternionToMatrix(invQuanPara, invMatrix.data); 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 //根据平面的法向量计算欧拉角，进而计算旋转矩阵 #else //根据平面的法向量计算欧拉角，进而计算旋转矩阵
//参数计算 //参数计算
SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]); 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); RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll);
#endif #endif
planePara.planeCalib[0] = rMatrix.data[0][0]; planePara.planeCalib[0] = rMatrix_1.data[0][0];
planePara.planeCalib[1] = rMatrix.data[0][1]; planePara.planeCalib[1] = rMatrix_1.data[0][1];
planePara.planeCalib[2] = rMatrix.data[0][2]; planePara.planeCalib[2] = rMatrix_1.data[0][2];
planePara.planeCalib[3] = rMatrix.data[1][0]; planePara.planeCalib[3] = rMatrix_1.data[1][0];
planePara.planeCalib[4] = rMatrix.data[1][1]; planePara.planeCalib[4] = rMatrix_1.data[1][1];
planePara.planeCalib[5] = rMatrix.data[1][2]; planePara.planeCalib[5] = rMatrix_1.data[1][2];
planePara.planeCalib[6] = rMatrix.data[2][0]; planePara.planeCalib[6] = rMatrix_1.data[2][0];
planePara.planeCalib[7] = rMatrix.data[2][1]; planePara.planeCalib[7] = rMatrix_1.data[2][1];
planePara.planeCalib[8] = rMatrix.data[2][2]; planePara.planeCalib[8] = rMatrix_1.data[2][2];
planePara.invRMatrix[0] = invMatrix.data[0][0]; planePara.invRMatrix[0] = invMatrix_1.data[0][0];
planePara.invRMatrix[1] = invMatrix.data[0][1]; planePara.invRMatrix[1] = invMatrix_1.data[0][1];
planePara.invRMatrix[2] = invMatrix.data[0][2]; planePara.invRMatrix[2] = invMatrix_1.data[0][2];
planePara.invRMatrix[3] = invMatrix.data[1][0]; planePara.invRMatrix[3] = invMatrix_1.data[1][0];
planePara.invRMatrix[4] = invMatrix.data[1][1]; planePara.invRMatrix[4] = invMatrix_1.data[1][1];
planePara.invRMatrix[5] = invMatrix.data[1][2]; planePara.invRMatrix[5] = invMatrix_1.data[1][2];
planePara.invRMatrix[6] = invMatrix.data[2][0]; planePara.invRMatrix[6] = invMatrix_1.data[2][0];
planePara.invRMatrix[7] = invMatrix.data[2][1]; planePara.invRMatrix[7] = invMatrix_1.data[2][1];
planePara.invRMatrix[8] = invMatrix.data[2][2]; 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 calibYRange = { 0, -1 };
SVzNLRangeD topYRange = { 0, -1 }; SVzNLRangeD topYRange = { 0, -1 };
@ -2363,8 +2358,62 @@ SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara(
} }
if (sumSize > 0) if (sumSize > 0)
sumMeanY = sumMeanY / (double)sumSize; 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; return planePara;
} }

3
sourceCode/wheelArchHeigthMeasure.cpp
View File

@ -7,7 +7,8 @@
//version 1.0.0 : base version release to customer //version 1.0.0 : base version release to customer
//version 1.1.0 : 添加了轮眉到地面高度输出 //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) const char* wd_wheelArchHeigthMeasureVersion(void)
{ {
return m_strVersion.c_str(); return m_strVersion.c_str();

18
wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp
View File

@ -2636,8 +2636,8 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine * scanData
} }
#define TEST_CONVERT_TO_GRID 0 #define TEST_CONVERT_TO_GRID 0
#define TEST_COMPUTE_WHEEL_ARCH 0 #define TEST_COMPUTE_WHEEL_ARCH 1
#define TEST_COMPUTE_CALIB_PARA 1 #define TEST_COMPUTE_CALIB_PARA 0
#define TEST_GROUP 1 #define TEST_GROUP 1
int main() int main()
@ -2683,7 +2683,7 @@ int main()
#if TEST_COMPUTE_CALIB_PARA #if TEST_COMPUTE_CALIB_PARA
char _calib_datafile[256]; 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<SVzNL3DPosition>> scanLines; std::vector< std::vector<SVzNL3DPosition>> scanLines;
vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines); vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines);
@ -2704,10 +2704,10 @@ int main()
#endif #endif
// //
char calibFile[250]; char calibFile[250];
sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/ground_calib_para.txt"); sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/ground_calib_para.txt");
_outputCalibPara(calibFile, calibPara); _outputCalibPara(calibFile, calibPara);
char _out_file[256]; 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); _outputScanDataFile_self(_out_file, scanLines, 0, 0, 0);
printf("%s: calib done!\n", _calib_datafile); printf("%s: calib done!\n", _calib_datafile);
} }
@ -2716,11 +2716,11 @@ int main()
#if TEST_COMPUTE_WHEEL_ARCH #if TEST_COMPUTE_WHEEL_ARCH
const char* dataPath[TEST_GROUP] = { const char* dataPath[TEST_GROUP] = {
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/", //0 "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/", //0
}; };
SVzNLRange fileIdx[TEST_GROUP] = { SVzNLRange fileIdx[TEST_GROUP] = {
{1,1}, {1,2},
}; };
SSG_planeCalibPara poseCalibPara; SSG_planeCalibPara poseCalibPara;
@ -2752,7 +2752,7 @@ int main()
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{ {
//fidx = 193; //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<std::vector< SVzNL3DPosition>> scanData; std::vector<std::vector< SVzNL3DPosition>> scanData;
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData); vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData);
if (scanData.size() == 0) if (scanData.size() == 0)
@ -2819,8 +2819,6 @@ int main()
_outputScanDataFile_RGBD_obj(_dbg_file, scanData, 0, 0, 0, wheelArcHeight); _outputScanDataFile_RGBD_obj(_dbg_file, scanData, 0, 0, 0, wheelArcHeight);
#endif #endif
printf("%s: height=%f, arcToGrund=%f, time=%d(ms)!\n", _scan_file, wheelArcHeight.archToCenterHeigth, wheelArcHeight.archToGroundHeigth, (int)(t2 - t1)); printf("%s: height=%f, arcToGrund=%f, time=%d(ms)!\n", _scan_file, wheelArcHeight.archToCenterHeigth, wheelArcHeight.archToGroundHeigth, (int)(t2 - t1));
} }
} }
#endif #endif