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; } }