Hard Sums
unknown
javascript
2 years ago
4.9 kB
7
Indexable
// import gl-matrix library import { mat3, mat4, quat, vec3 } from 'gl-matrix'; // define input arrays const basePoints3D = [[x0, y0, z0], [x1, y1, z1], [x2, y2, z2], [x3, y3, z3], [x4, y4, z4], [x5, y5, z5]]; const rotatedPoints2D = [[u0, v0], [u1, v1], [u2, v2], [u3, v3], [u4, v4], [u5, v5]]; // define camera intrinsic parameters (focal length and principal point) const fx = 1000; // focal length in x direction const fy = 1000; // focal length in y direction const cx = 320; // x-coordinate of principal point const cy = 240; // y-coordinate of principal point // create a camera matrix from the intrinsic parameters const cameraMatrix = mat3.fromValues(fx, 0, cx, 0, fy, cy, 0, 0, 1); // create arrays of 3D and 2D points const objectPoints = basePoints3D.flat(); const imagePoints = rotatedPoints2D.flat(); // define distortion coefficients (assuming no distortion) const distCoeffs = [0, 0, 0, 0, 0]; // define initial guess for rotation and translation const initR = mat3.create(); const initT = vec3.create(); // estimate pose using solvePnP function const { R, T } = solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, initR, initT); // log the rotation and translation matrices console.log(`Rotation Matrix: ${R}`); console.log(`Translation Matrix: ${T}`); function solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, initR, initT) { // convert 3D object points and 2D image points to homogeneous coordinates const objectPointsHomogeneous = objectPoints.map((point) => [point[0], point[1], point[2], 1]); const imagePointsHomogeneous = imagePoints.map((point) => [point[0], point[1], 1]); // estimate 2D-3D correspondences matrix A const A = []; for (let i = 0; i < objectPoints.length; i++) { const X = objectPointsHomogeneous[i]; const x = imagePointsHomogeneous[i]; const a1 = [-X[0], -X[1], -X[2], -1, 0, 0, 0, 0, x[0] * X[0], x[0] * X[1], x[0] * X[2], x[0]]; const a2 = [0, 0, 0, 0, -X[0], -X[1], -X[2], -1, x[1] * X[0], x[1] * X[1], x[1] * X[2], x[1]]; A.push(a1, a2); } // estimate 3D rotation and translation from A using SVD const svd = singularValueDecomposition(A); const V = svd.V; const Rvec = V.slice(-1)[0].slice(0, -1); const R = mat3.create(); quat.fromValues(Rvec[1], Rvec[2], Rvec[3], Rvec[0], R); const T = vec3.fromValues(V[V.length - 1][11], V[V.length - 1][12], V[V.length - 1][13]); // refine estimate using iterative algorithm const objectPointsMat = mat4.create(); mat4.fromRotationTranslation(objectPointsMat, R, T); const imagePointsMat = mat3.create(); mat3.mul(imagePointsMat, cameraMatrix, objectPointsMat.subarray(0, 3)); const imagePointsMatInv = mat3.create(); mat3.invert(imagePointsMatInv, imagePointsMat); const objectiveFunction = (x) => { const r = x.slice(0, 3); const t = x.slice(3); const rMat = mat3.create(); quat.normalize(r, r); quat.toMat3(r, rMat); const transformedPointsMat = mat4.create(); mat4.fromRotationTranslation(transformedPointsMat, rMat, t); const transformedPoints = objectPoints.map((point) => { const pointMat = vec4.fromValues(point[0], point[1], point[2], 1); vec4.transformMat4(pointMat, pointMat, transformedPointsMat); return [pointMat[0] / pointMat[3], pointMat[1] / pointMat[3], pointMat[2] / pointMat[3]]; }).flat(); const projectedPoints = imagePoints.map((point) => { const pointMat = vec3.fromValues(point[0], point[1], 1); vec3.transformMat3(pointMat, pointMat, imagePointsMatInv); return [pointMat[0] / pointMat[2], pointMat[1] / pointMat[2]]; }).flat(); const residuals = []; for (let i = 0; i < transformedPoints.length; i++) { residuals.push(transformedPoints[i] - projectedPoints[i]); } return residuals; }; const x0 = [...Rvec, ...T]; const { solution: x } = levenbergMarquardt(objectiveFunction, x0); // extract refined rotation and translation from result const r = x.slice(0, 4); const t = x.slice(4); const rMat = mat3.create(); quat.normalize(r, r); quat.toMat3(r, rMat); const transformedPointsMat = mat4.create(); mat4.fromRotationTranslation(transformedPointsMat, rMat, t); const transformedPoints = objectPoints.map((point) => { const pointMat = vec4.fromValues(point[0], point[1], point[2], 1); vec4.transformMat4(pointMat, pointMat, transformedPointsMat); return [pointMat[0] / pointMat[3], pointMat[1] / pointMat[3], pointMat[2] / pointMat[3]]; }).flat(); const projectedPoints = imagePoints.map((point) => { const pointMat = vec3.fromValues(point[0], point[1], 1); vec3.transformMat3(pointMat, pointMat, imagePointsMatInv); return [pointMat[0] / pointMat[2], pointMat[1] / pointMat[2]]; }).flat(); return { rotation: r, translation: t, projectedPoints, transformedPoints };
Editor is loading...