43 using namespace alvar;
49 median_poses =
new Pose[median_n];
53 median_poses[median_ind] = new_pose;
54 median_ind = (median_ind+1) % median_n;
59 if(median_ind == median_n-1)
61 ret_pose = median_poses[median_ind-1];
67 for(
int i=0; i<median_n; i++){
68 double total_dist = 0;
69 for(
int j=0; j<median_n; j++){
70 total_dist += pow(median_poses[i].translation[0] - median_poses[j].translation[0], 2);
71 total_dist += pow(median_poses[i].translation[1] - median_poses[j].translation[1], 2);
72 total_dist += pow(median_poses[i].translation[2] - median_poses[j].translation[2], 2);
73 total_dist += pow(median_poses[i].quaternion[0] - median_poses[j].quaternion[0], 2);
74 total_dist += pow(median_poses[i].quaternion[1] - median_poses[j].quaternion[1], 2);
75 total_dist += pow(median_poses[i].quaternion[2] - median_poses[j].quaternion[2], 2);
76 total_dist += pow(median_poses[i].quaternion[3] - median_poses[j].quaternion[3], 2);
78 if(i==0) min_dist = total_dist;
80 if(total_dist < min_dist){
81 min_dist = total_dist;
86 ret_pose = median_poses[min_ind];
Pose representation derived from the Rotation class