Adds calibu::GetAxesLengths.

arpg · May 13, 2014 · 119dd57 · 119dd57
1 parent 9d05b10
commit 119dd57
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 0 deletions.
diff --git a/include/calibu/conics/Conic.h b/include/calibu/conics/Conic.h
@@ -65,4 +65,8 @@ std::pair<Eigen::Vector3d,Eigen::Matrix3d > 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
@@ -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;