Merge branch 'ethz-asl:master' into master

include/rovio/Camera.hpp

@@ -41,7 +41,8 @@ class Camera{
    * */
   enum ModelType{
     RADTAN,    //!< Radial tangential distortion model.
-    EQUIDIST   //!< Equidistant distortion model.
+    EQUIDIST,  //!< Equidistant distortion model.
+    DS         //!< Double sphere distortion model.
   } type_;
 
   Eigen::Matrix3d K_; //!< Intrinsic parameter matrix.
@@ -52,6 +53,11 @@ class Camera{
   double p1_,p2_,s1_,s2_,s3_,s4_;
   //@}
 
+  //@{
+  /** \brief Radius (as ratio of images shortest side) within which features can be initalized. */
+  double valid_radius_;
+  //@}
+
   /** \brief Constructor.
    *
    *  Initializes the camera object as pinhole camera, i.e. all distortion coefficients are set to zero.
@@ -81,6 +87,12 @@ class Camera{
    */
   void loadEquidist(const std::string& filename);
 
+  /** \brief Loads and sets the distortion parameters {k1_, k2_} for the Double Sphere distortion model from
+   *         yaml-file.
+   *   @param filename - Path to the yaml-file, containing the distortion coefficients.
+   */
+  void loadDoubleSphere(const std::string& filename);
+
   /** \brief Loads and sets the distortion model and the corresponding distortion coefficients from yaml-file.
    *
    *   @param filename - Path to the yaml-file, containing the distortion model and distortion coefficient data.
@@ -119,6 +131,22 @@ class Camera{
    */
   void distortEquidist(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const;
 
+  /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Double Sphere distortion model.
+   *
+   *   @param in  - Undistorted point coordinates on the unit plane (in camera coordinates).
+   *   @param out - Distorted point coordinates on the unit plane (in camera coordinates).
+   */
+  void distortDoubleSphere(const Eigen::Vector2d& in, Eigen::Vector2d& out) const;
+
+  /** \brief Distorts a point on the unit plane (in camera coordinates) according to the Double Sphere distortion model
+   *         and outputs additionally the corresponding jacobian matrix (input to output).
+   *
+   *   @param in  - Undistorted point coordinates on the unit plane (in camera coordinates).
+   *   @param out - Distorted point coordinates on the unit plane (in camera coordinates).
+   *   @param J   - Jacobian matrix of the distortion process (input to output).
+   */
+  void distortDoubleSphere(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const;
+
   /** \brief Distorts a point on the unit plane, according to the set distortion model (#ModelType) and to the set
    *         distortion coefficients.
    *

include/rovio/ImagePyramid.hpp

@@ -134,8 +134,9 @@ class ImagePyramid{
    * @param l                  - Pyramid level at which the corners should be extracted.
    * @param detectionThreshold - Detection threshold of the used cv::FastFeatureDetector.
    *                             See http://docs.opencv.org/trunk/df/d74/classcv_1_1FastFeatureDetector.html
+   * @param valid_radius       - Radius inside which a feature is considered valid (as ratio of shortest image side)
    */
-  void detectFastCorners(FeatureCoordinatesVec & candidates, int l, int detectionThreshold) const{
+  void detectFastCorners(FeatureCoordinatesVec & candidates, int l, int detectionThreshold, double valid_radius = std::numeric_limits<double>::max()) const{
     std::vector<cv::KeyPoint> keypoints;
 #if (CV_MAJOR_VERSION < 3)
     cv::FastFeatureDetector feature_detector_fast(detectionThreshold, true);
@@ -144,8 +145,18 @@ class ImagePyramid{
     auto feature_detector_fast = cv::FastFeatureDetector::create(detectionThreshold, true);
     feature_detector_fast->detect(imgs_[l], keypoints);
 #endif
+
     candidates.reserve(candidates.size()+keypoints.size());
     for (auto it = keypoints.cbegin(), end = keypoints.cend(); it != end; ++it) {
+
+      const double x_dist = it->pt.x - imgs_[l].cols/2.0;
+      const double y_dist = it->pt.y - imgs_[l].rows/2.0;
+      const double max_valid_dist = valid_radius*std::min(imgs_[l].cols, imgs_[l].rows);
+
+      if((x_dist*x_dist + y_dist*y_dist) > (max_valid_dist*max_valid_dist)){
+        continue;
+      }
+
       candidates.push_back(
               levelTranformCoordinates(FeatureCoordinates(cv::Point2f(it->pt.x, it->pt.y)),l,0));
     }

include/rovio/ImgUpdate.hpp

@@ -985,7 +985,7 @@ ImgOutlierDetection<typename FILTERSTATE::mtState>,false>{
         const double t1 = (double) cv::getTickCount();
         candidates_.clear();
         for(int l=endLevel_;l<=startLevel_;l++){
-          meas.aux().pyr_[camID].detectFastCorners(candidates_,l,fastDetectionThreshold_);
+          meas.aux().pyr_[camID].detectFastCorners(candidates_,l,fastDetectionThreshold_, mpMultiCamera_->cameras_[camID].valid_radius_);
         }
         const double t2 = (double) cv::getTickCount();
         if(verbose_) std::cout << "== Detected " << candidates_.size() << " on levels " << endLevel_ << "-" << startLevel_ << " (" << (t2-t1)/cv::getTickFrequency()*1000 << " ms)" << std::endl;

src/Camera.cpp

@@ -8,6 +8,7 @@ namespace rovio{
     p1_ = 0.0; p2_ = 0.0; s1_ = 0.0; s2_ = 0.0; s3_ = 0.0; s4_ = 0.0;
     K_.setIdentity();
     type_ = RADTAN;
+    valid_radius_ = std::numeric_limits<double>::max();
   };
 
   Camera::~Camera(){};
@@ -47,6 +48,19 @@ namespace rovio{
     std::cout << "Set distortion parameters (Equidist) to: k1(" << k1_ << "), k2(" << k2_ << "), k3(" << k3_ << "), k4(" << k4_ << ")" << std::endl;
   }
 
+  void Camera::loadDoubleSphere(const std::string& filename){
+    loadCameraMatrix(filename);
+    YAML::Node config = YAML::LoadFile(filename);
+    k1_ = config["distortion_coefficients"]["data"][0].as<double>();
+    k2_ = config["distortion_coefficients"]["data"][1].as<double>();
+
+    if(config["valid_radius"]){
+      valid_radius_ = config["valid_radius"].as<double>();
+    }
+
+    std::cout << "Set distortion parameters (Double Sphere) to: k1(" << k1_ << "), k2(" << k2_ << "), valid_radius(" << valid_radius_ << ")" << std::endl;
+  }
+
   void Camera::load(const std::string& filename){
     YAML::Node config = YAML::LoadFile(filename);
     std::string distortionModel;
@@ -57,6 +71,9 @@ namespace rovio{
     } else if(distortionModel == "equidistant"){
       type_ = EQUIDIST;
       loadEquidist(filename);
+    } else if(distortionModel == "ds"){
+      type_ = DS;
+      loadDoubleSphere(filename);
     } else {
       std::cout << "ERROR: no camera Model detected!";
     }
@@ -144,6 +161,54 @@ namespace rovio{
     J(1,1) = s + in(1)*s_r*r_y;
   }
 
+  void Camera::distortDoubleSphere(const Eigen::Vector2d& in, Eigen::Vector2d& out) const{
+
+      const double x2 = in(0) * in(0);
+      const double y2 = in(1) * in(1);
+
+      if((x2 + y2) < 1e-16){
+        out(0) = in(0);
+        out(1) = in(1);
+        return;
+      }
+
+      const double d1 = std::sqrt(x2 + y2 + 1.0);
+      const double d2 = std::sqrt(x2 + y2 + (k1_*d1 + 1.0)*(k1_*d1 + 1.0));
+      const double scaling = 1.0f/(k2_*d2 + (1-k2_)*(k1_*d1+1.0));
+
+      out(0) = in(0) * scaling;
+      out(1) = in(1) * scaling;
+  }
+
+  void Camera::distortDoubleSphere(const Eigen::Vector2d& in, Eigen::Vector2d& out, Eigen::Matrix2d& J) const{
+    const double x2 = in(0) * in(0);
+    const double y2 = in(1) * in(1);
+
+    if((x2 + y2) < 1e-16){
+      out(0) = in(0);
+      out(1) = in(1);
+      J.setIdentity();
+      return;
+    }
+
+    const double d1 = std::sqrt(x2 + y2 + 1.0);
+    const double d2 = std::sqrt(x2 + y2 + (k1_*d1 + 1.0)*(k1_*d1 + 1.0));
+    const double s = 1.0f/(k2_*d2 + (1-k2_)*(k1_*d1+1.0));
+
+    out(0) = in(0) * s;
+    out(1) = in(1) * s;
+
+    const double d1dx = in(0)/d1;
+    const double d1dy = in(1)/d1;
+    const double d2dx = (in(0) + d1dx*k1_*(d1*k1_ + 1.0))/(d2);
+    const double d2dy = (in(1) + d1dy*k1_*(d1*k1_ + 1.0))/(d2);
+
+    J(0,0) = -in(0)*(d2dx*k2_ - d1dx*k1_*(k2_ - 1.0))*s*s + s;
+    J(0,1) = -s*s*in(0)*(d2dy*k2_ - d1dy*k1_*(k2_ - 1.0));
+    J(1,0) = -s*s*in(1)*(d2dx*k2_ - d1dx*k1_*(k2_ - 1.0));
+    J(1,1) = -in(1)*(d2dy*k2_ - d1dy*k1_*(k2_ - 1.0))*s*s + s;
+  }
+
   void Camera::distort(const Eigen::Vector2d& in, Eigen::Vector2d& out) const{
     switch(type_){
       case RADTAN:
@@ -152,6 +217,9 @@ namespace rovio{
       case EQUIDIST:
         distortEquidist(in,out);
         break;
+      case DS:
+        distortDoubleSphere(in,out);
+        break;
       default:
         distortRadtan(in,out);
         break;
@@ -166,6 +234,9 @@ namespace rovio{
       case EQUIDIST:
         distortEquidist(in,out,J);
         break;
+      case DS:
+        distortDoubleSphere(in,out,J);
+        break;
       default:
         distortRadtan(in,out,J);
         break;