29 matrix_t ref_point_matrix[ref_points_num][3],
39 for (i = 0; i < ref_points_num; i++) {
44 denom = f_i + dist_vec[i];
46 JTf[0] += (point[0] - ref_point_matrix[i][0]) * f_i
48 JTf[1] += (point[1] - ref_point_matrix[i][1]) * f_i
50 JTf[2] += (point[2] - ref_point_matrix[i][2]) * f_i
57 matrix_t ref_point_matrix[ref_points_num][3],
67 for (i = 0; i < ref_points_num; i++) {
71 dist_vec[i]) + dist_vec[i];
74 JTJ[0][0] += (point[0] - ref_point_matrix[i][0]) *
75 (point[0] - ref_point_matrix[i][0])
77 JTJ[1][1] += (point[1] - ref_point_matrix[i][1]) *
78 (point[1] - ref_point_matrix[i][1])
80 JTJ[2][2] += (point[2] - ref_point_matrix[i][2]) *
81 (point[2] - ref_point_matrix[i][2])
83 JTJ[0][1] += (point[0] - ref_point_matrix[i][0]) *
84 (point[1] - ref_point_matrix[i][1])
86 JTJ[0][2] += (point[0] - ref_point_matrix[i][0]) *
87 (point[2] - ref_point_matrix[i][2])
89 JTJ[1][2] += (point[1] - ref_point_matrix[i][1]) *
90 (point[2] - ref_point_matrix[i][2])
95 JTJ[1][0] = JTJ[0][1];
96 JTJ[2][0] = JTJ[0][2];
97 JTJ[2][1] = JTJ[1][2];
101 matrix_t ref_point_matrix[ref_points_num][3],
107 for (i = 0; i < ref_points_num; i++) {
112 - ref_point_matrix[i][0],
117 - ref_point_matrix[i][1],
121 - ref_point_matrix[i][2],
124 J[i][0] = (point[0] - ref_point_matrix[i][0]) / denom;
125 J[i][1] = (point[1] - ref_point_matrix[i][1]) / denom;
126 J[i][2] = (point[2] - ref_point_matrix[i][2]) / denom;
133 matrix_t ref_point_matrix[ref_points_num][3],
143 for (i = 0; i < ref_points_num; i++) {
148 - ref_point_matrix[i][0],
153 - ref_point_matrix[i][1],
157 - ref_point_matrix[i][2],
160 j_i_0 = (point[0] - ref_point_matrix[i][0]) / denom;
161 j_i_1 = (point[1] - ref_point_matrix[i][1]) / denom;
162 j_i_2 = (point[2] - ref_point_matrix[i][2]) / denom;
164 J_s[i] = j_i_0 * s[0] + j_i_1 * s[1] + j_i_2 * s[2];