Isis 3 Programmer Reference
Ransac.h
1 #ifndef Ransac_h
2 #define Ransac_h
3 
8 /* SPDX-License-Identifier: CC0-1.0 */
9 #include <math.h>
10 
11 namespace Isis {
12  //functions for solving the normal equations
13  int choleski_solve(double *a, double *b, int nsize, int flag); //solves the normal
14  int inverse (double *a, int nsize); //inverses ata (only to be used after ata has been choleski decomposed by choleski_sovle)
15  int isymp(int row, int col); //used for memory optimized symetric matricies, returns the single dimensional array location equivalent to the 2D location in the symetric matrix
16 
17  int indeces_from_set(int *indeces,int set, int set_size,int n);
18  int binomial_coeficient(int n, int k);
19 
20  inline int isymp(int row, int col)
21  {
22  /* returns the index position of ellement [i][j] in the memory optimized symetric matrix a
23  row -> row in square symetric matrix
24  col -> column in square symetric matrix
25  */
26  int k;
27  ++row;
28  ++col;
29  if(row < col) k= row + col *(col -1)/2;
30  else k= col + row * (row - 1)/2;
31  --k;
32  return k;
33 
34  }
35 
36  inline int binomial_coeficient(int n, int k)
37  {
38  if( k > n)
39  return 0;
40 
41  if( n < 1)
42  return 0;
43 
44  int i,j,bc;
45  bc=1;
46  j=k;
47 
48  for(i=k-1;i>0;i--)
49  {
50  j *= i;
51  }
52 
53 
54  for(i=n-k+1;i<=n;i++)
55  bc *=i;
56 
57  bc/=j;
58 
59  return bc;
60  }
61 
62 
63  /* given a desired set size, and the number of items in the population this function returns the indeces
64  of set number set. For example:
65 
66  set_size = 3 n=5
67 
68  Set Index0 Index1 Index2
69  1 0 1 2
70  2 0 1 3
71  3 0 1 4
72  4 0 1 5
73  5 0 2 3
74  6 0 2 4
75  7 0 2 5
76  8 0 3 4
77  9 0 3 5
78  so indeces_from_set( , 5, 3, 6) would return 0, 2, 3
79 
80  set -> the number of the set starting from 1
81  set_size-> how many are in each set
82  n -> the population size
83  indeces <- the set_size indeces for set number set
84 
85  return 1 if successfuly, otherwise -1
86 
87  */
88  inline int indeces_from_set(int *indeces,int set, int set_size,int n)
89  {
90  //frist set_size must not be greater than n
91  if( set_size > n)
92  return -1;
93 
94  //set must be >= 0
95  if( set < 0)
96  return -1;
97 
98  //set must also be less than or equal to the number of possible sets
99  if( set > binomial_coeficient(n, set_size) )
100  return -1;
101 
102  int i,j,k;
103 
104  for( i =0,j=0;i<set_size-1;i++,j=indeces[i-1]+1)
105  {
106  k = binomial_coeficient( n - j - 1, set_size - i - 1);
107  while( set > k)
108  {
109  j++;
110  set -= k;
111  k = binomial_coeficient( n - j - 1, set_size - i - 1);
112  }
113  indeces[i] = j;
114  }
115 
116  indeces[i] = j+set-1;
117 
118  return 1;
119  }
120 
121 
122  int decompose (double *, int);
123  int foresub (double *,double *, int);
124  int backsub (double *,double *, int);
125 
126 #define SOLVED 1
127 #define NOT_SOLVABLE 0
128 #define NO_ERRORS -1
129 
130 
131  inline int choleski_solve (double *a, double *b, int nsize, int flag)
132  {
133  /* solves the set of linear equations square_matrix(a)delta = b
134  a <-> positive definite symetric matrix a becomes L of the LLt choleski decomposition or the inverse of a (a symetric matrix still in memory optimized mode)
135  b <-> array of doubles of size nsize, the constant part of the system of linear equations
136  nsize -> number of unkowns, also lenght of b, also square(a) is nsize x nsize
137  flag -> type of solutions sought: 1 decomposition only, 2 decomposition and solve, 3 decompose solve and inverse a
138  */
139 
140  if (flag < 1 && flag > 3)
141  return NOT_SOLVABLE;
142 
143  /*************** STEP 1: DECOMPOSE THE A MATRIX ***************/
144  if (! decompose (a, nsize)) return NOT_SOLVABLE;
145 
146  if (flag == 1) return SOLVED;
147 
148  /*************** STEP 2: FORWARD SUBSTITUTION *****************/
149  if (! foresub (a, b, nsize)) return NOT_SOLVABLE;
150 
151 
152  /*************** STEP 2: BACKWARD SUBSTITUTION ****************/
153  if (! backsub (a, b, nsize)) return NOT_SOLVABLE;
154 
155  if (flag == 2) return SOLVED;
156 
157  /*************** STEP 4: INVERSE THE A MATRIX *****************/
158  inverse (a, nsize);
159  return SOLVED;
160  }
161 
162  /*******************************************************************
163  FUNCTION DECOMPOSE
164  *******************************************************************/
165  inline int decompose (double *a, int nsize)
166  {
167  /* decomposes the memory optimized symetric matirx a into LLt (Choleski decomposition)
168  a <-> positive definite symetric matrix a becomes L of the LLt choleski decomposition
169  n -> size of the square matrix a represents or number of unknowns
170  */
171  double sum;
172  double *ap1 = a - 1;
173  double *ap2;
174  double *ap3;
175  double *ap4;
176  double *ap5;
177  double *ap6 = a;
178  double *ap7;
179  int i, j, k, m, n = 2;
180 
181  ap7 = a;
182  for (k = 0; k < nsize; k++)
183  {
184  ap2 = a;
185  ap3 = ap7 + 1;
186  ap4 = a -1;
187  m = 2;
188  for (i = 0; i < k; i++)
189  {
190  sum = 0.0;
191  ap5 = ap1 + 1;
192  ap4++;
193  for (j = 0; j < i; j++)
194  {
195  sum += *ap4++ * *ap5++;
196  }
197  if (*ap2 == 0.0)
198  return NOT_SOLVABLE;
199  *ap3 = (*ap3 - sum) / *ap2;ap3++;
200  ap2 += m++;
201  }
202  sum = 0.0;
203  ap1++;
204  for (j = 0; j < k; j++)
205  {
206  sum += *ap1 * *ap1;ap1++;
207  }
208  /*if ((*ap6 - sum) <= 0.0)
209  return (errcode (06));
210  *ap6 = sqrt(*ap6 - sum);*/
211 
212 
213  //if ((*ap6 - sum) <= 1e-20)
214  if ((*ap6 - sum) <= 0)
215  *ap6 = sqrt(-(*ap6 - sum));
216  else
217  *ap6 = sqrt(*ap6 - sum);
218 
219 
220  ap7 = ap6;
221  ap6 += n++;
222  }
223  return SOLVED;
224  }
225 
226  /*******************************************************************
227  FUNCTION FORESUB
228  *******************************************************************/
229  inline int foresub (double *a,double *b,int nsize)
230  {
231  int i, j;
232  double sum;
233  double *ap1 = a;
234  double *bp1 = b;
235  double *bp2;
236 
237  if (*ap1 == 0.0)
238  return NOT_SOLVABLE;
239  *bp1++ /= *ap1++;
240  for (i = 1; i < nsize; i++)
241  {
242  sum = 0.0;
243  for (j = 0, bp2 = b; j < i; j++)
244  {
245  sum += *ap1++ * *bp2++;
246  }
247  if (*ap1 == 0.0)
248  return NOT_SOLVABLE;
249  *bp1 = (*bp1 - sum) / *ap1;bp1++;ap1++;
250  }
251  return SOLVED;
252  }
253 
254  /*******************************************************************
255  FUNCTION BACKSUB
256  *******************************************************************/
257  inline int backsub (double *a,double *b,int nsize)
258  //double *a,*b;
259  //int nsize;
260  {
261  int i,j;
262  int k = nsize - 1;
263  int m = nsize;
264  int n;
265  double sum;
266  double *ap1 = a + (k + k * (k + 1) / 2);
267  double *ap2 = ap1 - 1;
268  double *ap3;
269  double *bp1 = b + k;
270  double *bp2 = bp1;
271  double *bp3;
272 
273  if (*ap1 == 0.0)
274  return NOT_SOLVABLE;
275  *bp1 = *bp1 / *ap1;bp1--;
276  for (i = k - 1; i >= 0; i--)
277  {
278  sum = 0.0;
279  n = nsize - 1;
280  for (j = nsize - 1, ap3 = ap2--, bp3 = bp2; j >= i + 1; j--)
281  {
282  sum += *ap3 * *bp3--;
283  ap3 -= n--;
284  }
285  ap1 -= m--;
286  if (*ap1 == 0.0)
287  return NOT_SOLVABLE;
288  *bp1 = (*bp1 - sum) / *ap1;bp1--;
289  }
290  return SOLVED;
291  }
292 
293  /*******************************************************************
294  FUNCTION INVERSE
295  *******************************************************************/
296  inline int inverse (double *a, int nsize)
297  //double *a;
298  //int nsize;
299  {
300  int i, j, k, m, n;
301  double sum;
302  double *ap1 = a;
303  double *ap2;
304  double *ap3;
305  double *ap4;
306  double *ap5;
307 
308  /* FIRST INVERSE ALL DIAGONAL ELEMENTS */
309  m = 2;
310  while (ap1 < (a + (nsize * (nsize + 1)/2)))
311  {
312  *ap1 = 1. / *ap1;
313  ap1 += m++;
314  }
315 
316  /* DO FIRST SET OF MANIPULATIONS */
317  ap5 = a + 1;
318  for (i = 1; i < nsize; i++)
319  {
320  ap4 = ap5 - 1;
321  m = i + 1;
322  n = i;
323  for (j = m; j <= nsize; j++)
324  {
325  ap1 = ap5 - 1;
326  ap2 = ap3 = ap4 + i;
327  sum = 0.0;
328  for (k = i; k <= n;)
329  {
330  sum += *ap1 * *ap2++;
331  ap1 += k++;
332  }
333  ap4 += j;
334  *ap3 = -(*ap4) * sum;
335  n = j;
336  }
337  ap5 += m;
338  }
339 
340  /* DO SECOND SET OF MANIPULATIONS */
341  n = 0;
342  ap1 = ap2 = ap3 = ap4 = a;
343  for (i = 0; i < nsize;)
344  {
345  ap5 = ap4;
346  m = 0;
347  for (j = i; j < nsize;)
348  {
349  ap5 += j;
350  ap1 = ap3 = ap5 + i + n;
351  ap2 = ap5 + m++;
352  sum = 0.0;
353  for (k = j; k < nsize;)
354  {
355  sum += *ap1 * *ap2;
356  ap1 += ++k;
357  ap2 += k;
358  }
359  *ap3 = sum;
360  j++;
361  }
362  ap4 += ++i;
363  n--;
364  }
365  return SOLVED;
366  }
367 }
368 
369 #endif
Isis
This is free and unencumbered software released into the public domain.
Definition: Apollo.h:16