Exemplo n.º 1
0
void mapper_71_write(uint8_t val, uint16_t addr) {
    if (!(~addr & 0xC000)) {
        // $C000-$FFFF
        prg_bank = val;
        apply_state();
    }
}
Exemplo n.º 2
0
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();
}
Exemplo n.º 3
0
    // 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;
    }
Exemplo n.º 4
0
    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;
    }
Exemplo n.º 5
0
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();
}
Exemplo n.º 6
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();
    }
  }
}
Exemplo n.º 7
0
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;
  }
}