whycon::LocalizationSystem::Pose whycon::LocalizationSystem::get_pose(const whycon::CircleDetector::Circle& circle) const {
Pose result;
double x,y,x1,x2,y1,y2,sx1,sx2,sy1,sy2,major,minor,v0,v1;
//transform the center
transform(circle.x,circle.y, x, y);
//calculate the major axis
//endpoints in image coords
sx1 = circle.x + circle.v0 * circle.m0 * 2;
sx2 = circle.x - circle.v0 * circle.m0 * 2;
sy1 = circle.y + circle.v1 * circle.m0 * 2;
sy2 = circle.y - circle.v1 * circle.m0 * 2;
//endpoints in camera coords
transform(sx1, sy1, x1, y1);
transform(sx2, sy2, x2, y2);
//semiaxis length
major = sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2))/2.0;
v0 = (x2-x1)/major/2.0;
v1 = (y2-y1)/major/2.0;
//calculate the minor axis
//endpoints in image coords
sx1 = circle.x + circle.v1 * circle.m1 * 2;
sx2 = circle.x - circle.v1 * circle.m1 * 2;
sy1 = circle.y - circle.v0 * circle.m1 * 2;
sy2 = circle.y + circle.v0 * circle.m1 * 2;
//endpoints in camera coords
transform(sx1, sy1, x1, y1);
transform(sx2, sy2, x2, y2);
//semiaxis length
minor = sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2))/2.0;
//construct the conic
double a,b,c,d,e,f;
a = v0*v0/(major*major)+v1*v1/(minor*minor);
b = v0*v1*(1/(major*major)-1/(minor*minor));
c = v0*v0/(minor*minor)+v1*v1/(major*major);
d = (-x*a-b*y);
e = (-y*c-b*x);
f = (a*x*x+c*y*y+2*b*x*y-1);
cv::Matx33d data(a,b,d,
b,c,e,
d,e,f);
// compute conic eigenvalues and eigenvectors
cv::Vec3d eigenvalues;
cv::Matx33d eigenvectors;
cv::eigen(data, eigenvalues, eigenvectors);
// compute ellipse parameters in real-world
double L1 = eigenvalues(1);
double L2 = eigenvalues(0);
double L3 = eigenvalues(2);
int V2 = 0;
int V3 = 2;
// position
double z = circle_diameter/sqrt(-L2*L3)/2.0;
cv::Matx13d position_mat = L3 * sqrt((L2 - L1) / (L2 - L3)) * eigenvectors.row(V2) + L2 * sqrt((L1 - L3) / (L2 - L3)) * eigenvectors.row(V3);
result.pos = cv::Vec3f(position_mat(0), position_mat(1), position_mat(2));
int S3 = (result.pos(2) * z < 0 ? -1 : 1);
result.pos *= S3 * z;
/*float dist = sqrt(L1 * L1 * L1) * circle_diameter * 0.5;
std::cout << "d1 " << dist << " " << cv::norm(result.pos) << std::endl;*/
WHYCON_DEBUG("ellipse center: " << x << "," << y << " " << " computed position: " << result.pos << " " << result.pos / result.pos(2));
// rotation
cv::Matx13d normal_mat = sqrt((L2 - L1) / (L2 - L3)) * eigenvectors.row(V2) + sqrt((L1 - L3) / (L2 - L3)) * eigenvectors.row(V3);
cv::normalize(cv::Vec3f(normal_mat(0), normal_mat(1), normal_mat(2)), result.rot, 1, cv::NORM_L2SQR);
result.rot(0) = atan2(result.rot(1), result.rot(0));
result.rot(1) = acos(result.rot(2));
result.rot(2) = 0; /* not recoverable */
/* TODO: to be checked */
/*cv::Matx33d data_inv;
cv::invert(data, data_inv);
cv::Matx31d projection = data_inv * normal_mat.t();
std::cout << "det " << cv::determinant(data_inv) << std::endl;
//cv::Vec3f new_center = cv::normalize(cv::Vec3f(projection(0), projection(1), projection(2))) * dist;
cv::Vec3f new_center = cv::Vec3f(projection(0) / projection(2), projection(1) / projection(2), 1) * dist;
std::cout << "center: " << new_center(0) << "," << new_center(1) << "," << new_center(2) << " vs " << result.pos << std::endl;
std::cout << "normalized: " << cv::normalize(new_center) << " " << cv::normalize(result.pos) << std::endl;*/
return result;
}
