Ejemplo n.º 1
0
  double eval_joint_factor_debug(const Joint &joint, const PartBBox& bbox_parent, const PartBBox &bbox_child, 
				 const double parent_scale, const double child_scale, 
				 double_vector &joint_pos_offset, double &joint_rot_offset, const double rot_step_size)
  {

    double rot_sigma = joint.rot_sigma;
    assert(rot_sigma >= 0);

    double child_rot = atan2(bbox_child.part_x_axis(1), bbox_child.part_x_axis(0));
    double parent_rot = atan2(bbox_parent.part_x_axis(1), bbox_parent.part_x_axis(0));
    cout << "child_rot = " << child_rot << endl;
    cout << "parent_rot = " << parent_rot << endl;
    cout << "rot_step_size = " << rot_step_size << endl;
    
    double_matrix Tgc = prod(hc::get_rotation_matrix(child_rot), hc::get_scaling_matrix(child_scale));
    double_vector offset_c_10 = hc::get_vector(joint.offset_c(0), joint.offset_c(1));
    double_vector offset_g_10 = prod(Tgc, offset_c_10);
    double_matrix Tjoint_c = hc::get_translation_matrix(offset_g_10(0), offset_g_10(1));
    double_matrix Tjoint2_c = prod(Tjoint_c,boost_math::identity_double_matrix(3));
    double x1_c, y1_c;
    hc::map_point(Tjoint2_c, bbox_child.part_pos(0), bbox_child.part_pos(1), x1_c, y1_c);
    
    double_matrix Tgp = prod(hc::get_rotation_matrix(parent_rot), hc::get_scaling_matrix(parent_scale));
    double_vector offset_p_01 = hc::get_vector(joint.offset_p(0), joint.offset_p(1));
    double_vector offset_g_01 = prod(Tgp, offset_p_01);
    double_matrix Tjoint_p = hc::get_translation_matrix(offset_g_01(0), offset_g_01(1));
    double_matrix Tjoint2_p = prod(Tjoint_p,boost_math::identity_double_matrix(3));
    double x1_p, y1_p;
    hc::map_point(Tjoint2_p, bbox_parent.part_pos(0), bbox_parent.part_pos(1), x1_p, y1_p);
    
//     assert(child_scale == 1.0);
//     assert(parent_scale == 1.0);

    double_vector joint_pos_child = bbox_child.part_pos + 
      child_scale * bbox_child.part_x_axis * joint.offset_c(0) + 
      child_scale * bbox_child.part_y_axis * joint.offset_c(1);

    double_vector joint_pos_parent = bbox_parent.part_pos + 
      parent_scale * bbox_parent.part_x_axis * joint.offset_p(0) + 
      parent_scale * bbox_parent.part_y_axis * joint.offset_p(1);
    
    cout << "round part positions" << endl;
    joint_pos_child(0) = round(joint_pos_child(0));
    joint_pos_child(1) = round(joint_pos_child(1));
    joint_pos_parent(0) = round(joint_pos_parent(0));
    joint_pos_parent(1) = round(joint_pos_parent(1));    
    
    joint_pos_offset = joint_pos_child - joint_pos_parent;
    
    cout << round(y1_c) << " " << round(x1_c) << endl;
    cout << joint_pos_child(1) << " " << joint_pos_child(0) << endl;
    cout << endl;
    cout << round(y1_p) << " " << round(x1_p) << endl;
    cout << joint_pos_parent(1) << " " << joint_pos_parent(0) << endl;
    
    double rot_mean = joint.rot_mean;
    if (rot_step_size > 0 && rot_step_size <= M_PI){
      cout << "round joint mean" << endl;
      rot_mean = boost_math::round(joint.rot_mean / rot_step_size) * rot_step_size;
    }
    joint_rot_offset = (child_rot - parent_rot) - rot_mean;
    /*
    while (joint_rot_offset < -M_PI)
      joint_rot_offset += M_PI;

    while (joint_rot_offset > M_PI)
      joint_rot_offset -= M_PI;
    
    assert(joint_rot_offset >= -M_PI && joint_rot_offset <= M_PI);
    */
    double detC1 = joint.detC;
    assert(detC1 > 0);

    double avg_scale = 0.5*(parent_scale + child_scale);
    //double avg_scale = std::max(parent_scale,child_scale);
    double_matrix inv_scaleC1 = joint.invC / square(avg_scale);
    
    //double d_pos = inner_prod(joint_pos_offset, ublas::prod<double_vector>(invC1, joint_pos_offset));
    double d_pos = inner_prod(joint_pos_offset, ublas::prod<double_vector>(inv_scaleC1, joint_pos_offset));
    double d_rot = square(joint_rot_offset) / square(rot_sigma);

    double p_joint;
      

    p_joint = exp(-0.5*(d_pos + d_rot)) / (2*M_PI*sqrt(2*M_PI) * sqrt(detC1) * rot_sigma);

    //assert(p_joint >= 0);
    return p_joint;
  }
Ejemplo n.º 2
0
  double eval_joint_factor(const Joint &joint, const PartBBox& bbox_parent, const PartBBox &bbox_child, 
			   const double parent_scale, const double child_scale, 
			   double_vector &joint_pos_offset, double &joint_rot_offset, const double rot_step_size)
  {

    double rot_sigma = joint.rot_sigma;
    assert(rot_sigma >= 0);
    /*
    if (rot_sigma <= 0) {
      rot_sigma = 5*M_PI / 180.0;
    }
    */
    double child_rot = atan2(bbox_child.part_x_axis(1), bbox_child.part_x_axis(0));
    double parent_rot = atan2(bbox_parent.part_x_axis(1), bbox_parent.part_x_axis(0));

//     assert(child_scale == 1.0);
//     assert(parent_scale == 1.0);

    double_vector joint_pos_child = bbox_child.part_pos + 
      child_scale * bbox_child.part_x_axis * joint.offset_c(0) + 
      child_scale * bbox_child.part_y_axis * joint.offset_c(1);

    double_vector joint_pos_parent = bbox_parent.part_pos + 
      parent_scale * bbox_parent.part_x_axis * joint.offset_p(0) + 
      parent_scale * bbox_parent.part_y_axis * joint.offset_p(1);
    
    /* need to discretize the values as work on integer grid */
    joint_pos_child(0) = round(joint_pos_child(0));
    joint_pos_child(1) = round(joint_pos_child(1));
    joint_pos_parent(0) = round(joint_pos_parent(0));
    joint_pos_parent(1) = round(joint_pos_parent(1));    
    
    joint_pos_offset = joint_pos_child - joint_pos_parent;
    
    /* discretize rotation mean */
    double rot_mean = joint.rot_mean;
    if (rot_step_size > 0 && rot_step_size <= M_PI){
      rot_mean = boost_math::round(joint.rot_mean / rot_step_size) * rot_step_size;
    }
    joint_rot_offset = (child_rot - parent_rot) - rot_mean;
    
    /*
    while (joint_rot_offset < -M_PI)
      joint_rot_offset += M_PI;

    while (joint_rot_offset > M_PI)
      joint_rot_offset -= M_PI;
    */
    /*
    while (joint_rot_offset < -M_PI)
      joint_rot_offset += 2*M_PI;

    while (joint_rot_offset > M_PI)
      joint_rot_offset -= 2*M_PI;
    */
    /*
    while (joint_rot_offset < -2*M_PI)
      joint_rot_offset += 2*M_PI;

    while (joint_rot_offset > 2*M_PI)
      joint_rot_offset -= 2*M_PI;
    */
    /*
    while (joint_rot_offset < 0)
      joint_rot_offset += 2*M_PI;

    while (joint_rot_offset > 2*M_PI)
      joint_rot_offset -= 2*M_PI;
    */
    
    //assert(joint_rot_offset >= -M_PI && joint_rot_offset <= M_PI);
    // no wraparound
    
    //assert(joint_rot_offset >= -2*M_PI && joint_rot_offset <= 2*M_PI);
    
    /**
       how to estimate the vairance for parts at different scales ?
       
       MA: for now use the average scale
     */
    
    double detC1 = joint.detC;
    assert(detC1 > 0);
    
    double avg_scale = 0.5*(parent_scale + child_scale);
    //double avg_scale = std::max(parent_scale,child_scale);
    double_matrix inv_scaleC1 = joint.invC / square(avg_scale);

    //double d_pos = inner_prod(joint_pos_offset, ublas::prod<double_vector>(invC1, joint_pos_offset));
    double d_pos = inner_prod(joint_pos_offset, ublas::prod<double_vector>(inv_scaleC1, joint_pos_offset));
    double d_rot = square(joint_rot_offset) / square(rot_sigma);

    double p_joint;
    
    /** 
	MA: this is supposed to be comparable accross scales 

	the normalization constant does not depend on scales since we have implicitly rescaled positions to training scale
     */
    p_joint = exp(-0.5*(d_pos + d_rot)) / (2*M_PI*sqrt(2*M_PI) * sqrt(detC1) * rot_sigma);
    /*
    cout << "d_pos: " << d_pos << endl;  
    cout << p_joint << endl;
    cout << detC1 << endl;
    cout << rot_sigma << endl;
    */
    //p_joint = exp(-(d_pos + d_rot)); // (2*M_PI*sqrt(2*M_PI) * sqrt(detC1) * rot_sigma);
    /*
     cout << "rot_sigma: "  << rot_sigma << endl;
     cout << "joint.C:" << endl;
     boost_math::print_matrix(joint.C);
     //cout << "invC:" << endl; 
     //boost_math::print_matrix(invC1);
     cout << "d_pos: " << d_pos << endl;  
     cout << "d_rot: " << d_rot << endl; 
     cout << "p_joint: "  << p_joint << endl;
    */
    
    //assert(p_joint >= 0);
    return p_joint;
  }