OpenCV 根据对应的三维点估计刚体变换的旋转平移矩阵(RT矩阵)
2017-07-02 21:01
1471 查看
OpenCV通过estimateAffine3D() 提供了三维仿射变换模型的最小二乘估计方法,但是遗憾的是没有提供三维刚体变换模型(即旋转/平移矩阵,RT矩阵)的估计方法。
下面的代码提供了对该方法的一种实现。
struct TRigidTrans3D
{
double matR[9];
double X;
double Y;
double Z;
};
void GetRigidTrans3D(cv::Point3f* srcPoints, cv::Point3f* dstPoints, int pointsNum, TRigidTrans3D& transform)
{
double srcSumX = 0.0f;
double srcSumY = 0.0f;
double srcSumZ = 0.0f;
double dstSumX = 0.0f;
double dstSumY = 0.0f;
double dstSumZ = 0.0f;
for (int i = 0; i < pointsNum; ++ i)
{
srcSumX += srcPoints[i].x;
srcSumY += srcPoints[i].y;
srcSumZ += srcPoints[i].z;
dstSumX += dstPoints[i].x;
dstSumY += dstPoints[i].y;
dstSumZ += dstPoints[i].z;
}
cv::Point3f centerSrc, centerDst;
centerSrc.x = float(srcSumX / pointsNum);
centerSrc.y = float(srcSumY / pointsNum);
centerSrc.z = float(srcSumZ / pointsNum);
centerDst.x = float(dstSumX / pointsNum);
centerDst.y = float(dstSumY / pointsNum);
centerDst.z = float(dstSumZ / pointsNum);
cv::Mat srcMat(3, pointsNum, CV_32FC1);
cv::Mat dstMat(3, pointsNum, CV_32FC1);
float* srcDat = (float*)(srcMat.data);
float* dstDat = (float*)(dstMat.data);
for (int i = 0; i < pointsNum; ++ i)
{
srcDat[i] = srcPoints[i].x - centerSrc.x;
srcDat[pointsNum + i] = srcPoints[i].y - centerSrc.y;
srcDat[pointsNum * 2 + i] = srcPoints[i].z - centerSrc.z;
dstDat[i] = dstPoints[i].x - centerDst.x;
dstDat[pointsNum + i] = dstPoints[i].y - centerDst.y;
dstDat[pointsNum * 2 + i] = dstPoints[i].z - centerDst.z;
}
cv::Mat matS = srcMat * dstMat.t();
cv::Mat matU, matW, matV;
cv::SVDecomp(matS, matW, matU, matV);
cv::Mat matTemp = matU * matV;
double det = cv::determinant(matTemp);
double datM[] = {1, 0, 0, 0, 1, 0, 0, 0, det};
cv::Mat matM(3, 3, CV_64FC1, datM);
cv::Mat matR = matV.t() * matM * matU.t();
memcpy(transform.matR, matR.data, sizeof(double) * 9);
double* datR = (double*)(matR.data);
transform.X = centerDst.x - (centerSrc.x * datR[0] + centerSrc.y * datR[1] + centerSrc.z * datR[2]);
transform.Y = centerDst.y - (centerSrc.x * datR[3] + centerSrc.y * datR[4] + centerSrc.z * datR[5]);
transform.Z = centerDst.z - (centerSrc.x * datR[6] + centerSrc.y * datR[7] + centerSrc.z * datR[8]);
}
下面的代码提供了对该方法的一种实现。
struct TRigidTrans3D
{
double matR[9];
double X;
double Y;
double Z;
};
void GetRigidTrans3D(cv::Point3f* srcPoints, cv::Point3f* dstPoints, int pointsNum, TRigidTrans3D& transform)
{
double srcSumX = 0.0f;
double srcSumY = 0.0f;
double srcSumZ = 0.0f;
double dstSumX = 0.0f;
double dstSumY = 0.0f;
double dstSumZ = 0.0f;
for (int i = 0; i < pointsNum; ++ i)
{
srcSumX += srcPoints[i].x;
srcSumY += srcPoints[i].y;
srcSumZ += srcPoints[i].z;
dstSumX += dstPoints[i].x;
dstSumY += dstPoints[i].y;
dstSumZ += dstPoints[i].z;
}
cv::Point3f centerSrc, centerDst;
centerSrc.x = float(srcSumX / pointsNum);
centerSrc.y = float(srcSumY / pointsNum);
centerSrc.z = float(srcSumZ / pointsNum);
centerDst.x = float(dstSumX / pointsNum);
centerDst.y = float(dstSumY / pointsNum);
centerDst.z = float(dstSumZ / pointsNum);
cv::Mat srcMat(3, pointsNum, CV_32FC1);
cv::Mat dstMat(3, pointsNum, CV_32FC1);
float* srcDat = (float*)(srcMat.data);
float* dstDat = (float*)(dstMat.data);
for (int i = 0; i < pointsNum; ++ i)
{
srcDat[i] = srcPoints[i].x - centerSrc.x;
srcDat[pointsNum + i] = srcPoints[i].y - centerSrc.y;
srcDat[pointsNum * 2 + i] = srcPoints[i].z - centerSrc.z;
dstDat[i] = dstPoints[i].x - centerDst.x;
dstDat[pointsNum + i] = dstPoints[i].y - centerDst.y;
dstDat[pointsNum * 2 + i] = dstPoints[i].z - centerDst.z;
}
cv::Mat matS = srcMat * dstMat.t();
cv::Mat matU, matW, matV;
cv::SVDecomp(matS, matW, matU, matV);
cv::Mat matTemp = matU * matV;
double det = cv::determinant(matTemp);
double datM[] = {1, 0, 0, 0, 1, 0, 0, 0, det};
cv::Mat matM(3, 3, CV_64FC1, datM);
cv::Mat matR = matV.t() * matM * matU.t();
memcpy(transform.matR, matR.data, sizeof(double) * 9);
double* datR = (double*)(matR.data);
transform.X = centerDst.x - (centerSrc.x * datR[0] + centerSrc.y * datR[1] + centerSrc.z * datR[2]);
transform.Y = centerDst.y - (centerSrc.x * datR[3] + centerSrc.y * datR[4] + centerSrc.z * datR[5]);
transform.Z = centerDst.z - (centerSrc.x * datR[6] + centerSrc.y * datR[7] + centerSrc.z * datR[8]);
}
相关文章推荐
- OpenCV 根据对应的三维点估计刚体变换的旋转平移矩阵(RT矩阵)
- opencv 仿射变换 根据眼睛坐标进行人脸对齐 计算变换后对应坐标
- Nyoj 298 点的变换[利用矩阵求解坐标点的转换,平移,绕原点旋转,沿x,y轴翻转]
- opencv 图像仿射变换 计算仿射变换后对应特征点的新坐标 图像旋转、缩放、平移
- Decomposing a matrix(用于分解变换矩阵至旋转、平移,缩放分量)
- Decomposing a matrix(用于分解变换矩阵至旋转、平移,缩放分量)
- 三维矩阵旋转、平移的左乘与右乘分析
- NYOJ 298-点的变换(经典矩阵解决点平移、缩放、翻转和旋转)
- HDU 4087 三维上的平移缩放旋转矩阵变化
- 三维空间几何变换原理[平移、旋转、错切]
- 【OpenCV】仿射变换 根据眼睛坐标进行人脸对齐 计算变换后对应坐标
- opencv 仿射变换 根据眼睛坐标进行人脸对齐 计算变换后对应坐标
- 三维空间中刚体的变换旋转和平移
- 讨论opencv位姿估计结果与实际运动轨迹的关系(涉及变换矩阵与其逆矩阵)
- opencv 图像仿射变换 计算仿射变换后对应特征点的新坐标 图像旋转、缩放、平移
- opencv 仿射变换 根据眼睛坐标进行人脸对齐 计算变换后对应坐标
- 【计算机视觉】opencv靶标相机姿态解算2 根据四个特征点估计相机姿态 及 实时位姿估计与三维重建相机姿态
- 【计算机视觉】opencv靶标相机姿态解算3 根据两幅图像的位姿估计结果求某点的世界坐标 (三维重建)
- Decomposing a matrix(用于分解变换矩阵至旋转、平移,缩放分量)
- opencv 仿射变换 根据眼睛坐标进行人脸对齐 计算变换后对应坐标