wheelArchHeigthMeasure version 1.2.0 :
修正了地面调平，防止法向量旋转180度
This commit is contained in:
parent
47e3a04bf0
commit
3ddb970acc
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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<SVzNL3DPosition>> 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<std::vector< SVzNL3DPosition>> 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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user