Assymetric Calibration Target

aslam_cv/aslam_cameras/include/aslam/cameras/GridCalibrationTargetBase.hpp

@@ -15,6 +15,7 @@
 #include <sm/boost/serialization.hpp>
 #include <sm/assert_macros.hpp>
 
+
 namespace aslam {
 namespace cameras {
 
@@ -78,7 +79,8 @@ class GridCalibrationTargetBase {
   ///        in outImagePoints was observed
   virtual bool computeObservation(const cv::Mat & /*image*/,
                                   Eigen::MatrixXd & /*outImagePoints*/,
-                                  std::vector<bool> & /*outCornerObserved*/) const
+                                  std::vector<bool> & /*outCornerObserved*/
+                                  ) const
   {
     SM_ASSERT_TRUE(Exception, true, "you need to implement this method for each target!");
     return false;

aslam_cv/aslam_cameras/include/aslam/cameras/PinholeProjection.hpp

@@ -11,7 +11,6 @@
 #include <aslam/cameras/GridCalibrationTargetObservation.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/core/eigen.hpp>
-#include <sm/logging.hpp>
 
 namespace aslam {
 namespace cameras {
@@ -209,7 +208,7 @@ class PinholeProjection {
   int height() const {
     return _rv;
   }
-
+  
   int keypointDimension() const {
     return KeypointDimension;
   }

aslam_cv/aslam_cameras/include/aslam/cameras/implementation/OmniProjection.hpp

@@ -744,7 +744,7 @@ bool OmniProjection<DISTORTION_T>::initializeIntrinsics(const std::vector<GridCa
 
   // Now we try to find a non-radial line to initialize the focal length
   bool success = false;
-
+  bool assymetric = false;
   //go though all images and pick the best estimate (=lowest mean reproj. err)
   for (size_t i = 0; i < observations.size(); ++i) {
     const GridCalibrationTargetObservation& obs = observations.at(i);
@@ -762,6 +762,11 @@ bool OmniProjection<DISTORTION_T>::initializeIntrinsics(const std::vector<GridCa
         Eigen::Vector2d imagePoint;
         Eigen::Vector3d gridPoint;
 
+        if (target.gridPoint(r,c)(0)==-1){
+          assymetric = true;
+          continue;
+        }
+
         if (obs.imageGridPoint(r, c, imagePoint)) {
           double u = imagePoint[0] - _cu;
           double v = imagePoint[1] - _cv;
@@ -774,8 +779,10 @@ bool OmniProjection<DISTORTION_T>::initializeIntrinsics(const std::vector<GridCa
       }
 
       // MIN_CORNERS is an arbitrary threshold for the number of corners
-      const size_t MIN_CORNERS = 4;
-      if (count > MIN_CORNERS)
+      size_t MIN_CORNERS = 4;
+      if (assymetric)
+        MIN_CORNERS = 2;
+      if (count >= MIN_CORNERS)
       {
         // Resize P to fit with the count of valid points.
         cv::Mat C;

aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp

@@ -1,4 +1,5 @@
 #include <opencv2/core/eigen.hpp>
+
 #include <Eigen/StdVector>
 
 namespace aslam {
@@ -26,7 +27,6 @@ PinholeProjection<DISTORTION_T>::PinholeProjection(
   _cv = config.getDouble("cv");
   _ru = config.getInt("ru");
   _rv = config.getInt("rv");
-
   updateTemporaries();
 }
 
@@ -36,8 +36,9 @@ PinholeProjection<DISTORTION_T>::PinholeProjection(double focalLengthU,
                                                    double imageCenterU,
                                                    double imageCenterV,
                                                    int resolutionU,
-                                                   int resolutionV,
-                                                   distortion_t distortion)
+                                                   int resolutionV, 
+                                                   distortion_t distortion
+                                                  )
     : _fu(focalLengthU),
       _fv(focalLengthV),
       _cu(imageCenterU),
@@ -54,13 +55,15 @@ PinholeProjection<DISTORTION_T>::PinholeProjection(double focalLengthU,
                                                    double imageCenterU,
                                                    double imageCenterV,
                                                    int resolutionU,
-                                                   int resolutionV)
+                                                   int resolutionV
+                                                   )
     : _fu(focalLengthU),
       _fv(focalLengthV),
       _cu(imageCenterU),
       _cv(imageCenterV),
       _ru(resolutionU),
       _rv(resolutionV) {
+
   updateTemporaries();
 }
 
@@ -86,6 +89,7 @@ bool PinholeProjection<DISTORTION_T>::euclideanToKeypoint(
   outKeypoint[0] = p[0] * rz;
   outKeypoint[1] = p[1] * rz;
 
+
   _distortion.distort(outKeypoint);
 
   outKeypoint[0] = _fu * outKeypoint[0] + _cu;
@@ -147,7 +151,6 @@ template<typename DERIVED_P, typename DERIVED_K>
 bool PinholeProjection<DISTORTION_T>::homogeneousToKeypoint(
     const Eigen::MatrixBase<DERIVED_P> & ph,
     const Eigen::MatrixBase<DERIVED_K> & outKeypoint) const {
-
   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
       Eigen::MatrixBase<DERIVED_P>, 4);
   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
@@ -210,6 +213,7 @@ bool PinholeProjection<DISTORTION_T>::keypointToEuclidean(
 
   kp[0] = (kp[0] - _cu) / _fu;
   kp[1] = (kp[1] - _cv) / _fv;
+
   _distortion.undistort(kp);  // revert distortion
 
   Eigen::MatrixBase<DERIVED_P> & outPoint = const_cast<Eigen::MatrixBase<
@@ -611,6 +615,7 @@ class PinholeHelpers {
     std::vector<cv::Point2d> ipts;
 
     double d = hypot(x1-x2, y1-y2);
+
     if (d > r1 + r2) {
       // circles are separate
       return ipts;
@@ -693,6 +698,7 @@ class PinholeHelpers {
     double median;
     size_t size = values.size();
     std::sort(values.begin(), values.end());
+
     if (size%2 == 0)
         median = (values[size / 2 - 1] + values[size / 2]) / 2;
     else
@@ -720,16 +726,19 @@ bool PinholeProjection<DISTORTION_T>::initializeIntrinsics(const std::vector<Gri
   _rv = observations[0].imRows();
   _distortion.clear();
 
+  //bool assymetricGrid =false;
   //process all images
   size_t nImages = observations.size();
 
+
   // Initialize focal length
   // C. Hughes, P. Denny, M. Glavin, and E. Jones,
   // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point
   // Extraction, PAMI 2010
   // Find circles from rows of chessboard corners, and for each pair
   // of circles, find vanishing points: v1 and v2.
   // f = ||v1 - v2|| / PI;
+
   std::vector<double> f_guesses;
 
   for (size_t i=0; i<nImages; ++i) {
@@ -739,25 +748,43 @@ bool PinholeProjection<DISTORTION_T>::initializeIntrinsics(const std::vector<Gri
 
     std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> center(target.rows());
     double radius[target.rows()];
-    bool skipImage=false;
 
+    int count=0;
+    int MIN_TAGS = 3; 
+    //If the grid is assymetric we never see all grid Points. Initialization can't be the same as for the symmetric case.
+    bool skipImage=false;
+    bool assymetric=false;
     for (size_t r=0; r<target.rows(); ++r) {
       std::vector<cv::Point2d> circle;
+
       for (size_t c=0; c<target.cols(); ++c) {
         Eigen::Vector2d imagePoint;
         Eigen::Vector3d gridPoint;
-
-        if (obs.imageGridPoint(r, c, imagePoint))
+        //For low resolution can fail -> change method to allow less observations or initialize with constant value
+        if (target.gridPoint(r,c)(0)==-1){
+          assymetric = true;
+          continue;
+        }
+        if (obs.imageGridPoint(r, c, imagePoint)){
           circle.push_back(cv::Point2f(imagePoint[0], imagePoint[1]));
-        else
+          count ++;  
+        }
+        else if(!assymetric)
           //skip this image if the board view is not complete
           skipImage=true;
+
       }
+
       PinholeHelpers::fitCircle(circle, center[r](0), center[r](1), radius[r]);
     }
+    //If we see two points of the same tag we have at least one horizontal line. See at least 3 tags? 2 tags?
+      if(count >= MIN_TAGS*4 && assymetric)
+        skipImage=false;
 
-    if(skipImage)
+    if(skipImage){
       continue;
+    }
+
 
     for (size_t j=0; j<target.rows(); ++j)
     {
@@ -766,10 +793,13 @@ bool PinholeProjection<DISTORTION_T>::initializeIntrinsics(const std::vector<Gri
         // find distance between pair of vanishing points which
         // correspond to intersection points of 2 circles
         std::vector < cv::Point2d > ipts;
+        if( std::isinf(center[j](0)) || std::isnan(center[j](0)) || std::isinf(center[k](0)) || std::isnan(center[k](0)) )
+             continue;
         ipts = PinholeHelpers::intersectCircles(center[j](0), center[j](1),
                                                 radius[j], center[k](0), center[k](1), radius[k]);
+
         if (ipts.size()<2)
-          continue;
+            continue;
 
          double f_guess = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI;
          f_guesses.push_back(f_guess);
@@ -780,8 +810,8 @@ bool PinholeProjection<DISTORTION_T>::initializeIntrinsics(const std::vector<Gri
   //get the median of the guesses
   if(f_guesses.empty())
     return false;
-  double f0 = PinholeHelpers::medianOfVectorElements(f_guesses);
 
+  double f0 = PinholeHelpers::medianOfVectorElements(f_guesses);
   //set the estimate
   _fu = f0;
   _fv = f0;
@@ -801,6 +831,7 @@ size_t PinholeProjection<DISTORTION_T>::computeReprojectionError(
 
   for (size_t i = 0; i < obs.target()->size(); ++i) {
     Eigen::Vector2d y, yhat;
+
     if (obs.imagePoint(i, y)
         && euclideanToKeypoint(T_camera_target * obs.target()->point(i),
                                yhat)) {
@@ -819,13 +850,14 @@ template<typename DISTORTION_T>
 bool PinholeProjection<DISTORTION_T>::estimateTransformation(
     const GridCalibrationTargetObservation & obs,
     sm::kinematics::Transformation & out_T_t_c) const {
-
+  
   std::vector<cv::Point2f> Ms;
   std::vector<cv::Point3f> Ps;
 
   // Get the observed corners in the image and target frame
   obs.getCornersImageFrame(Ms);
   obs.getCornersTargetFrame(Ps);
+
 
   // Convert all target corners to a fakey pinhole view.
   size_t count = 0;
@@ -848,7 +880,8 @@ bool PinholeProjection<DISTORTION_T>::estimateTransformation(
       ++count;
     }
   }
-
+
+
   Ps.resize(count);
   Ms.resize(count);
 
@@ -859,9 +892,10 @@ bool PinholeProjection<DISTORTION_T>::estimateTransformation(
 
   if (Ps.size() < 4)
     return false;
+
 
-  // Call the OpenCV pnp function.
-  cv::solvePnP(Ps, Ms, cv::Mat::eye(3, 3, CV_64F), distCoeffs, rvec, tvec);
+  // Call the OpenCV pnp function.  
+   cv::solvePnP(Ps, Ms, cv::Mat::eye(3, 3, CV_64F), distCoeffs, rvec, tvec);
 
   // convert the rvec/tvec to a transformation
   cv::Mat C_camera_model = cv::Mat::eye(3, 3, CV_64F);
@@ -878,6 +912,7 @@ bool PinholeProjection<DISTORTION_T>::estimateTransformation(
   return true;
 }
 
+
 }  // namespace cameras
 
 }  // namespace aslam

aslam_cv/aslam_cameras/src/GridCalibrationTargetObservation.cpp

@@ -33,6 +33,7 @@ void GridCalibrationTargetObservation::setTarget(
   _points = Eigen::MatrixXd::Zero(target->size(), 2);
   _success.clear();
   _success.resize(target->size(), false);
+
 }
 
 /// \brief get all (observed) corners in image coordinates

aslam_cv/aslam_cameras/src/GridDetector.cpp

@@ -92,10 +92,10 @@ bool GridDetector::findTarget(const cv::Mat & image,
 bool GridDetector::findTargetNoTransformation(const cv::Mat & image, const aslam::Time & stamp,
     GridCalibrationTargetObservation & outObservation) const {
   bool success = false;
-
   // Extract the calibration target corner points
   Eigen::MatrixXd cornerPoints;
   std::vector<bool> validCorners;
+
   success = _target->computeObservation(image, cornerPoints, validCorners);
 
   // Set the image, target, and timestamp regardless of success.
@@ -105,10 +105,13 @@ bool GridDetector::findTargetNoTransformation(const cv::Mat & image, const aslam
 
   // Set the observed corners in the observation
   for (int i = 0; i < cornerPoints.rows(); i++) {
-    if (validCorners[i])
+
+   //if Success we have validCorners and updateImagePoint runs
+
+    if (validCorners[i]){
       outObservation.updateImagePoint(i, cornerPoints.row(i).transpose());
+    }
   }
-
   return success;
 }
 

aslam_cv/aslam_cameras_april/CMakeLists.txt

@@ -12,6 +12,7 @@ include_directories(${Eigen_INCLUDE_DIRS})
 
 cs_add_library(${PROJECT_NAME} 
   src/GridCalibrationTargetAprilgrid.cpp
+  src/GridCalibrationTargetAssymetricAprilgrid.cpp
   )
 
 find_package(Boost REQUIRED COMPONENTS serialization system) 

aslam_cv/aslam_cameras_april/include/aslam/cameras/GridCalibrationTargetAprilgrid.hpp

@@ -15,7 +15,8 @@
 //#include "apriltags/Tag25h7.h"
 //#include "apriltags/Tag25h9.h"
 //#include "apriltags/Tag36h9.h"
-#include "apriltags/Tag36h11.h"
+//#include "apriltags/Tag36h11.h"
+#include "apriltags/AllTags.h"
 
 namespace aslam {
 namespace cameras {