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 {