void dgc_transform_inverse(dgc_transform_t in, dgc_transform_t out)
{
double temp, t1, t2, t3;
dgc_transform_copy(out, in);
temp = out[0][1];
out[0][1] = out[1][0];
out[1][0] = temp;
temp = out[0][2];
out[0][2] = out[2][0];
out[2][0] = temp;
temp = out[1][2];
out[1][2] = out[2][1];
out[2][1] = temp;
t1 =
-out[0][0] * out[0][3]
-out[0][1] * out[1][3]
-out[0][2] * out[2][3];
t2 =
-out[1][0] * out[0][3]
-out[1][1] * out[1][3]
-out[1][2] * out[2][3];
t3 =
-out[2][0] * out[0][3]
-out[2][1] * out[1][3]
-out[2][2] * out[2][3];
out[0][3] = t1;
out[1][3] = t2;
out[2][3] = t3;
}
// GICP cost function
double GICPOptimizer::f(const gsl_vector *x, void *params) {
GICPOptData *opt_data = (GICPOptData *)params;
double pt1[3];
double pt2[3];
double res[3]; // residual
double temp[3];
gsl_vector_view gsl_pt1 = gsl_vector_view_array(pt1, 3);
gsl_vector_view gsl_pt2 = gsl_vector_view_array(pt2, 3);
gsl_vector_view gsl_res = gsl_vector_view_array(res, 3);
gsl_vector_view gsl_temp = gsl_vector_view_array(temp, 3);
gsl_matrix_view gsl_M;
dgc_transform_t t;
// initialize the temp variable; if it happens to be NaN at start, bad things will happen in blas routines below
temp[0] = 0;
temp[1] = 0;
temp[2] = 0;
// take the base transformation
dgc_transform_copy(t, opt_data->base_t);
// apply the current state
apply_state(t, x);
double f = 0;
double temp_double = 0;
int N = opt_data->p1->Size();
int counter = 0;
for(int i = 0; i < N; i++) {
int j = opt_data->nn_indecies[i];
if(j != -1) {
// get point 1
pt1[0] = (*opt_data->p1)[i].x;
pt1[1] = (*opt_data->p1)[i].y;
pt1[2] = (*opt_data->p1)[i].z;
// get point 2
pt2[0] = (*opt_data->p2)[j].x;
pt2[1] = (*opt_data->p2)[j].y;
pt2[2] = (*opt_data->p2)[j].z;
//get M-matrix
gsl_M = gsl_matrix_view_array(&opt_data->M[i][0][0], 3, 3);
//transform point 1
dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], t);
res[0] = pt1[0] - pt2[0];
res[1] = pt1[1] - pt2[1];
res[2] = pt1[2] - pt2[2];
//cout << "res: (" << res[0] << ", " <<res[1] <<", " << res[2] << ")" << endl;
// temp := M*res
gsl_blas_dsymv(CblasLower, 1., &gsl_M.matrix, &gsl_res.vector, 0., &gsl_temp.vector);
// temp_double := res'*temp = temp'*M*temp
gsl_blas_ddot(&gsl_res.vector, &gsl_temp.vector, &temp_double);
// increment total error
f += temp_double/(double)opt_data->num_matches;
//cout << "temp: " << temp_double << endl;
//cout << "f: " << f << "\t (" << opt_data->num_matches << ")" << endl;
//print_gsl_matrix(&gsl_M.matrix, "M");
counter++;
}
}
printf("counter %d\n",counter);
return f;
}
void GICPOptimizer::fdf(const gsl_vector *x, void *params, double * f, gsl_vector *g) {
std::cout << ">>> fdf" << std::endl;
GICPOptData *opt_data = (GICPOptData *)params;
double pt1[3];
double pt2[3];
double res[3]; // residual
double temp[3]; // temp local vector
double temp_mat[9]; // temp matrix used for accumulating the rotation gradient
gsl_vector_view gsl_pt1 = gsl_vector_view_array(pt1, 3);
gsl_vector_view gsl_pt2 = gsl_vector_view_array(pt2, 3);
gsl_vector_view gsl_res = gsl_vector_view_array(res, 3);
gsl_vector_view gsl_temp = gsl_vector_view_array(temp, 3);
gsl_vector_view gsl_gradient_t = gsl_vector_subvector(g, 0, 3); // translation comp. of gradient
gsl_vector_view gsl_gradient_r = gsl_vector_subvector(g, 3, 3); // rotation comp. of gradient
gsl_matrix_view gsl_temp_mat_r = gsl_matrix_view_array(temp_mat, 3, 3);
gsl_matrix_view gsl_M;
dgc_transform_t t;
double temp_double;
// take the base transformation
dgc_transform_copy(t, opt_data->base_t);
// apply the current state
apply_state(t, x);
// zero all accumulator variables
*f = 0;
gsl_vector_set_zero(g);
gsl_vector_set_zero(&gsl_temp.vector);
gsl_matrix_set_zero(&gsl_temp_mat_r.matrix);
for(int i = 0; i < opt_data->p1->Size(); i++) {
int j = opt_data->nn_indecies[i];
if(j != -1) {
// get point 1
pt1[0] = (*opt_data->p1)[i].x;
pt1[1] = (*opt_data->p1)[i].y;
pt1[2] = (*opt_data->p1)[i].z;
// get point 2
pt2[0] = (*opt_data->p2)[j].x;
pt2[1] = (*opt_data->p2)[j].y;
pt2[2] = (*opt_data->p2)[j].z;
//cout << "accessing " << i << " of " << opt_data->p1->Size() << ", " << opt_data->p2->Size() << endl;
//get M-matrix
gsl_M = gsl_matrix_view_array(&opt_data->M[i][0][0], 3, 3);
print_gsl_matrix(&gsl_M.matrix, "M");
//transform point 1
dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], t);
std::cout << "pt1 " << pt1[0] << "," << pt1[1] << "," << pt1[2] << std::endl;
res[0] = pt1[0] - pt2[0];
res[1] = pt1[1] - pt2[1];
res[2] = pt1[2] - pt2[2];
std::cout << "res " << res[0] << "," << res[1] << "," << res[2] << std::endl;
// compute the transformed residual
// temp := M*res
//print_gsl_matrix(&gsl_M.matrix, "gsl_m");
gsl_blas_dsymv(CblasLower, 1., &gsl_M.matrix, &gsl_res.vector, 0., &gsl_temp.vector);
print_gsl_vector(&gsl_temp.vector, "temp");
// compute M-norm of the residual
// temp_double := res'*temp = temp'*M*res
gsl_blas_ddot(&gsl_res.vector, &gsl_temp.vector, &temp_double);
// accumulate total error: f += res'*M*res
*f += temp_double/(double)opt_data->num_matches;
std::cout << "f " << *f << std::endl;
// accumulate translation gradient:
// gsl_gradient_t += 2*M*res
gsl_blas_dsymv(CblasLower, 2./(double)opt_data->num_matches, &gsl_M.matrix, &gsl_res.vector, 1., &gsl_gradient_t.vector);
if(opt_data->solve_rotation) {
// accumulate the rotation gradient matrix
// get back the original untransformed point to compute the rotation gradient
pt1[0] = (*opt_data->p1)[i].x;
pt1[1] = (*opt_data->p1)[i].y;
pt1[2] = (*opt_data->p1)[i].z;
dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], opt_data->base_t);
// gsl_temp_mat_r += 2*(gsl_temp).(gsl_pt1)' [ = (2*M*residual).(gsl_pt1)' ]
gsl_blas_dger(2./(double)opt_data->num_matches, &gsl_pt1.vector, &gsl_temp.vector, &gsl_temp_mat_r.matrix);
}
}
}
print_gsl_vector(g, "gradient");
// the above loop sets up the gradient with respect to the translation, and the matrix derivative w.r.t. the rotation matrix
// this code sets up the matrix derivatives dR/dPhi, dR/dPsi, dR/dTheta. i.e. the derivatives of the whole rotation matrix with respect to the euler angles
// note that this code assumes the XYZ order of euler angles, with the Z rotation corresponding to bearing. This means the Z angle is negative of what it would be
// in the regular XYZ euler-angle convention.
if(opt_data->solve_rotation) {
// now use the d/dR matrix to compute the derivative with respect to euler angles and put it directly into g[3], g[4], g[5];
compute_dr(x, &gsl_temp_mat_r.matrix, g);
}
print_gsl_matrix(&gsl_temp_mat_r.matrix, "R");
print_gsl_vector(g, "gradient");
std::cout << "<<< fdf" << std::endl;
}
// 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();
}
}
}
void Config::integrateOffset(const dgc_transform_t offset) {
dgc_transform_copy(offset_, offset);
}
