00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
#include <volume_io/internal_volume_io.h>
00017
#include <bicpl/trans.h>
00018
#include <bicpl/numerical.h>
00019
00020
#ifndef lint
00021
static char rcsid[] =
"$Header: /software/source//libraries/bicpl/Transforms/optimize.c,v 1.15 2000/02/06 15:30:50 stever Exp $";
00022
#endif
00023
00024 #define FUNCTION_TOLERANCE 1e-6
00025 #define INITIAL_SIMPLEX_SIZE 3.0
00026 #define MAX_ITERS 300
00027
00028 typedef struct
00029
{
00030 int ndim;
00031 int npoints;
00032 Real *center;
00033 Real **pts1;
00034 Real **pts2;
00035 }
function_data;
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054 private Real
lsq_objective(
00055 Transform *lt,
00056 Real **pts1,
00057 Real **pts2,
00058
int npoints )
00059 {
00060
int i, j;
00061 Real sum, error2, delta;
00062 Real newpt[N_DIMENSIONS];
00063
00064 sum = 0.0;
00065
00066 for_less( i, 0, npoints )
00067 {
00068 transform_point( lt, pts2[i][X], pts2[i][Y], pts2[i][Z],
00069 &newpt[X], &newpt[Y], &newpt[Z] );
00070
00071 error2 = 0.0;
00072
00073 for_less( j, 0, N_DIMENSIONS )
00074 {
00075 delta = newpt[j] - pts1[i][j];
00076 error2 += delta * delta;
00077 }
00078
00079 sum += error2;
00080 }
00081
00082
return( sum);
00083 }
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102 private Real
fit_function(
00103
void *func_data,
00104
float params[] )
00105 {
00106 Transform transform;
00107
int i;
00108 Real r;
00109 Real trans[3];
00110 Real cent[3];
00111 Real rots[3];
00112 Real scale[3];
00113 Real shear[3];
00114
function_data *data;
00115
00116 data = (
function_data *) func_data;
00117
00118 for_less( i, 0, 3 ) {
00119 trans[i] = (Real) params[i+0];
00120 rots[i] = (Real) params[i+3];
00121 scale[i] = (Real) params[i+6];
00122 }
00123
00124 for_less( i, 0, 3 )
00125 shear[i] = 0.0;
00126
00127
if( data->
ndim == 10 )
00128 shear[0] = (Real) params[9];
00129
00130 for_less( i, 0, 3 )
00131 cent[i] = data->
center[i];
00132
00133
build_transformation_matrix( &transform, cent, trans, scale, shear, rots );
00134
00135
00136
00137 r =
lsq_objective( &transform, data->
pts1, data->
pts2, data->
npoints );
00138
00139
return(r);
00140 }
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164 public BOOLEAN
optimize_simplex(
00165 Real **pts1,
00166 Real **pts2,
00167
int npoints,
00168 Trans_type trans_type,
00169 Real center[],
00170 Real translations[],
00171 Real scales[],
00172 Real shears[],
00173 Real rotations[] )
00174 {
00175 Real initial_guess[10], solution[10], initial_step[10];
00176
int i, ndim, iters;
00177
function_data func_data;
00178
amoeba_struct amoeba;
00179
00180
switch (trans_type)
00181 {
00182
case TRANS_LSQ9:
00183 ndim = 9;
00184
break;
00185
case TRANS_LSQ10:
00186 ndim = 10;
00187
break;
00188
default:
00189 print_error(
"Unknown type of transformation requested (%d)\n",
00190 trans_type);
00191 print_error(
"Error in line %d, file %s\n",__LINE__, __FILE__);
00192
return(
FALSE );
00193 }
00194
00195
00196
00197 func_data.
pts1 = pts1;
00198 func_data.
pts2 = pts2;
00199 func_data.
npoints = npoints;
00200 func_data.
center = center;
00201 func_data.
ndim = ndim;
00202
00203
00204
00205
00206 initial_guess[0] = translations[0];
00207 initial_guess[1] = translations[1];
00208 initial_guess[2] = translations[2];
00209
00210 initial_guess[3] = rotations[0];
00211 initial_guess[4] = rotations[1];
00212 initial_guess[5] = rotations[2];
00213
00214 initial_guess[6] = scales[0];
00215 initial_guess[7] = scales[1];
00216 initial_guess[8] = scales[2];
00217
00218
if( ndim == 10 )
00219 initial_guess[9] = shears[0];
00220
00221
00222
00223 initial_step[0] =
INITIAL_SIMPLEX_SIZE;
00224 initial_step[1] =
INITIAL_SIMPLEX_SIZE;
00225 initial_step[2] =
INITIAL_SIMPLEX_SIZE;
00226
00227 initial_step[3] =
INITIAL_SIMPLEX_SIZE * DEG_TO_RAD;
00228 initial_step[4] =
INITIAL_SIMPLEX_SIZE * DEG_TO_RAD;
00229 initial_step[5] =
INITIAL_SIMPLEX_SIZE * DEG_TO_RAD;
00230
00231 initial_step[6] =
INITIAL_SIMPLEX_SIZE / 30.0;
00232 initial_step[7] =
INITIAL_SIMPLEX_SIZE / 30.0;
00233 initial_step[8] =
INITIAL_SIMPLEX_SIZE / 30.0;
00234
00235
if( ndim == 10 )
00236 initial_step[9] =
INITIAL_SIMPLEX_SIZE * DEG_TO_RAD;
00237
00238
00239
00240
initialize_amoeba( &amoeba, ndim, initial_guess, initial_step,
00241
fit_function, (
void *) &func_data,
FUNCTION_TOLERANCE );
00242
00243 for_less( iters, 0,
MAX_ITERS )
00244 {
00245
if( !
perform_amoeba( &amoeba ) )
00246
break;
00247 }
00248
00249 (
void)
get_amoeba_parameters( &amoeba, solution );
00250
00251
terminate_amoeba( &amoeba );
00252
00253
00254
00255 for_less( i, 0, 3 )
00256 {
00257 translations[i] = solution[i+0];
00258 rotations[i] = solution[i+3];
00259 scales[i] = solution[i+6];
00260 }
00261
00262
if( ndim == 10 )
00263 shears[0] = solution[9];
00264
00265
return(
TRUE );
00266 }