Hard Sums

// 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 };