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