diff --git a/include/calibu/conics/Conic.h b/include/calibu/conics/Conic.h index 9aaa214..866889e 100644 --- a/include/calibu/conics/Conic.h +++ b/include/calibu/conics/Conic.h @@ -65,4 +65,8 @@ std::pair PlaneFromConics( CALIBU_EXPORT Conic UnmapConic( const Conic& c, const CameraModelInterface& cam ); +/** Returns the major and minor axes lengths of the conic */ +CALIBU_EXPORT +Eigen::Vector2d GetAxesLengths(const Conic& c); + } diff --git a/src/conics/Conic.cpp b/src/conics/Conic.cpp index 266f73d..384cd27 100644 --- a/src/conics/Conic.cpp +++ b/src/conics/Conic.cpp @@ -28,6 +28,19 @@ using namespace Eigen; namespace calibu { +Eigen::Vector2d GetAxesLengths(const Conic& c) { + // Extract and scale the eigenvalues to get the major, minor axes lengths + Eigen::Vector2d eigenval = c.C.topLeftCorner<2, 2>().eigenvalues().real(); + + // See + // http://en.wikipedia.org/wiki/Matrix_representation_of_conic_sections + // for explanation of matrix C and its relation to the axes + double det_ratio = -c.C.determinant() / + c.C.topLeftCorner<2, 2>().determinant(); + return Eigen::Vector2d(sqrt(det_ratio / eigenval[0]), + sqrt(det_ratio / eigenval[1])); +} + double Distance( const Conic& c1, const Conic& c2, double circle_radius ) { const Matrix3d Q = c1.Dual * c2.C;