Merge pull request #194 from T0ny0/fix_Kinect2_depth_calibration

Depth calibration: using appropriate bin_width and bin_height for all cameras

corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h

@@ -66,6 +66,16 @@ namespace clams
 
   class RTABMAP_EXP DiscreteDepthDistortionModel
   {
+  public:
+    // returns all divisors of num
+    static std::set<size_t> getDivisors(const size_t &num);
+
+    // returns divisor from divisors closest to ref
+    static size_t getClosestToRef(const std::set<size_t> &divisors, const double &ref);
+
+    // sets bin_width and bin_height to appropriate values
+    static void getBinSize(const size_t &width, const size_t &height, size_t &bin_width, size_t &bin_height);
+
   public:
     DiscreteDepthDistortionModel() :
     	width_(0),

corelib/src/clams/discrete_depth_distortion_model.cpp

@@ -153,6 +153,57 @@ namespace clams
       UDEBUG("Frustum: multipliers=%d", multipliers_.rows());
     }
 
+  std::set<size_t> DiscreteDepthDistortionModel::getDivisors(const size_t &num)
+  {
+    std::set<size_t> divisors;
+    for(size_t i = 1; i <= num; i++)
+    {
+      if(num % i == 0)
+      {
+        divisors.insert(i);
+      }
+    }
+    return divisors;
+  }
+
+  size_t DiscreteDepthDistortionModel::getClosestToRef(const std::set<size_t> &divisors, const double &ref)
+  {
+    std::set<size_t>::iterator low, prev;
+    low = divisors.lower_bound(ref);
+    if(low == divisors.end())
+    {
+      return *(--divisors.end());
+    }
+    else if(low == divisors.begin())
+    {
+      return *low;
+    }
+    else
+    {
+      prev = low;
+      --prev;
+      if((ref - *prev) <= (*low - ref))
+      {
+        return *prev;
+      }
+      else
+      {
+        return *low;
+      }
+    }
+  }
+
+  void DiscreteDepthDistortionModel::getBinSize(const size_t &width, const size_t &height, size_t &bin_width, size_t &bin_height) {
+    double ratio = width / static_cast<double>(height);
+    std::set<size_t> divisors = getDivisors(width);
+    double ref_bin_width = 8;
+    bin_width = getClosestToRef(divisors, ref_bin_width);
+    divisors = getDivisors(height);
+    double ref_bin_height = ref_bin_width / ratio;
+    bin_height = getClosestToRef(divisors, ref_bin_height);
+  }
+
+
   DiscreteDepthDistortionModel::DiscreteDepthDistortionModel(const DiscreteDepthDistortionModel& other)
   {
     *this = other;

guilib/src/DepthCalibrationDialog.cpp

@@ -391,7 +391,9 @@ void DepthCalibrationDialog::calibrate(
 			const cv::Size & imageSize = sequence.begin()->second.cameraModels()[0].imageSize();
 			if(_model == 0)
 			{
-				_model = new clams::DiscreteDepthDistortionModel(imageSize.width, imageSize.height);
+				size_t bin_width, bin_height;
+                                clams::DiscreteDepthDistortionModel::getBinSize(imageSize.width, imageSize.height, bin_width, bin_height);
+				_model = new clams::DiscreteDepthDistortionModel(imageSize.width, imageSize.height, bin_width, bin_height);
 			}
 			UASSERT(_model->getWidth() == imageSize.width && _model->getHeight() == imageSize.height);