implemented cornerAperture() and working on testing yet ...

src/corner_detector.cpp

@@ -237,6 +237,7 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
         }
     }
     std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
+    for(std::list<laserscanutils::Corner>::iterator corner_it1 = _corner_list.begin(); corner_it1 != _corner_list.end(); corner_it1++) corner_it1->print(); 
 
     return _corner_list.size();
 }
@@ -244,10 +245,10 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
 laserscanutils::ScalarT laserscanutils::cornerAperture(const Eigen::Vector3s & _p1, const Eigen::Vector3s & _c, const Eigen::Vector3s & _p2)
 {
     //TODO: use lines as inputs instead of _p1 and _p2 -> better estimation of the aperture value
- /*  
+    
     //buid lines from c to p1 and p2
-    Eigen::Vector3s l1 = _p1.cross(_c);
-    Eigen::Vector3s l2 = _p2.cross(_c);
+    Eigen::Vector3s l1 = _c.cross(_p1);
+    Eigen::Vector3s l2 = _c.cross(_p2);
     
     //normalize lines
     l1 = l1 / ( sqrt( l1(0)*l1(0)+l1(1)*l1(1) ) );
@@ -256,19 +257,14 @@ laserscanutils::ScalarT laserscanutils::cornerAperture(const Eigen::Vector3s & _
     //compute angle
     ScalarT aux = l1(0)*l2(0) + l1(1)*l2(1); 
     if (fabs(aux) > 1.) aux = aux / fabs(aux); //limit here to +-1 to avoid complex numbers due to numerical precision when acos(>1). Anyway, corners will be far from this situation
-    alpha = acos(aux); //alpha in [0,pi]
+    ScalarT alpha = acos(aux); //alpha in [0,pi]
     
     //build matrix M and compute its determinant
-    Eigen::Matrix3s M << _p1,_c
-    ScalarT detM = 
-                
+    Eigen::Matrix3s M;
+    M << _p1,_c, _p2;
+    ScalarT detM = M.determinant(); 
     
     //if det<0, return the complementary angle
-    if(detM<0) then
-        ret_alpha = 2*%pi-alpha;
-    else
-        ret_alpha = alpha;    
-    end
-*/
-    return 0;
+    if ( detM < 0 ) return ( 2*M_PI - alpha );
+    else return alpha;    
 }

src/examples/CMakeLists.txt

@@ -2,4 +2,8 @@
 IF(faramotics_FOUND)
     ADD_EXECUTABLE(test_extract_corners test_extract_corners.cpp)
     TARGET_LINK_LIBRARIES(test_extract_corners ${GLUT_glut_LIBRARY} ${pose_state_time_LIBRARIES} ${faramotics_LIBRARIES} ${PROJECT_NAME})
-ENDIF(faramotics_FOUND)
+ENDIF(faramotics_FOUND)
\ No newline at end of file
+
+# corner aperture test
+ADD_EXECUTABLE(test_corner_aperture test_corner_aperture.cpp)
+TARGET_LINK_LIBRARIES(test_corner_aperture ${PROJECT_NAME})

src/examples/test_corner_aperture.cpp

+//std includes
+#include <iostream>
+
+//laserscanutils
+#include "entities.h"
+#include "corner_detector.h"
+
+//main
+int main(int argc, char** argv)
+{
+    std::cout << "\n ========= Corner aperture test ===========\n";
+
+    //declare two points and a corner in between
+    Eigen::Vector3s p1; p1 << 2,4,1;
+    Eigen::Vector3s p2; p2 << 4,1,1;
+    Eigen::Vector3s c; c << 4,4,1;
+    
+    //compute aperture 
+    laserscanutils::ScalarT ap = laserscanutils::cornerAperture(p1,c,p2);
+    std::cout << "Aperture: " << ap << std::endl;
+    
+    //exit
+    return 0;
+}

src/examples/test_extract_corners.cpp

@@ -158,6 +158,6 @@ int main(int argc, char** argv)
 	delete myRender;
     delete myScanner;
     
-    //exit
+	//exit
 	return 0;
 }