int dgc_transform_write(dgc_transform_t t, const char *filename)
{
FILE *fp;
double x, y, z;
double rx, ry, rz;
fp = fopen(filename, "w");
if(fp == NULL) {
dgc_warning("Error: could not open transform file %s for writing.\n", filename);
return -1;
}
dgc_transform_get_rotation(t, &rx, &ry, &rz);
dgc_transform_get_translation(t, &x, &y, &z);
fprintf(fp,"RX RAD %lf\n", rx);
fprintf(fp,"RY RAD %lf\n", ry);
fprintf(fp,"RZ RAD %lf\n", rz);
fprintf(fp,"T M %lf %lf %lf\n", x, y, z);
fclose(fp);
return 0;
}
// this method outputs data files containing the value of the error function on a 2d grid.
// a seperate file and grid is constructed for each pair of coordinates
// this is used to verify convergence of the numerical minimization
void GICPOptimizer::PlotError(dgc_transform_t t, GICPOptData &opt_data, const char* filename)
{
double resolution = .005;
double min = -.1;
double max = .1;
// initialize the starting point
double tx, ty, tz, rx, ry, rz;
dgc_transform_get_translation(t, &tx, &ty, &tz);
dgc_transform_get_rotation(t, &rx, &ry, &rz);
gsl_vector_set(x, 0, tx);
gsl_vector_set(x, 1, ty);
gsl_vector_set(x, 2, tz);
gsl_vector_set(x, 3, rx);
gsl_vector_set(x, 4, ry);
gsl_vector_set(x, 5, rz);
dgc_transform_t temp;
int N = (max-min)/resolution;
for(int n1 = 0; n1 < 6; n1++)
{
for(int n2 = 0; n2 < n1; n2++)
{
// set up the filename for this pair of coordinates
ostringstream full_filename;
full_filename << filename << "_" << n1 << "vs" << n2 << ".dat";
// open the file
ofstream fout(full_filename.str().c_str());
if(!fout)
{
cout << "Could not open file for writing." << endl;
return;
}
// loop through pairs of values for these two coordinates while keeping others fixed
double val1_0 = gsl_vector_get(x, n1);
for(int k1 = 0; k1 < N; k1++)
{
gsl_vector_set(x, n1, val1_0 + min + resolution*k1);
double val2_0 = gsl_vector_get(x, n2);
for(int k2 = 0; k2 < N; k2++)
{
gsl_vector_set(x, n2, val2_0 + min + resolution*k2);
dgc_transform_copy(temp, opt_data.base_t);
apply_state(temp, x);
double error = f(x, &opt_data);
fout << error << "\t";
}
gsl_vector_set(x, n2, val2_0); // restore to old value
fout << endl;
}
gsl_vector_set(x, n1, val1_0); // restore to old value
// close the file for this pair of coordinates
fout.close();
}
}
}
bool GICPOptimizer::Optimize(dgc_transform_t t, GICPOptData & opt_data)
{
double line_search_tol = .01;
double gradient_tol = 1e-2;
double step_size = 1.;
// set up the gsl function_fdf struct
gsl_multimin_function_fdf func;
func.f = f;
func.df = df;
func.fdf = fdf;
func.n = N;
func.params = &opt_data;
// initialize the starting point
double tx, ty, tz, rx, ry, rz;
dgc_transform_get_translation(t, &tx, &ty, &tz);
dgc_transform_get_rotation(t, &rx, &ry, &rz);
gsl_vector_set(x, 0, tx);
gsl_vector_set(x, 1, ty);
gsl_vector_set(x, 2, tz);
gsl_vector_set(x, 3, rx);
gsl_vector_set(x, 4, ry);
gsl_vector_set(x, 5, rz);
// initialize the minimizer
gsl_multimin_fdfminimizer_set(gsl_minimizer, &func, x, step_size, line_search_tol);
//iterate the minimization algorithm using gsl primatives
status = GSL_CONTINUE;
iter = 0;
if(debug)
{
cout << "iter\t\tf-value\t\tstatus" << endl;
cout << iter << "\t\t" << gsl_minimizer->f << "\t\t" << Status() << endl;
}
while(status == GSL_CONTINUE && iter < max_iter)
{
iter++;
status = gsl_multimin_fdfminimizer_iterate(gsl_minimizer);
if(debug)
cout << iter << "\t\t" << gsl_minimizer->f << "\t\t" << Status() <<endl;
if(status) break;
status = gsl_multimin_test_gradient (gsl_minimizer->gradient, gradient_tol);
}
if(status == GSL_SUCCESS || iter == max_iter)
{
//set t to the converged solution
dgc_transform_identity(t);
// apply the current state to the base
apply_state(t, gsl_minimizer->x);
//dgc_transform_print(t, "converged to:");
return true;
}
else
{
// the algorithm failed to converge
return false;
}
}
