diff --git a/src/core/core.ts b/src/core/core.ts index 920ca0b..54ec167 100644 --- a/src/core/core.ts +++ b/src/core/core.ts @@ -17,6 +17,7 @@ import type { ransac_params_t } from "../motion_estimator/ransac_params_t"; import type { motion_estimator } from "../motion_estimator/motion_estimator"; import type { optical_flow_lk } from "../optical_flow_lk/optical_flow_lk"; import type { orb } from "../orb/orb"; +import type { affine2d, homography2d } from "../motion_model/motion_model"; /** * Base class of the library: holds the shared constants, the per-instance @@ -43,10 +44,8 @@ export default class jsfeatNext { static yape: typeof yape; static yape06: typeof yape06; static ransac_params_t: typeof ransac_params_t; - // affine2d / homography2d are still implemented inline in src/jsfeatNext.ts; - // these slots get precise `typeof` types as #47 extracts them into modules. - static affine2d: any; - static homography2d: any; + static affine2d: typeof affine2d; + static homography2d: typeof homography2d; static motion_estimator: typeof motion_estimator; static optical_flow_lk: typeof optical_flow_lk; static orb: typeof orb; diff --git a/src/jsfeatNext.ts b/src/jsfeatNext.ts index c744601..518dc5a 100644 --- a/src/jsfeatNext.ts +++ b/src/jsfeatNext.ts @@ -22,6 +22,7 @@ import { compute_laplacian, hessian_min_eigen_value } from "./yape06/yape06_util import { yape06 } from "./yape06/yape06"; import { ransac_params_t } from "./motion_estimator/ransac_params_t"; import { motion_estimator } from "./motion_estimator/motion_estimator"; +import { motion_model, affine2d, homography2d } from "./motion_model/motion_model"; import { optical_flow_lk } from "./optical_flow_lk/optical_flow_lk"; import { JSFEAT_CONSTANTS } from "./constants/constants"; @@ -30,481 +31,6 @@ import { JSFEAT_CONSTANTS } from "./constants/constants"; // way to becoming a thin aggregator that only attaches the modules. export default jsfeatNext; -class motion_model extends jsfeatNext { - public T0: matrix_t; - public T1: matrix_t; - public AtA: matrix_t; - public AtB: matrix_t; - - constructor() { - super(); - this.T0 = new matrix_t(3, 3, JSFEAT_CONSTANTS.F32_t | JSFEAT_CONSTANTS.C1_t); - this.T1 = new matrix_t(3, 3, JSFEAT_CONSTANTS.F32_t | JSFEAT_CONSTANTS.C1_t); - this.AtA = new matrix_t(6, 6, JSFEAT_CONSTANTS.F32_t | JSFEAT_CONSTANTS.C1_t); - this.AtB = new matrix_t(6, 1, JSFEAT_CONSTANTS.F32_t | JSFEAT_CONSTANTS.C1_t); - } - - sqr(x: number): number { - return x * x; - } - - // does isotropic normalization - iso_normalize_points(from: point_t[], to: point_t[], T0: number[], T1: number[], count: number): void { - let i = 0; - let cx0 = 0.0, - cy0 = 0.0, - d0 = 0.0, - s0 = 0.0; - let cx1 = 0.0, - cy1 = 0.0, - d1 = 0.0, - s1 = 0.0; - let dx = 0.0, - dy = 0.0; - - for (; i < count; ++i) { - cx0 += from[i].x; - cy0 += from[i].y; - cx1 += to[i].x; - cy1 += to[i].y; - } - - cx0 /= count; - cy0 /= count; - cx1 /= count; - cy1 /= count; - - for (i = 0; i < count; ++i) { - dx = from[i].x - cx0; - dy = from[i].y - cy0; - d0 += Math.sqrt(dx * dx + dy * dy); - dx = to[i].x - cx1; - dy = to[i].y - cy1; - d1 += Math.sqrt(dx * dx + dy * dy); - } - - d0 /= count; - d1 /= count; - - s0 = Math.SQRT2 / d0; - s1 = Math.SQRT2 / d1; - - T0[0] = T0[4] = s0; - T0[2] = -cx0 * s0; - T0[5] = -cy0 * s0; - T0[1] = T0[3] = T0[6] = T0[7] = 0.0; - T0[8] = 1.0; - - T1[0] = T1[4] = s1; - T1[2] = -cx1 * s1; - T1[5] = -cy1 * s1; - T1[1] = T1[3] = T1[6] = T1[7] = 0.0; - T1[8] = 1.0; - } - - have_collinear_points(points: point_t[], count: number): boolean { - let j = 0, - k = 0, - i = (count - 1) | 0; - let dx1 = 0.0, - dy1 = 0.0, - dx2 = 0.0, - dy2 = 0.0; - - // check that the i-th selected point does not belong - // to a line connecting some previously selected points - for (; j < i; ++j) { - dx1 = points[j].x - points[i].x; - dy1 = points[j].y - points[i].y; - for (k = 0; k < j; ++k) { - dx2 = points[k].x - points[i].x; - dy2 = points[k].y - points[i].y; - if ( - Math.abs(dx2 * dy1 - dy2 * dx1) <= - JSFEAT_CONSTANTS.EPSILON * (Math.abs(dx1) + Math.abs(dy1) + Math.abs(dx2) + Math.abs(dy2)) - ) - return true; - } - } - return false; - } -} - -class affine2d extends motion_model { - constructor() { - super(); - } - - run(from: point_t[], to: point_t[], model: matrix_t, count: number): number { - let i = 0, - j = 0; - const dt = model.type | JSFEAT_CONSTANTS.C1_t; - const md = model.data, - t0d = this.T0.data, - t1d = this.T1.data; - let pt0, - pt1, - px = 0.0, - py = 0.0; - const _matmath = new matmath(); - const _linalg = new jsfeatNext.linalg(); - - this.iso_normalize_points(from, to, t0d, t1d, count); - - const a_buff = this.cache.get_buffer((2 * count * 6) << 3); - const b_buff = this.cache.get_buffer((2 * count) << 3); - - const a_mt = new matrix_t(6, 2 * count, dt, a_buff.data); - const b_mt = new matrix_t(1, 2 * count, dt, b_buff.data); - const ad = a_mt.data, - bd = b_mt.data; - - for (; i < count; ++i) { - pt0 = from[i]; - pt1 = to[i]; - - px = t0d[0] * pt0.x + t0d[1] * pt0.y + t0d[2]; - py = t0d[3] * pt0.x + t0d[4] * pt0.y + t0d[5]; - - j = i * 2 * 6; - (ad[j] = px), (ad[j + 1] = py), (ad[j + 2] = 1.0), (ad[j + 3] = 0.0), (ad[j + 4] = 0.0), (ad[j + 5] = 0.0); - - j += 6; - (ad[j] = 0.0), (ad[j + 1] = 0.0), (ad[j + 2] = 0.0), (ad[j + 3] = px), (ad[j + 4] = py), (ad[j + 5] = 1.0); - - bd[i << 1] = t1d[0] * pt1.x + t1d[1] * pt1.y + t1d[2]; - bd[(i << 1) + 1] = t1d[3] * pt1.x + t1d[4] * pt1.y + t1d[5]; - } - - _matmath.multiply_AtA(this.AtA, a_mt); - _matmath.multiply_AtB(this.AtB, a_mt, b_mt); - - _linalg.lu_solve(this.AtA, this.AtB); - - (md[0] = this.AtB.data[0]), (md[1] = this.AtB.data[1]), (md[2] = this.AtB.data[2]); - (md[3] = this.AtB.data[3]), (md[4] = this.AtB.data[4]), (md[5] = this.AtB.data[5]); - (md[6] = 0.0), (md[7] = 0.0), (md[8] = 1.0); // fill last row - - // denormalize - _matmath.invert_3x3(this.T1, this.T1); - _matmath.multiply_3x3(model, this.T1, model); - _matmath.multiply_3x3(model, model, this.T0); - - // free buffer - this.cache.put_buffer(a_buff); - this.cache.put_buffer(b_buff); - - return 1; - } - - // Per-point reprojection error for the affine model. Ported from original - // jsfeat's affine2d; jsfeatNext was missing it, which made RANSAC/LMEDS - // with an affine2d kernel throw. See issue #51. - error(from: point_t[], to: point_t[], model: matrix_t, err: Int32Array | Float32Array, count: number): void { - let i = 0; - let pt0, pt1; - const m = model.data; - - for (; i < count; ++i) { - pt0 = from[i]; - pt1 = to[i]; - - err[i] = - this.sqr(pt1.x - m[0] * pt0.x - m[1] * pt0.y - m[2]) + - this.sqr(pt1.y - m[3] * pt0.x - m[4] * pt0.y - m[5]); - } - } - - check_subset(from: point_t[], to: point_t[], count: number): boolean { - return true; // all good - } -} - -class homography2d extends motion_model { - public mLtL: matrix_t; - public Evec: matrix_t; - - constructor() { - super(); - this.mLtL = new matrix_t(9, 9, JSFEAT_CONSTANTS.F32_t | JSFEAT_CONSTANTS.C1_t); - this.Evec = new matrix_t(9, 9, JSFEAT_CONSTANTS.F32_t | JSFEAT_CONSTANTS.C1_t); - } - - run(from: point_t[], to: point_t[], model: matrix_t, count: number): number { - let i = 0, - j = 0; - const md = model.data, - t0d = this.T0.data, - t1d = this.T1.data; - const LtL = this.mLtL.data, - evd = this.Evec.data; - let x = 0.0, - y = 0.0, - X = 0.0, - Y = 0.0; - const _linalg = new jsfeatNext.linalg(); - const _matmath = new matmath(); - - // norm - let smx = 0.0, - smy = 0.0, - cmx = 0.0, - cmy = 0.0, - sMx = 0.0, - sMy = 0.0, - cMx = 0.0, - cMy = 0.0; - - for (; i < count; ++i) { - cmx += to[i].x; - cmy += to[i].y; - cMx += from[i].x; - cMy += from[i].y; - } - - cmx /= count; - cmy /= count; - cMx /= count; - cMy /= count; - - for (i = 0; i < count; ++i) { - smx += Math.abs(to[i].x - cmx); - smy += Math.abs(to[i].y - cmy); - sMx += Math.abs(from[i].x - cMx); - sMy += Math.abs(from[i].y - cMy); - } - - if ( - Math.abs(smx) < JSFEAT_CONSTANTS.EPSILON || - Math.abs(smy) < JSFEAT_CONSTANTS.EPSILON || - Math.abs(sMx) < JSFEAT_CONSTANTS.EPSILON || - Math.abs(sMy) < JSFEAT_CONSTANTS.EPSILON - ) - return 0; - - smx = count / smx; - smy = count / smy; - sMx = count / sMx; - sMy = count / sMy; - - t0d[0] = sMx; - t0d[1] = 0; - t0d[2] = -cMx * sMx; - t0d[3] = 0; - t0d[4] = sMy; - t0d[5] = -cMy * sMy; - t0d[6] = 0; - t0d[7] = 0; - t0d[8] = 1; - - t1d[0] = 1.0 / smx; - t1d[1] = 0; - t1d[2] = cmx; - t1d[3] = 0; - t1d[4] = 1.0 / smy; - t1d[5] = cmy; - t1d[6] = 0; - t1d[7] = 0; - t1d[8] = 1; - // - - // construct system - i = 81; - while (--i >= 0) { - LtL[i] = 0.0; - } - for (i = 0; i < count; ++i) { - x = (to[i].x - cmx) * smx; - y = (to[i].y - cmy) * smy; - X = (from[i].x - cMx) * sMx; - Y = (from[i].y - cMy) * sMy; - - LtL[0] += X * X; - LtL[1] += X * Y; - LtL[2] += X; - - LtL[6] += X * -x * X; - LtL[7] += X * -x * Y; - LtL[8] += X * -x; - LtL[10] += Y * Y; - LtL[11] += Y; - - LtL[15] += Y * -x * X; - LtL[16] += Y * -x * Y; - LtL[17] += Y * -x; - LtL[20] += 1.0; - - LtL[24] += -x * X; - LtL[25] += -x * Y; - LtL[26] += -x; - LtL[30] += X * X; - LtL[31] += X * Y; - LtL[32] += X; - LtL[33] += X * -y * X; - LtL[34] += X * -y * Y; - LtL[35] += X * -y; - LtL[40] += Y * Y; - LtL[41] += Y; - LtL[42] += Y * -y * X; - LtL[43] += Y * -y * Y; - LtL[44] += Y * -y; - LtL[50] += 1.0; - LtL[51] += -y * X; - LtL[52] += -y * Y; - LtL[53] += -y; - LtL[60] += -x * X * -x * X + -y * X * -y * X; - LtL[61] += -x * X * -x * Y + -y * X * -y * Y; - LtL[62] += -x * X * -x + -y * X * -y; - LtL[70] += -x * Y * -x * Y + -y * Y * -y * Y; - LtL[71] += -x * Y * -x + -y * Y * -y; - LtL[80] += -x * -x + -y * -y; - } - // - - // symmetry - for (i = 0; i < 9; ++i) { - for (j = 0; j < i; ++j) LtL[i * 9 + j] = LtL[j * 9 + i]; - } - - _linalg.eigenVV(this.mLtL, this.Evec); - - (md[0] = evd[72]), (md[1] = evd[73]), (md[2] = evd[74]); - (md[3] = evd[75]), (md[4] = evd[76]), (md[5] = evd[77]); - (md[6] = evd[78]), (md[7] = evd[79]), (md[8] = evd[80]); - - // denormalize - _matmath.multiply_3x3(model, this.T1, model); - _matmath.multiply_3x3(model, model, this.T0); - - // set bottom right to 1.0 - x = 1.0 / md[8]; - md[0] *= x; - md[1] *= x; - md[2] *= x; - md[3] *= x; - md[4] *= x; - md[5] *= x; - md[6] *= x; - md[7] *= x; - md[8] = 1.0; - - return 1; - } - - error(from: point_t[], to: point_t[], model: matrix_t, err: Int32Array | Float32Array, count: number): void { - let i = 0; - let pt0, - pt1, - ww = 0.0, - dx = 0.0, - dy = 0.0; - const m = model.data; - - for (; i < count; ++i) { - pt0 = from[i]; - pt1 = to[i]; - - ww = 1.0 / (m[6] * pt0.x + m[7] * pt0.y + 1.0); - dx = (m[0] * pt0.x + m[1] * pt0.y + m[2]) * ww - pt1.x; - dy = (m[3] * pt0.x + m[4] * pt0.y + m[5]) * ww - pt1.y; - err[i] = dx * dx + dy * dy; - } - } - - check_subset(from: point_t[], to: point_t[], count: number): boolean { - // seems to reject good subsets actually - //if( have_collinear_points(from, count) || have_collinear_points(to, count) ) { - //return false; - //} - const _matmath = new matmath(); - if (count == 4) { - let negative = 0; - - const fp0 = from[0], - fp1 = from[1], - fp2 = from[2], - fp3 = from[3]; - const tp0 = to[0], - tp1 = to[1], - tp2 = to[2], - tp3 = to[3]; - - // set1 - let A11 = fp0.x, - A12 = fp0.y, - A13 = 1.0; - let A21 = fp1.x, - A22 = fp1.y, - A23 = 1.0; - let A31 = fp2.x, - A32 = fp2.y, - A33 = 1.0; - - let B11 = tp0.x, - B12 = tp0.y, - B13 = 1.0; - let B21 = tp1.x, - B22 = tp1.y, - B23 = 1.0; - let B31 = tp2.x, - B32 = tp2.y, - B33 = 1.0; - - let detA = _matmath.determinant_3x3(A11, A12, A13, A21, A22, A23, A31, A32, A33); - let detB = _matmath.determinant_3x3(B11, B12, B13, B21, B22, B23, B31, B32, B33); - - if (detA * detB < 0) negative++; - - // set2 - (A11 = fp1.x), (A12 = fp1.y); - (A21 = fp2.x), (A22 = fp2.y); - (A31 = fp3.x), (A32 = fp3.y); - - (B11 = tp1.x), (B12 = tp1.y); - (B21 = tp2.x), (B22 = tp2.y); - (B31 = tp3.x), (B32 = tp3.y); - - detA = _matmath.determinant_3x3(A11, A12, A13, A21, A22, A23, A31, A32, A33); - detB = _matmath.determinant_3x3(B11, B12, B13, B21, B22, B23, B31, B32, B33); - - if (detA * detB < 0) negative++; - - // set3 - (A11 = fp0.x), (A12 = fp0.y); - (A21 = fp2.x), (A22 = fp2.y); - (A31 = fp3.x), (A32 = fp3.y); - - (B11 = tp0.x), (B12 = tp0.y); - (B21 = tp2.x), (B22 = tp2.y); - (B31 = tp3.x), (B32 = tp3.y); - - detA = _matmath.determinant_3x3(A11, A12, A13, A21, A22, A23, A31, A32, A33); - detB = _matmath.determinant_3x3(B11, B12, B13, B21, B22, B23, B31, B32, B33); - - if (detA * detB < 0) negative++; - - // set4 - (A11 = fp0.x), (A12 = fp0.y); - (A21 = fp1.x), (A22 = fp1.y); - (A31 = fp3.x), (A32 = fp3.y); - - (B11 = tp0.x), (B12 = tp0.y); - (B21 = tp1.x), (B22 = tp1.y); - (B31 = tp3.x), (B32 = tp3.y); - - detA = _matmath.determinant_3x3(A11, A12, A13, A21, A22, A23, A31, A32, A33); - detB = _matmath.determinant_3x3(B11, B12, B13, B21, B22, B23, B31, B32, B33); - - if (detA * detB < 0) negative++; - - if (negative != 0 && negative != 4) { - return false; - } - } - return true; // all good - } -} - jsfeatNext.cache = cache; jsfeatNext.pyramid_t = pyramid_t; @@ -531,292 +57,7 @@ jsfeatNext.yape = yape; jsfeatNext.yape06 = yape06; -jsfeatNext.motion_estimator = class motion_estimator extends jsfeatNext { - constructor() { - super(); - } - - get_subset( - kernel: homography2d, - from: point_t[], - to: point_t[], - need_cnt: number, - max_cnt: number, - from_sub: point_t[], - to_sub: point_t[] - ): boolean { - const max_try = 1000; - const indices = []; - let i = 0, - j = 0, - ssiter = 0, - idx_i = 0, - ok = false; - for (; ssiter < max_try; ++ssiter) { - i = 0; - for (; i < need_cnt && ssiter < max_try; ) { - ok = false; - idx_i = 0; - while (!ok) { - ok = true; - idx_i = indices[i] = Math.floor(Math.random() * max_cnt) | 0; - for (j = 0; j < i; ++j) { - if (idx_i == indices[j]) { - ok = false; - break; - } - } - } - from_sub[i] = from[idx_i]; - to_sub[i] = to[idx_i]; - if (!kernel.check_subset(from_sub, to_sub, i + 1)) { - ssiter++; - continue; - } - ++i; - } - break; - } - - return i == need_cnt && ssiter < max_try; - } - - find_inliers( - kernel: homography2d, - model: matrix_t, - from: point_t[], - to: point_t[], - count: number, - thresh: number, - err: Int32Array | Float32Array, - mask: number[] - ): number { - let numinliers: number = 0, - i = 0, - f = 0; - const t = thresh * thresh; - - kernel.error(from, to, model, err, count); - - for (; i < count; ++i) { - f = ((err[i] <= t)); - mask[i] = f; - numinliers += f; - } - return numinliers; - } - - ransac( - params: ransac_params_t, - kernel: any, - from: point_t[], - to: point_t[], - count: number, - model: matrix_t, - mask: matrix_t, - max_iters: number - ): boolean { - if (typeof max_iters === "undefined") { - max_iters = 1000; - } - - if (count < params.size) return false; - - const model_points = params.size; - let niters = max_iters, - iter = 0; - let result: boolean = false; - - const subset0: any = []; - const subset1: any = []; - let found = false; - - const mc = model.cols, - mr = model.rows; - const dt = model.type | JSFEAT_CONSTANTS.C1_t; - - const m_buff = this.cache.get_buffer((mc * mr) << 3); - const ms_buff = this.cache.get_buffer(count); - const err_buff = this.cache.get_buffer(count << 2); - const M = new matrix_t(mc, mr, dt, m_buff.data); - const curr_mask = new matrix_t(count, 1, JSFEAT_CONSTANTS.U8C1_t, ms_buff.data); - - let inliers_max = -1, - numinliers = 0; - let nmodels = 0; - - const err = err_buff.f32; - - // special case - if (count == model_points) { - if (kernel.run(from, to, M, count) <= 0) { - this.cache.put_buffer(m_buff); - this.cache.put_buffer(ms_buff); - this.cache.put_buffer(err_buff); - return false; - } - - M.copy_to(model); - if (mask) { - while (--count >= 0) { - mask.data[count] = 1; - } - } - this.cache.put_buffer(m_buff); - this.cache.put_buffer(ms_buff); - this.cache.put_buffer(err_buff); - return true; - } - - for (; iter < niters; ++iter) { - // generate subset - found = this.get_subset(kernel, from, to, model_points, count, subset0, subset1); - if (!found) { - if (iter == 0) { - this.cache.put_buffer(m_buff); - this.cache.put_buffer(ms_buff); - this.cache.put_buffer(err_buff); - return false; - } - break; - } - - nmodels = kernel.run(subset0, subset1, M, model_points); - if (nmodels <= 0) continue; - - // TODO handle multimodel output - - numinliers = this.find_inliers(kernel, M, from, to, count, params.thresh, err, curr_mask.data); - - if (numinliers > Math.max(inliers_max, model_points - 1)) { - M.copy_to(model); - inliers_max = numinliers; - if (mask) curr_mask.copy_to(mask); - niters = params.update_iters((count - numinliers) / count, niters); - result = true; - } - } - - this.cache.put_buffer(m_buff); - this.cache.put_buffer(ms_buff); - this.cache.put_buffer(err_buff); - - return result; - } - - lmeds( - params: ransac_params_t, - kernel: any, - from: point_t[], - to: point_t[], - count: number, - model: matrix_t, - mask: matrix_t, - max_iters: number - ): boolean { - if (typeof max_iters === "undefined") { - max_iters = 1000; - } - - if (count < params.size) return false; - - const model_points = params.size; - let niters = max_iters, - iter = 0; - let result: boolean = false; - const _math = new jsfeatNext.math(); - - const subset0: any = []; - const subset1: any = []; - let found = false; - - const mc = model.cols, - mr = model.rows; - const dt = model.type | JSFEAT_CONSTANTS.C1_t; - - const m_buff = this.cache.get_buffer((mc * mr) << 3); - const ms_buff = this.cache.get_buffer(count); - const err_buff = this.cache.get_buffer(count << 2); - const M = new matrix_t(mc, mr, dt, m_buff.data); - const curr_mask = new matrix_t(count, 1, JSFEAT_CONSTANTS.U8_t | JSFEAT_CONSTANTS.C1_t, ms_buff.data); - - let numinliers = 0; - let nmodels = 0; - - const err = err_buff.f32; - let min_median = 1000000000.0, - sigma = 0.0, - median = 0.0; - - params.eps = 0.45; - niters = params.update_iters(params.eps, niters); - - // special case - if (count == model_points) { - if (kernel.run(from, to, M, count) <= 0) { - this.cache.put_buffer(m_buff); - this.cache.put_buffer(ms_buff); - this.cache.put_buffer(err_buff); - return false; - } - - M.copy_to(model); - if (mask) { - while (--count >= 0) { - mask.data[count] = 1; - } - } - this.cache.put_buffer(m_buff); - this.cache.put_buffer(ms_buff); - this.cache.put_buffer(err_buff); - return true; - } - - for (; iter < niters; ++iter) { - // generate subset - found = this.get_subset(kernel, from, to, model_points, count, subset0, subset1); - if (!found) { - if (iter == 0) { - this.cache.put_buffer(m_buff); - this.cache.put_buffer(ms_buff); - this.cache.put_buffer(err_buff); - return false; - } - break; - } - - nmodels = kernel.run(subset0, subset1, M, model_points); - if (nmodels <= 0) continue; - - // TODO handle multimodel output - - kernel.error(from, to, M, err, count); - median = _math.median(err, 0, count - 1); - - if (median < min_median) { - min_median = median; - M.copy_to(model); - result = true; - } - } - - if (result) { - sigma = 2.5 * 1.4826 * (1 + 5.0 / (count - model_points)) * Math.sqrt(min_median); - sigma = Math.max(sigma, 0.001); - - numinliers = this.find_inliers(kernel, model, from, to, count, sigma, err, curr_mask.data); - if (mask) curr_mask.copy_to(mask); - - result = numinliers >= model_points; - } - - this.cache.put_buffer(m_buff); - this.cache.put_buffer(ms_buff); - this.cache.put_buffer(err_buff); - - return result; - } -}; +jsfeatNext.motion_estimator = motion_estimator; jsfeatNext.ransac_params_t = ransac_params_t; diff --git a/src/motion_estimator/motion_estimator.ts b/src/motion_estimator/motion_estimator.ts index d249c03..40ed428 100644 --- a/src/motion_estimator/motion_estimator.ts +++ b/src/motion_estimator/motion_estimator.ts @@ -1,10 +1,25 @@ +import jsfeatNext from "../core/core"; import { IHomography2d } from "../homography2d/homography2d"; import { matrix_t } from "../matrix_t/matrix_t"; import { point_t } from "../point_t/point_t"; import { ransac_params_t } from "./ransac_params_t"; -export class motion_estimator { +import { JSFEAT_CONSTANTS } from "../constants/constants"; +import { homography2d } from "../motion_model/motion_model"; +import { math } from "../math/math"; + +/** + * Real implementation, moved out of the src/jsfeatNext.ts monolith (issue #47). + * This file previously held a type-only stub — the implementation below is the + * inline code from the monolith, verbatim (the only change: lmeds instantiates + * the math module directly instead of via the jsfeatNext.math static slot). + */ +export class motion_estimator extends jsfeatNext { + constructor() { + super(); + } + get_subset( - kernel: IHomography2d, + kernel: homography2d, from: point_t[], to: point_t[], need_cnt: number, @@ -12,10 +27,44 @@ export class motion_estimator { from_sub: point_t[], to_sub: point_t[] ): boolean { - throw new Error("Method not implemented."); + const max_try = 1000; + const indices = []; + let i = 0, + j = 0, + ssiter = 0, + idx_i = 0, + ok = false; + for (; ssiter < max_try; ++ssiter) { + i = 0; + for (; i < need_cnt && ssiter < max_try; ) { + ok = false; + idx_i = 0; + while (!ok) { + ok = true; + idx_i = indices[i] = Math.floor(Math.random() * max_cnt) | 0; + for (j = 0; j < i; ++j) { + if (idx_i == indices[j]) { + ok = false; + break; + } + } + } + from_sub[i] = from[idx_i]; + to_sub[i] = to[idx_i]; + if (!kernel.check_subset(from_sub, to_sub, i + 1)) { + ssiter++; + continue; + } + ++i; + } + break; + } + + return i == need_cnt && ssiter < max_try; } + find_inliers( - kernel: IHomography2d, + kernel: homography2d, model: matrix_t, from: point_t[], to: point_t[], @@ -24,8 +73,21 @@ export class motion_estimator { err: Int32Array | Float32Array, mask: number[] ): number { - throw new Error("Method not implemented."); + let numinliers: number = 0, + i = 0, + f = 0; + const t = thresh * thresh; + + kernel.error(from, to, model, err, count); + + for (; i < count; ++i) { + f = ((err[i] <= t)); + mask[i] = f; + numinliers += f; + } + return numinliers; } + ransac( params: ransac_params_t, kernel: any, @@ -36,8 +98,94 @@ export class motion_estimator { mask: matrix_t, max_iters: number ): boolean { - throw new Error("Method not implemented."); + if (typeof max_iters === "undefined") { + max_iters = 1000; + } + + if (count < params.size) return false; + + const model_points = params.size; + let niters = max_iters, + iter = 0; + let result: boolean = false; + + const subset0: any = []; + const subset1: any = []; + let found = false; + + const mc = model.cols, + mr = model.rows; + const dt = model.type | JSFEAT_CONSTANTS.C1_t; + + const m_buff = this.cache.get_buffer((mc * mr) << 3); + const ms_buff = this.cache.get_buffer(count); + const err_buff = this.cache.get_buffer(count << 2); + const M = new matrix_t(mc, mr, dt, m_buff.data); + const curr_mask = new matrix_t(count, 1, JSFEAT_CONSTANTS.U8C1_t, ms_buff.data); + + let inliers_max = -1, + numinliers = 0; + let nmodels = 0; + + const err = err_buff.f32; + + // special case + if (count == model_points) { + if (kernel.run(from, to, M, count) <= 0) { + this.cache.put_buffer(m_buff); + this.cache.put_buffer(ms_buff); + this.cache.put_buffer(err_buff); + return false; + } + + M.copy_to(model); + if (mask) { + while (--count >= 0) { + mask.data[count] = 1; + } + } + this.cache.put_buffer(m_buff); + this.cache.put_buffer(ms_buff); + this.cache.put_buffer(err_buff); + return true; + } + + for (; iter < niters; ++iter) { + // generate subset + found = this.get_subset(kernel, from, to, model_points, count, subset0, subset1); + if (!found) { + if (iter == 0) { + this.cache.put_buffer(m_buff); + this.cache.put_buffer(ms_buff); + this.cache.put_buffer(err_buff); + return false; + } + break; + } + + nmodels = kernel.run(subset0, subset1, M, model_points); + if (nmodels <= 0) continue; + + // TODO handle multimodel output + + numinliers = this.find_inliers(kernel, M, from, to, count, params.thresh, err, curr_mask.data); + + if (numinliers > Math.max(inliers_max, model_points - 1)) { + M.copy_to(model); + inliers_max = numinliers; + if (mask) curr_mask.copy_to(mask); + niters = params.update_iters((count - numinliers) / count, niters); + result = true; + } + } + + this.cache.put_buffer(m_buff); + this.cache.put_buffer(ms_buff); + this.cache.put_buffer(err_buff); + + return result; } + lmeds( params: ransac_params_t, kernel: any, @@ -48,6 +196,106 @@ export class motion_estimator { mask: matrix_t, max_iters: number ): boolean { - throw new Error("Method not implemented."); + if (typeof max_iters === "undefined") { + max_iters = 1000; + } + + if (count < params.size) return false; + + const model_points = params.size; + let niters = max_iters, + iter = 0; + let result: boolean = false; + const _math = new math(); + + const subset0: any = []; + const subset1: any = []; + let found = false; + + const mc = model.cols, + mr = model.rows; + const dt = model.type | JSFEAT_CONSTANTS.C1_t; + + const m_buff = this.cache.get_buffer((mc * mr) << 3); + const ms_buff = this.cache.get_buffer(count); + const err_buff = this.cache.get_buffer(count << 2); + const M = new matrix_t(mc, mr, dt, m_buff.data); + const curr_mask = new matrix_t(count, 1, JSFEAT_CONSTANTS.U8_t | JSFEAT_CONSTANTS.C1_t, ms_buff.data); + + let numinliers = 0; + let nmodels = 0; + + const err = err_buff.f32; + let min_median = 1000000000.0, + sigma = 0.0, + median = 0.0; + + params.eps = 0.45; + niters = params.update_iters(params.eps, niters); + + // special case + if (count == model_points) { + if (kernel.run(from, to, M, count) <= 0) { + this.cache.put_buffer(m_buff); + this.cache.put_buffer(ms_buff); + this.cache.put_buffer(err_buff); + return false; + } + + M.copy_to(model); + if (mask) { + while (--count >= 0) { + mask.data[count] = 1; + } + } + this.cache.put_buffer(m_buff); + this.cache.put_buffer(ms_buff); + this.cache.put_buffer(err_buff); + return true; + } + + for (; iter < niters; ++iter) { + // generate subset + found = this.get_subset(kernel, from, to, model_points, count, subset0, subset1); + if (!found) { + if (iter == 0) { + this.cache.put_buffer(m_buff); + this.cache.put_buffer(ms_buff); + this.cache.put_buffer(err_buff); + return false; + } + break; + } + + nmodels = kernel.run(subset0, subset1, M, model_points); + if (nmodels <= 0) continue; + + // TODO handle multimodel output + + kernel.error(from, to, M, err, count); + median = _math.median(err, 0, count - 1); + + if (median < min_median) { + min_median = median; + M.copy_to(model); + result = true; + } + } + + if (result) { + sigma = 2.5 * 1.4826 * (1 + 5.0 / (count - model_points)) * Math.sqrt(min_median); + sigma = Math.max(sigma, 0.001); + + numinliers = this.find_inliers(kernel, model, from, to, count, sigma, err, curr_mask.data); + if (mask) curr_mask.copy_to(mask); + + result = numinliers >= model_points; + } + + this.cache.put_buffer(m_buff); + this.cache.put_buffer(ms_buff); + this.cache.put_buffer(err_buff); + + return result; } } diff --git a/src/motion_model/motion_model.ts b/src/motion_model/motion_model.ts new file mode 100644 index 0000000..6bdd68e --- /dev/null +++ b/src/motion_model/motion_model.ts @@ -0,0 +1,488 @@ +import jsfeatNext from "../core/core"; +import { matrix_t } from "../matrix_t/matrix_t"; +import { point_t } from "../point_t/point_t"; +import { JSFEAT_CONSTANTS } from "../constants/constants"; +import matmath from "../matmath/matmath"; +import { linalg } from "../linalg/linalg"; + +/** + * Motion-model kernels for motion_estimator (issue #47): the motion_model + * base plus the affine2d and homography2d kernels, moved verbatim from the + * src/jsfeatNext.ts monolith (the only change: kernels instantiate linalg + * via direct module import instead of the jsfeatNext.linalg static slot). + * In original jsfeat these live under the jsfeat.motion_model namespace. + */ +export class motion_model extends jsfeatNext { + public T0: matrix_t; + public T1: matrix_t; + public AtA: matrix_t; + public AtB: matrix_t; + + constructor() { + super(); + this.T0 = new matrix_t(3, 3, JSFEAT_CONSTANTS.F32_t | JSFEAT_CONSTANTS.C1_t); + this.T1 = new matrix_t(3, 3, JSFEAT_CONSTANTS.F32_t | JSFEAT_CONSTANTS.C1_t); + this.AtA = new matrix_t(6, 6, JSFEAT_CONSTANTS.F32_t | JSFEAT_CONSTANTS.C1_t); + this.AtB = new matrix_t(6, 1, JSFEAT_CONSTANTS.F32_t | JSFEAT_CONSTANTS.C1_t); + } + + sqr(x: number): number { + return x * x; + } + + // does isotropic normalization + iso_normalize_points(from: point_t[], to: point_t[], T0: number[], T1: number[], count: number): void { + let i = 0; + let cx0 = 0.0, + cy0 = 0.0, + d0 = 0.0, + s0 = 0.0; + let cx1 = 0.0, + cy1 = 0.0, + d1 = 0.0, + s1 = 0.0; + let dx = 0.0, + dy = 0.0; + + for (; i < count; ++i) { + cx0 += from[i].x; + cy0 += from[i].y; + cx1 += to[i].x; + cy1 += to[i].y; + } + + cx0 /= count; + cy0 /= count; + cx1 /= count; + cy1 /= count; + + for (i = 0; i < count; ++i) { + dx = from[i].x - cx0; + dy = from[i].y - cy0; + d0 += Math.sqrt(dx * dx + dy * dy); + dx = to[i].x - cx1; + dy = to[i].y - cy1; + d1 += Math.sqrt(dx * dx + dy * dy); + } + + d0 /= count; + d1 /= count; + + s0 = Math.SQRT2 / d0; + s1 = Math.SQRT2 / d1; + + T0[0] = T0[4] = s0; + T0[2] = -cx0 * s0; + T0[5] = -cy0 * s0; + T0[1] = T0[3] = T0[6] = T0[7] = 0.0; + T0[8] = 1.0; + + T1[0] = T1[4] = s1; + T1[2] = -cx1 * s1; + T1[5] = -cy1 * s1; + T1[1] = T1[3] = T1[6] = T1[7] = 0.0; + T1[8] = 1.0; + } + + have_collinear_points(points: point_t[], count: number): boolean { + let j = 0, + k = 0, + i = (count - 1) | 0; + let dx1 = 0.0, + dy1 = 0.0, + dx2 = 0.0, + dy2 = 0.0; + + // check that the i-th selected point does not belong + // to a line connecting some previously selected points + for (; j < i; ++j) { + dx1 = points[j].x - points[i].x; + dy1 = points[j].y - points[i].y; + for (k = 0; k < j; ++k) { + dx2 = points[k].x - points[i].x; + dy2 = points[k].y - points[i].y; + if ( + Math.abs(dx2 * dy1 - dy2 * dx1) <= + JSFEAT_CONSTANTS.EPSILON * (Math.abs(dx1) + Math.abs(dy1) + Math.abs(dx2) + Math.abs(dy2)) + ) + return true; + } + } + return false; + } +} + +export class affine2d extends motion_model { + constructor() { + super(); + } + + run(from: point_t[], to: point_t[], model: matrix_t, count: number): number { + let i = 0, + j = 0; + const dt = model.type | JSFEAT_CONSTANTS.C1_t; + const md = model.data, + t0d = this.T0.data, + t1d = this.T1.data; + let pt0, + pt1, + px = 0.0, + py = 0.0; + const _matmath = new matmath(); + const _linalg = new linalg(); + + this.iso_normalize_points(from, to, t0d, t1d, count); + + const a_buff = this.cache.get_buffer((2 * count * 6) << 3); + const b_buff = this.cache.get_buffer((2 * count) << 3); + + const a_mt = new matrix_t(6, 2 * count, dt, a_buff.data); + const b_mt = new matrix_t(1, 2 * count, dt, b_buff.data); + const ad = a_mt.data, + bd = b_mt.data; + + for (; i < count; ++i) { + pt0 = from[i]; + pt1 = to[i]; + + px = t0d[0] * pt0.x + t0d[1] * pt0.y + t0d[2]; + py = t0d[3] * pt0.x + t0d[4] * pt0.y + t0d[5]; + + j = i * 2 * 6; + (ad[j] = px), (ad[j + 1] = py), (ad[j + 2] = 1.0), (ad[j + 3] = 0.0), (ad[j + 4] = 0.0), (ad[j + 5] = 0.0); + + j += 6; + (ad[j] = 0.0), (ad[j + 1] = 0.0), (ad[j + 2] = 0.0), (ad[j + 3] = px), (ad[j + 4] = py), (ad[j + 5] = 1.0); + + bd[i << 1] = t1d[0] * pt1.x + t1d[1] * pt1.y + t1d[2]; + bd[(i << 1) + 1] = t1d[3] * pt1.x + t1d[4] * pt1.y + t1d[5]; + } + + _matmath.multiply_AtA(this.AtA, a_mt); + _matmath.multiply_AtB(this.AtB, a_mt, b_mt); + + _linalg.lu_solve(this.AtA, this.AtB); + + (md[0] = this.AtB.data[0]), (md[1] = this.AtB.data[1]), (md[2] = this.AtB.data[2]); + (md[3] = this.AtB.data[3]), (md[4] = this.AtB.data[4]), (md[5] = this.AtB.data[5]); + (md[6] = 0.0), (md[7] = 0.0), (md[8] = 1.0); // fill last row + + // denormalize + _matmath.invert_3x3(this.T1, this.T1); + _matmath.multiply_3x3(model, this.T1, model); + _matmath.multiply_3x3(model, model, this.T0); + + // free buffer + this.cache.put_buffer(a_buff); + this.cache.put_buffer(b_buff); + + return 1; + } + + // Per-point reprojection error for the affine model. Ported from original + // jsfeat's affine2d; jsfeatNext was missing it, which made RANSAC/LMEDS + // with an affine2d kernel throw. See issue #51. + error(from: point_t[], to: point_t[], model: matrix_t, err: Int32Array | Float32Array, count: number): void { + let i = 0; + let pt0, pt1; + const m = model.data; + + for (; i < count; ++i) { + pt0 = from[i]; + pt1 = to[i]; + + err[i] = + this.sqr(pt1.x - m[0] * pt0.x - m[1] * pt0.y - m[2]) + + this.sqr(pt1.y - m[3] * pt0.x - m[4] * pt0.y - m[5]); + } + } + + check_subset(from: point_t[], to: point_t[], count: number): boolean { + return true; // all good + } +} + +export class homography2d extends motion_model { + public mLtL: matrix_t; + public Evec: matrix_t; + + constructor() { + super(); + this.mLtL = new matrix_t(9, 9, JSFEAT_CONSTANTS.F32_t | JSFEAT_CONSTANTS.C1_t); + this.Evec = new matrix_t(9, 9, JSFEAT_CONSTANTS.F32_t | JSFEAT_CONSTANTS.C1_t); + } + + run(from: point_t[], to: point_t[], model: matrix_t, count: number): number { + let i = 0, + j = 0; + const md = model.data, + t0d = this.T0.data, + t1d = this.T1.data; + const LtL = this.mLtL.data, + evd = this.Evec.data; + let x = 0.0, + y = 0.0, + X = 0.0, + Y = 0.0; + const _linalg = new linalg(); + const _matmath = new matmath(); + + // norm + let smx = 0.0, + smy = 0.0, + cmx = 0.0, + cmy = 0.0, + sMx = 0.0, + sMy = 0.0, + cMx = 0.0, + cMy = 0.0; + + for (; i < count; ++i) { + cmx += to[i].x; + cmy += to[i].y; + cMx += from[i].x; + cMy += from[i].y; + } + + cmx /= count; + cmy /= count; + cMx /= count; + cMy /= count; + + for (i = 0; i < count; ++i) { + smx += Math.abs(to[i].x - cmx); + smy += Math.abs(to[i].y - cmy); + sMx += Math.abs(from[i].x - cMx); + sMy += Math.abs(from[i].y - cMy); + } + + if ( + Math.abs(smx) < JSFEAT_CONSTANTS.EPSILON || + Math.abs(smy) < JSFEAT_CONSTANTS.EPSILON || + Math.abs(sMx) < JSFEAT_CONSTANTS.EPSILON || + Math.abs(sMy) < JSFEAT_CONSTANTS.EPSILON + ) + return 0; + + smx = count / smx; + smy = count / smy; + sMx = count / sMx; + sMy = count / sMy; + + t0d[0] = sMx; + t0d[1] = 0; + t0d[2] = -cMx * sMx; + t0d[3] = 0; + t0d[4] = sMy; + t0d[5] = -cMy * sMy; + t0d[6] = 0; + t0d[7] = 0; + t0d[8] = 1; + + t1d[0] = 1.0 / smx; + t1d[1] = 0; + t1d[2] = cmx; + t1d[3] = 0; + t1d[4] = 1.0 / smy; + t1d[5] = cmy; + t1d[6] = 0; + t1d[7] = 0; + t1d[8] = 1; + // + + // construct system + i = 81; + while (--i >= 0) { + LtL[i] = 0.0; + } + for (i = 0; i < count; ++i) { + x = (to[i].x - cmx) * smx; + y = (to[i].y - cmy) * smy; + X = (from[i].x - cMx) * sMx; + Y = (from[i].y - cMy) * sMy; + + LtL[0] += X * X; + LtL[1] += X * Y; + LtL[2] += X; + + LtL[6] += X * -x * X; + LtL[7] += X * -x * Y; + LtL[8] += X * -x; + LtL[10] += Y * Y; + LtL[11] += Y; + + LtL[15] += Y * -x * X; + LtL[16] += Y * -x * Y; + LtL[17] += Y * -x; + LtL[20] += 1.0; + + LtL[24] += -x * X; + LtL[25] += -x * Y; + LtL[26] += -x; + LtL[30] += X * X; + LtL[31] += X * Y; + LtL[32] += X; + LtL[33] += X * -y * X; + LtL[34] += X * -y * Y; + LtL[35] += X * -y; + LtL[40] += Y * Y; + LtL[41] += Y; + LtL[42] += Y * -y * X; + LtL[43] += Y * -y * Y; + LtL[44] += Y * -y; + LtL[50] += 1.0; + LtL[51] += -y * X; + LtL[52] += -y * Y; + LtL[53] += -y; + LtL[60] += -x * X * -x * X + -y * X * -y * X; + LtL[61] += -x * X * -x * Y + -y * X * -y * Y; + LtL[62] += -x * X * -x + -y * X * -y; + LtL[70] += -x * Y * -x * Y + -y * Y * -y * Y; + LtL[71] += -x * Y * -x + -y * Y * -y; + LtL[80] += -x * -x + -y * -y; + } + // + + // symmetry + for (i = 0; i < 9; ++i) { + for (j = 0; j < i; ++j) LtL[i * 9 + j] = LtL[j * 9 + i]; + } + + _linalg.eigenVV(this.mLtL, this.Evec); + + (md[0] = evd[72]), (md[1] = evd[73]), (md[2] = evd[74]); + (md[3] = evd[75]), (md[4] = evd[76]), (md[5] = evd[77]); + (md[6] = evd[78]), (md[7] = evd[79]), (md[8] = evd[80]); + + // denormalize + _matmath.multiply_3x3(model, this.T1, model); + _matmath.multiply_3x3(model, model, this.T0); + + // set bottom right to 1.0 + x = 1.0 / md[8]; + md[0] *= x; + md[1] *= x; + md[2] *= x; + md[3] *= x; + md[4] *= x; + md[5] *= x; + md[6] *= x; + md[7] *= x; + md[8] = 1.0; + + return 1; + } + + error(from: point_t[], to: point_t[], model: matrix_t, err: Int32Array | Float32Array, count: number): void { + let i = 0; + let pt0, + pt1, + ww = 0.0, + dx = 0.0, + dy = 0.0; + const m = model.data; + + for (; i < count; ++i) { + pt0 = from[i]; + pt1 = to[i]; + + ww = 1.0 / (m[6] * pt0.x + m[7] * pt0.y + 1.0); + dx = (m[0] * pt0.x + m[1] * pt0.y + m[2]) * ww - pt1.x; + dy = (m[3] * pt0.x + m[4] * pt0.y + m[5]) * ww - pt1.y; + err[i] = dx * dx + dy * dy; + } + } + + check_subset(from: point_t[], to: point_t[], count: number): boolean { + // seems to reject good subsets actually + //if( have_collinear_points(from, count) || have_collinear_points(to, count) ) { + //return false; + //} + const _matmath = new matmath(); + if (count == 4) { + let negative = 0; + + const fp0 = from[0], + fp1 = from[1], + fp2 = from[2], + fp3 = from[3]; + const tp0 = to[0], + tp1 = to[1], + tp2 = to[2], + tp3 = to[3]; + + // set1 + let A11 = fp0.x, + A12 = fp0.y, + A13 = 1.0; + let A21 = fp1.x, + A22 = fp1.y, + A23 = 1.0; + let A31 = fp2.x, + A32 = fp2.y, + A33 = 1.0; + + let B11 = tp0.x, + B12 = tp0.y, + B13 = 1.0; + let B21 = tp1.x, + B22 = tp1.y, + B23 = 1.0; + let B31 = tp2.x, + B32 = tp2.y, + B33 = 1.0; + + let detA = _matmath.determinant_3x3(A11, A12, A13, A21, A22, A23, A31, A32, A33); + let detB = _matmath.determinant_3x3(B11, B12, B13, B21, B22, B23, B31, B32, B33); + + if (detA * detB < 0) negative++; + + // set2 + (A11 = fp1.x), (A12 = fp1.y); + (A21 = fp2.x), (A22 = fp2.y); + (A31 = fp3.x), (A32 = fp3.y); + + (B11 = tp1.x), (B12 = tp1.y); + (B21 = tp2.x), (B22 = tp2.y); + (B31 = tp3.x), (B32 = tp3.y); + + detA = _matmath.determinant_3x3(A11, A12, A13, A21, A22, A23, A31, A32, A33); + detB = _matmath.determinant_3x3(B11, B12, B13, B21, B22, B23, B31, B32, B33); + + if (detA * detB < 0) negative++; + + // set3 + (A11 = fp0.x), (A12 = fp0.y); + (A21 = fp2.x), (A22 = fp2.y); + (A31 = fp3.x), (A32 = fp3.y); + + (B11 = tp0.x), (B12 = tp0.y); + (B21 = tp2.x), (B22 = tp2.y); + (B31 = tp3.x), (B32 = tp3.y); + + detA = _matmath.determinant_3x3(A11, A12, A13, A21, A22, A23, A31, A32, A33); + detB = _matmath.determinant_3x3(B11, B12, B13, B21, B22, B23, B31, B32, B33); + + if (detA * detB < 0) negative++; + + // set4 + (A11 = fp0.x), (A12 = fp0.y); + (A21 = fp1.x), (A22 = fp1.y); + (A31 = fp3.x), (A32 = fp3.y); + + (B11 = tp0.x), (B12 = tp0.y); + (B21 = tp1.x), (B22 = tp1.y); + (B31 = tp3.x), (B32 = tp3.y); + + detA = _matmath.determinant_3x3(A11, A12, A13, A21, A22, A23, A31, A32, A33); + detB = _matmath.determinant_3x3(B11, B12, B13, B21, B22, B23, B31, B32, B33); + + if (detA * detB < 0) negative++; + + if (negative != 0 && negative != 4) { + return false; + } + } + return true; // all good + } +}