gpc.c
Go to the documentation of this file.
1 /* GPC: A library for the solution of General Point Correspondence problems.
2  Copyright (C) 2006 Andrea Censi (andrea at censi dot org)
3 
4  This program is free software; you can redistribute it and/or
5  modify it under the terms of the GNU General Public License
6  as published by the Free Software Foundation; either version 2
7  of the License, or (at your option) any later version.
8 
9  This program is distributed in the hope that it will be useful,
10  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  GNU General Public License for more details.
13 
14  You should have received a copy of the GNU General Public License
15  along with this program; if not, write to the Free Software
16  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17 */
18 
19 #include <math.h>
20 #include <gsl/gsl_math.h>
21 #include "gpc.h"
22 #include "gpc_utils.h"
23 
24 /* Note that we use static values here so we don't need to evaluate that all the time */
25 #define M(matrix, rows, col) static gsl_matrix*matrix=0; if(!matrix) matrix = gsl_matrix_alloc(rows,col);
26 #define MF(matrix) /*gsl_matrix_free(matrix)*/
27 
28 
29 int gpc_solve(int K, const struct gpc_corr*c,
30  const double*x0, const double *cov_x0,
31  double *x_out)
32 {
33  M(bigM, 4,4); M(g, 4,1); M(bigM_k,2,4);
34  M(bigM_k_t,4,2); M(C_k,2,2); M(q_k, 2,1);
35  M(temp41, 4,1); M(temp22,2,2); M(temp22b,2,2);
36  M(temp42, 4,2); M(temp44,4,4); M(temp21, 2,1);
37  M(temp22c, 2,2); M(temp12,1,2);
38 
39  gsl_matrix_set_zero(bigM);
40  gsl_matrix_set_zero(g);
41  gsl_matrix_set_zero(temp42);
42 
43  double d_bigM[4][4] = { {0,0,0,0}, {0,0,0,0}, {0,0,0,0}, {0,0,0,0}};
44  double d_g[4] = {0, 0, 0, 0};
45  int k;
46  for(k=0;k<K;k++) {
47  if(!c[k].valid) continue;
48 
49  double C00 = c[k].C[0][0];
50  double C01 = c[k].C[0][1];
51  double C10 = c[k].C[1][0];
52  double C11 = c[k].C[1][1];
53 
54  if(C01 != C10) {
55  fprintf(stderr, "k=%d; I expect C to be a symmetric matrix.\n", k);
56  return 0;
57  }
58 #if GPC_CHECK_SEMIDEF
59  double det = C00*C11 - C01*C10;
60  double trace = C00 + C11;
61  int is_semidef_pos = (det >= 0) && (trace>0);
62  if(!is_semidef_pos) {
63  fprintf(stderr, "k=%d; I expect the matrix to be semidef positive (det>=0 and trace>0), det = %.20f trace= %.10f C = [%.15f,%.15f;%.15f %.15f]\n",
64  k, det, trace, C00,C01,C10,C11);
65  return 0;
66  }
67 #endif
68 
69  double qx = c[k].q[0];
70  double qy = c[k].q[1];
71  double px = c[k].p[0];
72  double py = c[k].p[1];
73 
74  /* [ C00, c01, px C00 + c01 py , c01 px - py C00 ] */
75  d_bigM[0][0] += C00;
76  d_bigM[0][1] += C01;
77  d_bigM[0][2] += +px * C00 + py * C01;
78  d_bigM[0][3] += -py * C00 + px * C01;
79 
80  /* [ C10 , C11 , py C11 + px C10 , px C11 - py C10 ] */
81  d_bigM[1][0] += C10;
82  d_bigM[1][1] += C11;
83  d_bigM[1][2] += +px * C10 + py * C11;
84  d_bigM[1][3] += +px * C11 - py * C10;
85 
86  /*Col 1 = [ py C10 + px C00 ]
87  Col 2 = [ py C11 + c01 px ]
88  Col 3 = [ py (py C11 + px C10) + px (px C00 + c01 py) ]
89  Col 4 = [ py (px C11 - py C10) + px (c01 px - py C00) ]
90  */
91  d_bigM[2][0] += px * C00 + py * C10;
92  d_bigM[2][1] += px * C01 + py * C11;
93  d_bigM[2][2] += (px*px)*(+C00) + (px*py)*(+C10+C01) + (py*py)*(+C11);
94  d_bigM[2][3] += (px*px)*(+C01) + (px*py)*(-C00+C11) + (py*py)*(-C10);
95 
96  /*Col 1 = [ px C10 - py C00 ]
97  Col 2 = [ px C11 - c01 py ]
98  Col 3 = [ px (py C11 + px C10) - py (px C00 + c01 py) ]
99  Col 4 = [ px (px C11 - py C10) - py (c01 px - py C00) ]*/
100  d_bigM[3][0] += -py * C00 + px * C10;
101  d_bigM[3][1] += -py * C01 + px * C11;
102  d_bigM[3][2] += (px*px)*(+C10) + (px*py)*(-C00+C11) + (py*py)*(-C01);
103  d_bigM[3][3] += (px*px)*(+C11) + (px*py)*(-C10-C01) + (py*py)*(+C00);
104 
105  d_g[0] += C00*qx+C10*qy;
106  d_g[1] += C01*qx+C11*qy;
107  d_g[2] += qx * (C00*px+C01*py) + qy * (C10*px+C11*py);
108  d_g[3] += qx * (C00*(-py)+C01*px) + qy * (C10*(-py)+C11*px);
109 
110  }
111 
112  {
113  unsigned int a,b;
114  for(a=0;a<4;a++)
115  *gsl_matrix_ptr(g, a, 0) = -2 * d_g[a];
116  for(a=0;a<4;a++)
117  for(b=0;b<4;b++)
118  gsl_matrix_set(bigM, a, b, 2 * d_bigM[a][b]);
119  }
120 
121 
122  M(mA,2,2); gms(mA,0,0, gmg(bigM,0,0)); gms(mA,0,1, gmg(bigM,0,1));
123  gms(mA,1,0, gmg(bigM,1,0)); gms(mA,1,1, gmg(bigM,1,1));
124  M(mB,2,2); gms(mB,0,0, gmg(bigM,0,2)); gms(mB,0,1, gmg(bigM,0,3));
125  gms(mB,1,0, gmg(bigM,1,2)); gms(mB,1,1, gmg(bigM,1,3));
126  M(mD,2,2); gms(mD,0,0, gmg(bigM,2,2)); gms(mD,0,1, gmg(bigM,2,3));
127  gms(mD,1,0, gmg(bigM,3,2)); gms(mD,1,1, gmg(bigM,3,3));
128 
129  M(mS,2,2); M(mSa,2,2);
130 
131 
132  /* mS = mD - mB.trans * mA.inv * mB;
133  temp22b = inv(A) */
134  m_inv(mA, temp22b);
135  /* temp22c = inv(A) * mB */
136  m_mult(temp22b, mB, temp22c);
137  /* temp22 = mB' */
138  m_trans(mB, temp22);
139  m_mult(temp22, temp22c, temp22b);
140  m_scale(-1.0,temp22b);
141  m_add(mD,temp22b,mS);
142 
143  /* mSa = mS.inv * mS.det; */
144  m_inv(mS, mSa);
145  m_scale(m_det(mS), mSa);
146 
147  if(TRACE_ALGO) {
148  m_display("mA",mA);
149  m_display("mB",mB);
150  m_display("mD",mD);
151  m_display("mS",mS);
152  m_display("mSa",mSa);
153  }
154 
155  M(g1,2,1); M(g2,2,1);
156  M(g1t,1,2); M(g2t,1,2);
157  M(mAi,2,2); M(mBt,2,2);
158 
159  gms(g1,0,0,gmg(g,0,0));
160  gms(g1,1,0,gmg(g,1,0));
161  gms(g2,0,0,gmg(g,2,0));
162  gms(g2,1,0,gmg(g,3,0));
163  m_trans(g1, g1t);
164  m_trans(g2, g2t);
165  m_trans(mB, mBt);
166  m_inv(mA, mAi);
167 
168  M(m1t,1,2); M(m1,2,1);
169  M(m2t,1,2); M(m2,2,1);
170  M(m3t,1,2); M(m3,2,1);
171 
172  /* m1t = g1t*mAi*mB */
173  m_mult(g1t,mAi,temp12);
174  m_mult(temp12,mB,m1t);
175 
176  m_trans(m1t,m1);
177  /* m2t = m1t*mSa */
178  m_mult(m1t,mSa,m2t);
179  m_trans(m2t,m2);
180  /* m3t = g2t*mSa */
181  m_mult(g2t,mSa,m3t);
182  m_trans(m3t,m3);
183 
184  double p[3] = {
185  m_dot(m2t,m2) - 2*m_dot(m2t,m3) + m_dot(m3t,m3),
186  4*m_dot(m2t,m1) - 8*m_dot(m2t,g2) + 4*m_dot(g2t,m3),
187  4*m_dot(m1t,m1) - 8*m_dot(m1t,g2) + 4*m_dot(g2t,g2)};
188 
189  double l[3] = {m_det(mS), 2*gmg(mS,0,0)+2*gmg(mS,1,1), 4};
190 
191  /* q = p - l^2 */
192  double q[5] = {p[0]-(l[0]*l[0]), p[1]-(2*l[1]*l[0]),
193  p[2]-(l[1]*l[1]+2*l[0]*l[2]), -(2*l[2]*l[1]), -(l[2]*l[2])};
194 
195  if(TRACE_ALGO) {
196  fprintf(stderr, "p = %f %f %f \n", p[2], p[1], p[0]);
197  fprintf(stderr, "l = %f %f %f \n", l[2], l[1], l[0]);
198  fprintf(stderr, "q = %f %f %f %f %f \n", q[4], q[3], q[2], q[1], q[0]);
199  }
200 
201  /*
202  double lambdas[4];
203  if(!poly_real_roots(5, q, lambdas)) {
204  fprintf(stderr, "Cannot solve polynomial.\n");
205  return 0;
206  }
207 
208  double lambdas_error[4];
209  double lambdas_pose[4][3];
210 
211  for(int i=0;i<4;i++) {
212  double lambda = lambdas[i];
213 
214  if(TRACE_ALGO) {
215  fprintf(stderr, "lambda = %f \n", lambda);
216  }
217 
218  M(W,4,4); gsl_matrix_set_zero(W); gms(W,2,2,1.0); gms(W,3,3,1.0);
219  M(x,4,1);
220 
221  m_scale(2*lambda, W);
222  gsl_matrix_add(bigM,W);
223  m_inv(bigM, temp44);
224  m_mult(temp44, g, x);
225  m_scale(-1.0, x);
226 
227  lambdas_pose[i][0] = gmg(x,0,0);
228  lambdas_pose[i][1] = gmg(x,1,0);
229  lambdas_pose[i][2] = atan2(gmg(x,3,0),gmg(x,2,0));
230 
231  lambdas_error[i] = gpc_total_error(c, K, lambdas_pose[i]);
232  }
233 
234  int lowest_error = 0;
235  for(int i=0;i<4;i++) {
236  printf("#%d lambda=%lf error=%lf\n",i,lambdas[i],lambdas_error[i]);
237  if(lambdas_error[i] < lambdas_error[lowest_error])
238  lowest_error = i;
239  }
240 
241  double lr;
242  poly_greatest_real_root(5,q,&lr);
243  printf("Choose %d: lambda = %lf bigger real root = %lf \n",lowest_error,lambdas[lowest_error],lr);
244 
245  x_out[0]=lambdas_pose[lowest_error][0];
246  x_out[1]=lambdas_pose[lowest_error][1];
247  x_out[2]=lambdas_pose[lowest_error][2];
248  */
249 
250  double lambda;
251  if(!poly_greatest_real_root(5, q, &lambda)) return 0;
252 
253  M(W,4,4); gsl_matrix_set_zero(W); gms(W,2,2,1.0); gms(W,3,3,1.0);
254  M(x,4,1);
255 
256  m_scale(2*lambda, W);
257  gsl_matrix_add(bigM,W);
258  m_inv(bigM, temp44);
259  m_mult(temp44, g, x);
260  m_scale(-1.0, x);
261 
262  x_out[0] = gmg(x,0,0);
263  x_out[1] = gmg(x,1,0);
264  x_out[2] = atan2(gmg(x,3,0),gmg(x,2,0));
265 
266  if(TRACE_ALGO) {
267  fprintf(stderr, "x = %f %f %f deg\n", x_out[0], x_out[1],x_out[2]*180/M_PI);
268  }
269 
270  MF(mA); MF(mB); MF(mD); MF(mS); MF(mSa);
271  MF(m1t); MF(m1); MF(m2t); MF(m2);
272  MF(m3t); MF(m3); MF(W); MF(x);
273  MF(bigM); MF(g); MF(bigM_k);
274  MF(bigM_k_t); MF(C_k); MF(q_k);
275  MF(temp42); MF(temp44); MF(temp21);
276  MF(temp41); MF(temp22); MF(temp22b);
277  MF(temp22c); MF(temp12);
278  MF(g1); MF(g2);
279  MF(g1t); MF(g2t);
280  MF(mAi); MF(mBt);
281  return 1;
282 }
283 
284 
285 double gpc_error(const struct gpc_corr*co, const double*x) {
286  double c = cos(x[2]);
287  double s = sin(x[2]);
288  double e[2];
289  e[0] = c*(co->p[0]) -s*(co->p[1]) + x[0] - co->q[0];
290  e[1] = s*(co->p[0]) +c*(co->p[1]) + x[1] - co->q[1];
291  double this_error = e[0]*e[0]*co->C[0][0]+2*e[0]*e[1]*co->C[0][1]+e[1]*e[1]*co->C[1][1];
292 
293  if(0) /* due to limited numerical precision, error might be negative */
294  if(this_error < 0) {
295  fprintf(stderr, "Something fishy: error = %lf e = [%lf %lf] C = [%lf,%lf;%lf,%lf]\n", this_error,
296  e[0],e[1],co->C[0][0],co->C[0][1],co->C[1][0],co->C[1][1]);
297  }
298  return this_error;
299 }
300 
301 double gpc_total_error(const struct gpc_corr*co, int n, const double*x){
302  double error = 0;
303  for(int i=0;i<n;i++) {
304  if(!co[i].valid) continue;
305  error += gpc_error(co+i,x);
306  }
307  if(0) /* due to limited numerical precision, error might be negative */
308  if(error<0) {
309  fprintf(stderr, "Something fishy!\n");
310  }
311  return error;
312 }
313 
314 /*
315  [ C00 ] [ c01 ]
316  [ ] [ ]
317  [ C10 ] [ C11 ]
318 (%o10) Col 1 = [ ] Col 2 = [ ]
319  [ py C10 + px C00 ] [ py C11 + c01 px ]
320  [ ] [ ]
321  [ px C10 - py C00 ] [ px C11 - c01 py ]
322  [ px C00 + c01 py ]
323  [ ]
324  [ py C11 + px C10 ]
325  [ ]
326  Col 3 = [ 2 2 ]
327  [ py C11 + px py C10 + px C00 + c01 px py ]
328  [ ]
329  [ 2 2 ]
330  [ px py C11 + px C10 - px py C00 - c01 py ]
331  [ c01 px - py C00 ]
332  [ ]
333  [ px C11 - py C10 ]
334  [ ]
335  Col 4 = [ 2 2 ]
336  [ px py C11 - py C10 - px py C00 + c01 px ]
337  [ ]
338  [ 2 2 ]
339  [ px C11 - px py C10 + py C00 - c01 px py ]*/
340 
341 
m_trans
void m_trans(const gsl_matrix *A, gsl_matrix *A_t)
Definition: gpc_utils.c:4
m_scale
void m_scale(double m, gsl_matrix *A)
Definition: gpc_utils.c:16
m3
#define m3(v1, v2, v3)
Definition: egsl_macros.h:14
m_display
void m_display(const char *str, gsl_matrix *m)
Definition: gpc_utils.c:124
gpc_corr::C
double C[2][2]
Definition: gpc.h:27
poly_greatest_real_root
int poly_greatest_real_root(unsigned int n, const double *a, double *root)
Definition: gpc_utils.c:77
gmg
#define gmg
Definition: gpc_utils.h:12
gpc_corr::p
double p[2]
Definition: gpc.h:24
MF
#define MF(matrix)
Definition: gpc.c:26
gpc_corr
Definition: gpc.h:23
M_PI
#define M_PI
Definition: math_utils.h:7
gpc_utils.h
m_add
void m_add(const gsl_matrix *A, const gsl_matrix *B, gsl_matrix *ApB)
Definition: gpc_utils.c:20
p
struct @0 p
m_inv
void m_inv(const gsl_matrix *A, gsl_matrix *invA)
Definition: gpc_utils.c:25
M
#define M(matrix, rows, col)
Definition: gpc.c:25
gms
#define gms
Definition: gpc_utils.h:13
gpc_corr::q
double q[2]
Definition: gpc.h:25
TRACE_ALGO
#define TRACE_ALGO
Definition: gpc.h:42
m_det
double m_det(const gsl_matrix *A)
Definition: gpc_utils.c:39
m_mult
void m_mult(const gsl_matrix *A, const gsl_matrix *B, gsl_matrix *AB)
Definition: gpc_utils.c:8
gpc_solve
int gpc_solve(int K, const struct gpc_corr *c, const double *x0, const double *cov_x0, double *x_out)
Definition: gpc.c:29
gpc_total_error
double gpc_total_error(const struct gpc_corr *co, int n, const double *x)
Definition: gpc.c:301
gpc_error
double gpc_error(const struct gpc_corr *co, const double *x)
Definition: gpc.c:285
m_dot
double m_dot(const gsl_matrix *A, const gsl_matrix *B)
Definition: gpc_utils.c:53

csm
Author(s): Andrea Censi
autogenerated on Tue May 11 2021 02:18:23