Refractive three-dimensional reconstruction for underwater stereo digital image correlation论文代码
在这篇论文中，作者提出了双目相机水下三维重建会产生畸变的解决方法，该方法的最关键一步就是折射界面方程的重建，但是在作者给的源码中却没有该部分的代码。
我在折射平面上粘贴圆形标记，然后使用双目相机得到标记点的三维坐标，最后通过RANSAC算法来拟合平面方程。
折射平面方程的构建代码能够用来拟合任何平面方程，如果你需要使用双目相机来拟合平面得到平面方程，该部分的代码同样实用。
我将在下面给出折射界面方程构建的代码以及折射三维重建的代码
折射三维重建的代码你也能够在该论文中作者给出的源码中获得：https://github.com/ZL-Su/Matrice
本文给出的代码源码以及相关测试文件能够在我的github中获得：https://github.com/duanying98/Refractive-three-dimensional-reconstruction-for-underwater-stereo-digital-image-correlation
平面方程构建代码
#include 折射三维重建代码
#include Unit direction vector
vec3d _Get_refracted_vector(vec3d vec1, vec3d N, double n1, double n2, double theta1, double theta2){
vec3d _Prev = { (n1 / n2) * vec1.x,
(n1 / n2) * vec1.y,
(n1 / n2) * vec1.z };
vec3d _Tail = { (n1 / n2 * cos(theta1) - cos(theta2)) * N.x,
(n1 / n2 * cos(theta1) - cos(theta2)) * N.y,
(n1 / n2 * cos(theta1) - cos(theta2)) * N.z };
return { _Prev.x - _Tail.x, _Prev.y - _Tail.y, _Prev.z - _Tail.z };
}
//Compute the intersection of two lines. 计算两条线的交点
/// End point of line 1; 线段1的端点
/// Direction of line 1; 线段1的方向向量
/// End point of line 2; 线段2的端点
/// Direction of line 2. 线段2的方向向量
/// Coordinates of the intersection 两条线段的交点坐标
vec3d _Intersection(vec3d point1, vec3d vec1, vec3d point2, vec3d vec2) {
vec3d vec3 = _Cross(vec1, vec2);
double x1 = -((point1.z * vec1.x * vec2.y * vec3.x - point2.z * vec1.x * vec2.y * vec3.x -
point1.x * vec1.z * vec2.y * vec3.x - point1.y * vec1.x * vec2.z * vec3.x +
point2.y * vec1.x * vec2.z * vec3.x + point1.x * vec1.y * vec2.z * vec3.x -
point1.z * vec1.x * vec2.x * vec3.y + point2.z * vec1.x * vec2.x * vec3.y +
point1.x * vec1.z * vec2.x * vec3.y - point2.x * vec1.x * vec2.z * vec3.y +
point1.y * vec1.x * vec2.x * vec3.z - point2.y * vec1.x * vec2.x * vec3.z -
point1.x * vec1.y * vec2.x * vec3.z + point2.x * vec1.x * vec2.y * vec3.z) /
(vec1.z * vec2.y * vec3.x - vec1.y * vec2.z * vec3.x - vec1.z * vec2.x * vec3.y +
vec1.x * vec2.z * vec3.y + vec1.y * vec2.x * vec3.z - vec1.x * vec2.y * vec3.z));
double y1 = -((-point1.z * vec1.y * vec2.y * vec3.x + point2.z * vec1.y * vec2.y * vec3.x +
point1.y * vec1.z * vec2.y * vec3.x - point2.y * vec1.y * vec2.z * vec3.x +
point1.z * vec1.y * vec2.x * vec3.y - point2.z * vec1.y * vec2.x * vec3.y -
point1.y * vec1.z * vec2.x * vec3.y + point1.y * vec1.x * vec2.z * vec3.y -
point1.x * vec1.y * vec2.z * vec3.y + point2.x * vec1.y * vec2.z * vec3.y +
point2.y * vec1.y * vec2.x * vec3.z - point1.y * vec1.x * vec2.y * vec3.z +
point1.x * vec1.y * vec2.y * vec3.z - point2.x * vec1.y * vec2.y * vec3.z) /
(-vec1.z * vec2.y * vec3.x + vec1.y * vec2.z * vec3.x + vec1.z * vec2.x * vec3.y -
vec1.x * vec2.z * vec3.y - vec1.y * vec2.x * vec3.z + vec1.x * vec2.y * vec3.z));
double z1 = -((point2.z * vec1.z * vec2.y * vec3.x - point1.z * vec1.y * vec2.z * vec3.x +
point1.y * vec1.z * vec2.z * vec3.x - point2.y * vec1.z * vec2.z * vec3.x -
point2.z * vec1.z * vec2.x * vec3.y + point1.z * vec1.x * vec2.z * vec3.y -
point1.x * vec1.z * vec2.z * vec3.y + point2.x * vec1.z * vec2.z * vec3.y +
point1.z * vec1.y * vec2.x * vec3.z - point1.y * vec1.z * vec2.x * vec3.z +
point2.y * vec1.z * vec2.x * vec3.z - point1.z * vec1.x * vec2.y * vec3.z +
point1.x * vec1.z * vec2.y * vec3.z - point2.x * vec1.z * vec2.y * vec3.z) /
(-vec1.z * vec2.y * vec3.x + vec1.y * vec2.z * vec3.x + vec1.z * vec2.x * vec3.y -
vec1.x * vec2.z * vec3.y - vec1.y * vec2.x * vec3.z + vec1.x * vec2.y * vec3.z));
double x2 = -((-point1.z * vec1.y * vec2.x * vec3.x + point2.z * vec1.y * vec2.x * vec3.x +
point1.y * vec1.z * vec2.x * vec3.x - point2.y * vec1.z * vec2.x * vec3.x +
point2.x * vec1.z * vec2.y * vec3.x - point2.x * vec1.y * vec2.z * vec3.x +
point1.z * vec1.x * vec2.x * vec3.y - point2.z * vec1.x * vec2.x * vec3.y -
point1.x * vec1.z * vec2.x * vec3.y + point2.x * vec1.x * vec2.z * vec3.y -
point1.y * vec1.x * vec2.x * vec3.z + point2.y * vec1.x * vec2.x * vec3.z +
point1.x * vec1.y * vec2.x * vec3.z - point2.x * vec1.x * vec2.y * vec3.z) /
(-vec1.z * vec2.y * vec3.x + vec1.y * vec2.z * vec3.x + vec1.z * vec2.x * vec3.y -
vec1.x * vec2.z * vec3.y - vec1.y * vec2.x * vec3.z + vec1.x * vec2.y * vec3.z));
double y2 = -((-point1.z * vec1.y * vec2.y * vec3.x + point2.z * vec1.y * vec2.y * vec3.x +
point1.y * vec1.z * vec2.y * vec3.x - point2.y * vec1.y * vec2.z * vec3.x -
point2.y * vec1.z * vec2.x * vec3.y + point1.z * vec1.x * vec2.y * vec3.y -
point2.z * vec1.x * vec2.y * vec3.y - point1.x * vec1.z * vec2.y * vec3.y +
point2.x * vec1.z * vec2.y * vec3.y + point2.y * vec1.x * vec2.z * vec3.y +
point2.y * vec1.y * vec2.x * vec3.z - point1.y * vec1.x * vec2.y * vec3.z +
point1.x * vec1.y * vec2.y * vec3.z - point2.x * vec1.y * vec2.y * vec3.z) /
(-vec1.z * vec2.y * vec3.x + vec1.y * vec2.z * vec3.x +
vec1.z * vec2.x * vec3.y - vec1.x * vec2.z * vec3.y -
vec1.y * vec2.x * vec3.z + vec1.x * vec2.y * vec3.z));
double z2 = -((point2.z * vec1.z * vec2.y * vec3.x - point1.z * vec1.y * vec2.z * vec3.x +
point1.y * vec1.z * vec2.z * vec3.x - point2.y * vec1.z * vec2.z * vec3.x -
point2.z * vec1.z * vec2.x * vec3.y + point1.z * vec1.x * vec2.z * vec3.y -
point1.x * vec1.z * vec2.z * vec3.y + point2.x * vec1.z * vec2.z * vec3.y +
point2.z * vec1.y * vec2.x * vec3.z - point2.z * vec1.x * vec2.y * vec3.z -
point1.y * vec1.x * vec2.z * vec3.z + point2.y * vec1.x * vec2.z * vec3.z +
point1.x * vec1.y * vec2.z * vec3.z - point2.x * vec1.y * vec2.z * vec3.z) /
(-vec1.z * vec2.y * vec3.x + vec1.y * vec2.z * vec3.x +
vec1.z * vec2.x * vec3.y - vec1.x * vec2.z * vec3.y -
vec1.y * vec2.x * vec3.z + vec1.x * vec2.y * vec3.z));
double x = (x1 + x2) / 2;
double y = (y1 + y2) / 2;
double z = (z1 + z2) / 2;
if (x == -0) {
x = 0;
}
if (y == -0) {
y = 0;
}
if (z == -0) {
z = 0;
}
return { x, y, z };
}
/********************************************************************************************
* \brief Driver for refractive 3D reconstruction. 折射三维重建
* \param 'raw_point' distorted object point P. 'raw_point'代表扭曲的点P。
* \param 'translation' translation vector of the right camera relative to the frame O-XYZ. translation代表右相机相对于参考坐标系O-XYZ的平移矢量。
* \param 'interf1' refracting interface Ax + By + Cz +D = 0, with N = (A. B. C).
* \param n1 n2 n3 thickness 折射率以及玻璃厚度
* \returns The true 3D coordinates of the object point. 返回真实的三维坐标
********************************************************************************************/
vec3d refractive_3d_reconstruction(vec3d raw_point, vec3d translation, vec4d interf1, double n1, double n2, double n3, double thickness) {
double delta = thickness;
vec3d t = translation;
double near_d = interf1.D;
// Build the interface 2
double far_d = near_d - delta * sqrt(pow(interf1.A, 2) + pow(interf1.B, 2) + pow(interf1.C, 2));
vec4d interf2 = { interf1.A, interf1.B, interf1.C, far_d };
// Compute the normalized normal of the interfaces 1 and 2
vec3d N1 = { interf1.A, interf1.B, interf1.C };
vec3d N2 = { interf2.A, interf2.B, interf2.C };
N1 = _Normalize(N1);
N2 = _Normalize(N2);
// Direction vectors of the ray L1 and L1'
vec3d nlineL1 = _Normalize(raw_point);
vec3d nlineR1 = _Normalize({ raw_point.x + t.x,raw_point.y + t.y,raw_point.z + t.z });
// Incident points P1 and P1' of L1 and L1' at the interface 1
vec3d pointL1 = _Intersection({ 0, 0, 0 }, nlineL1, interf1);
vec3d pointR1 = _Intersection({ -t.x, -t.y, -t.z }, nlineR1, interf1);
// Incident angles of rays L1 and L1'
double angleL1 = _Vector_angle(nlineL1, N1);
double angleR1 = _Vector_angle(nlineR1, N1);
// Refracted angles of rays L1 and L1'
double angleL2 = asin(n1 * sin(angleL1) / n2);
double angleR2 = asin(n1 * sin(angleR1) / n2);
// Direction vectors of the ray L2 and L2'
vec3d nlineL2 = _Get_refracted_vector(nlineL1, N1, n1, n2, angleL1, angleL2);
vec3d nlineR2 = _Get_refracted_vector(nlineR1, N1, n1, n2, angleR1, angleR2);
// Incident points P2 and P2' of L2 and L2' at the interface 2
vec3d pointL2 = _Intersection(pointL1, nlineL2, interf2);
vec3d pointR2 = _Intersection(pointR1, nlineR2, interf2);
// Incident angles of rays L2 and L2'
double angleL3 = angleL2;
double angleR3 = angleR2;
// Refracted angles of rays L2 and L2'
double angleL4 = asin(n2 * sin(angleL3) / n3);
double angleR4 = asin(n2 * sin(angleR3) / n3);
// Direction vectors of the ray L3 and L3'
vec3d nlineL3 = _Get_refracted_vector(nlineL2, N2, n2, n3, angleL3, angleL4);
vec3d nlineR3 = _Get_refracted_vector(nlineR2, N2, n2, n3, angleR3, angleR4);
//auto nCommonVerticalLine3 = detail::_Cross(nlineL3, nlineR3);
// Compute the true object point Q
return _Intersection(pointL2, nlineL3, pointR2, nlineR3);
}
int main() {
//折射重建测试
vec3d raw_point;
raw_point.x = 9;
raw_point.y = 0;
raw_point.z = 3 * 1.732;
vec3d translation;
translation.x = -18;
translation.y = 0;
translation.z = 0;
vec4d interf1;
interf1.A = 0;
interf1.B = 0;
interf1.C = 1;
interf1.D = -2;
double n1 = 1;
double n2 = 1.732;
double n3 = 1;
double thickness = 1;
vec3d res;
res = refractive_3d_reconstruction(raw_point, translation, interf1, n1, n2, n3, thickness);
cout << res.x << endl;
cout << res.y << endl;
cout << res.z << endl;
system("pause");
return 0;
}
参考：
- https://blog.csdn.net/qq_35789421/article/details/123627753
- https://blog.csdn.net/qq_45043397/article/details/120141976?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166865308616782395327114%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=166865308616782395327114&biz_id=0&spm=1018.2226.3001.4187
- https://blog.csdn.net/weixin_45341885/article/details/120127451?ops_request_misc=&request_id=&biz_id=102&spm=1018.2226.3001.4187
- https://github.com/YanShuang17/binocular_stereo_vision