void mapper_71_write(uint8_t val, uint16_t addr) {
if (!(~addr & 0xC000)) {
// $C000-$FFFF
prg_bank = val;
apply_state();
}
}
void mapper_71_init() {
// Last PRG bank and all CHR banks fixed
set_prg_16k_bank(1, -1);
set_chr_8k_bank(0);
prg_bank = 0;
apply_state();
}
// 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;
}
void Person::draw_aux(int t) {
glPushMatrix();
glColor3f(0.435294f, 0.258824f, 0.258824f);
auto d = mode.bodyMovement(t);
glTranslatef(position.x, position.y + d.ty, position.z);
glRotatef(rotation, 0, 1, 0);
trunk.draw(t);
glPushMatrix();
glTranslated(0, trunk.height / 2, trunk.depth / 2);
head.draw(t);
glPopMatrix();
// Left side (upper part)
glPushMatrix();
glTranslatef(trunk.width / 2 + larm.joint_radius / 2, trunk.height / 2 - larm.joint_radius / 2, trunk.depth /2);
glRotatef(90, 1,0,0);
apply_state(mode.leftArmMovement(t));
larm.draw(t);
glTranslatef(0, 0, larm.length + lforearm.joint_radius / 2);
apply_state(mode.leftForearmMovement(t));
lforearm.draw(t);
glTranslatef(0, 0, lforearm.length + lhand.joint_radius / 2);
apply_state(mode.leftHandMovement(t));
lhand.draw(t);
glPopMatrix();
// Right side (upper part)
glPushMatrix();
glTranslatef(-trunk.width / 2 - rarm.joint_radius / 2, trunk.height / 2 - rarm.joint_radius / 2, trunk.depth /2);
//arms should start in the vertical position, poiting to the floor
glRotatef(90, 1,0,0);
apply_state(mode.rightArmMovement(t));
larm.draw(t);
glTranslatef(0, 0, rarm.length + rforearm.joint_radius / 2);
apply_state(mode.rightForearmMovement(t));
lforearm.draw(t);
glTranslatef(0, 0, rforearm.length + rhand.joint_radius / 2);
apply_state(mode.rightHandMovement(t));
lhand.draw(t);
glPopMatrix();
// Left lower part
glPushMatrix();
glTranslatef(trunk.width * 0.3 + lthigh.joint_radius / 2, -trunk.height / 2 - lthigh.joint_radius / 2, lthigh.width / 2);
apply_state(mode.leftThighMovement(t));
lthigh.draw(t);
glTranslatef(0, -lthigh.length / 2 - lleg.radius / 2, lleg.radius);
apply_state(mode.leftLegMovement(t));
lleg.draw(t);
glTranslatef(0, -lleg.length - lfoot.joint_radius / 2, 0);
apply_state(mode.leftFootMovement(t));
lfoot.draw(t);
glPopMatrix();
// Right lower part
glPushMatrix();
glTranslatef(-trunk.width * 0.3 + rthigh.joint_radius / 2, -trunk.height / 2 - rthigh.joint_radius / 2, rthigh.width / 2);
apply_state(mode.rightThighMovement(t));
lthigh.draw(t);
glTranslatef(0, -rthigh.length / 2 - rleg.radius / 2, rleg.radius);
apply_state(mode.rightLegMovement(t));
lleg.draw(t);
glTranslatef(0, -rleg.length - rfoot.joint_radius / 2, 0);
apply_state(mode.rightFootMovement(t));
lfoot.draw(t);
glPopMatrix();
glPopMatrix();
}
// 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;
}
}