/** * {@link #icpGetInitXw2XcSub}関数のmat_dを計算します。 */ private static NyARDoubleMatrix33 makeMatD(NyARDoubleMatrix44 i_cp, NyARDoublePoint2d[] pos2d, int i_num, NyARDoubleMatrix33 o_mat) { double m02 = 0; double m12 = 0; double m20 = 0; double m21 = 0; double m22 = 0; for (int i = 0; i < i_num; i++) { double cx = i_cp.m02 - pos2d[i].x; double cy = i_cp.m12 - pos2d[i].y; m02 += (cx) * i_cp.m00; m12 += (cx) * i_cp.m01 + (cy) * i_cp.m11; m20 += i_cp.m00 * (cx); m21 += i_cp.m01 * (cx) + i_cp.m11 * (cy); m22 += (cx) * (cx) + (cy) * (cy); } o_mat.m00 = (i_cp.m00 * i_cp.m00) * i_num; o_mat.m01 = (i_cp.m00 * i_cp.m01) * i_num; o_mat.m02 = m02; o_mat.m10 = (i_cp.m01 * i_cp.m00) * i_num; o_mat.m11 = (i_cp.m01 * i_cp.m01 + i_cp.m11 * i_cp.m11) * i_num; o_mat.m12 = m12; o_mat.m20 = m20; o_mat.m21 = m21; o_mat.m22 = m22; o_mat.inverse(o_mat); return(o_mat); }
/** * {@link #icpGetInitXw2XcSub}関数のmat_dを計算します。 */ private static NyARDoubleMatrix33 makeMatD(NyARDoubleMatrix44 i_cp, NyARDoublePoint2d[] pos2d, int i_num, NyARDoubleMatrix33 o_mat) { double m02 = 0; double m12 = 0; double m20 = 0; double m21 = 0; double m22 = 0; for (int i = 0; i < i_num; i++) { double cx = i_cp.m02 - pos2d[i].x; double cy = i_cp.m12 - pos2d[i].y; m02 += (cx) * i_cp.m00; m12 += (cx) * i_cp.m01 + (cy) * i_cp.m11; m20 += i_cp.m00 * (cx); m21 += i_cp.m01 * (cx) + i_cp.m11 * (cy); m22 += (cx) * (cx) + (cy) * (cy); } o_mat.m00 = (i_cp.m00 * i_cp.m00) * i_num; o_mat.m01 = (i_cp.m00 * i_cp.m01) * i_num; o_mat.m02 = m02; o_mat.m10 = (i_cp.m01 * i_cp.m00) * i_num; o_mat.m11 = (i_cp.m01 * i_cp.m01 + i_cp.m11 * i_cp.m11) * i_num; o_mat.m12 = m12; o_mat.m20 = m20; o_mat.m21 = m21; o_mat.m22 = m22; o_mat.inverse(o_mat); return o_mat; }
// private boolean ComputeSubpixelHessian( // float H[9],float b[3], // const Image& lap0,const Image& lap1,const Image& lap2, // int x,int y) private bool updateLocation(DogFeaturePoint kp, LaplacianImage lap0, LaplacianImage lap1, LaplacianImage lap2) { double[] tmp = new double[2]; double[] b = new double[3]; // Downsample the feature point to the detection octave bilinear_downsample_point(tmp, kp.x, kp.y, kp.octave); double xp = tmp[0]; double yp = tmp[1]; // Compute the discrete pixel location int x = (int)(xp + 0.5f); int y = (int)(yp + 0.5f); double[] H = new double[9]; if (lap0.getWidth() == lap1.getWidth() && lap1.getWidth() == lap2.getWidth()) { //すべての画像サイズが同じ //assert lap0.getHeight() == lap1.getHeight() && lap1.getHeight() == lap2.getHeight();// "Width/height are not consistent"); ComputeSubpixelHessianSameOctave(H, b, lap0, lap1, lap2, x, y); } else if ((lap0.getWidth() == lap1.getWidth()) && ((lap1.getWidth() >> 1) == lap2.getWidth())) { //0,1が同じで2がその半分 //assert (lap0.getHeight() == lap1.getHeight()) && ((lap1.getHeight() >> 1) == lap2.getHeight());// Width/height are not consistent"); ComputeSubpixelHessianFineOctavePair(H, b, lap0, lap1, lap2, x, y); } else if (((lap0.getWidth() >> 1) == lap1.getWidth()) && (lap1.getWidth() == lap2.getWidth())) { //0の半分が1,2 //assert ((lap0.getWidth() >> 1) == lap1.getWidth()) && (lap1.getWidth() == lap2.getWidth());// Width/height are not consistent"); ComputeSubpixelHessianCoarseOctavePair(H, b, lap0, lap1, lap2, x, y); } else { // ASSERT(0, "Image sizes are inconsistent"); return(false); } // A*u=b // if (!SolveSymmetricLinearSystem3x3(u, H, b)) { NyARDoubleMatrix33 m = new NyARDoubleMatrix33(); m.m00 = H[0]; m.m01 = H[1]; m.m02 = H[2]; m.m10 = H[3]; m.m11 = H[4]; m.m12 = H[5]; m.m20 = H[6]; m.m21 = H[7]; m.m22 = H[8]; if (!m.inverse(m)) { return(false); } double u0 = (m.m00 * b[0] + m.m01 * b[1] + m.m02 * b[2]); double u1 = (m.m10 * b[0] + m.m11 * b[1] + m.m12 * b[2]); double u2 = (m.m20 * b[0] + m.m21 * b[1] + m.m22 * b[2]); // If points move too much in the sub-pixel update, then the point probably unstable. if ((u0 * u0) + (u1 * u1) > mMaxSubpixelDistanceSqr) { return(false); } // Compute the edge score if (!ComputeEdgeScore(tmp, H)) { return(false); } kp.edge_score = tmp[0]; // Compute a linear estimate of the intensity // ASSERT(kp.score == lap1.get<float>(y)[x], // "Score is not consistent with the DoG image"); double[] lap1_buf = (double[])lap1.getBuffer(); kp.score = lap1_buf[lap1.get(y) + x] - (b[0] * u0 + b[1] * u1 + b[2] * u2); // Update the location: // Apply the update on the downsampled location and then upsample // the result. // bilinear_upsample_point(kp.x, kp.y, xp+u[0], yp+u[1], kp.octave); bilinear_upsample_point(tmp, xp + u0, yp + u1, kp.octave); kp.x = tmp[0]; kp.y = tmp[1]; // Update the scale kp.sp_scale = kp.scale + u2; kp.sp_scale = ClipScalar(kp.sp_scale, 0, mLaplacianPyramid.numScalePerOctave()); return(true); }