Depth calibration: using appropriate bin_width and bin_height for all cameras

corelib/include/rtabmap/core/util3d.h

@@ -319,6 +319,15 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadCloud(
 		int downsampleStep = 1,
 		float voxelSize = 0.0f);
 
+// returns all divisors of num
+std::set<size_t> get_divisors(const size_t &num);
+
+// returns divisor from divisors closest to ref
+size_t GetClosestToRef(const std::set<size_t> &divisors, const double &ref);
+
+// sets bin_width and bin_height to appropriate values
+void GetBinSize(const size_t &width, const size_t &height, size_t &bin_width, size_t &bin_height);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/src/util3d.cpp

@@ -2275,6 +2275,56 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
 	return util3d::transformPointCloud(cloud, transform);
 }
 
+std::set<size_t> get_divisors(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 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 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 = get_divisors(width);
+	double ref_bin_width = 8;
+	bin_width = GetClosestToRef(divisors, ref_bin_width);
+	divisors = get_divisors(height);
+	double ref_bin_height = ref_bin_width / ratio;
+	bin_height = GetClosestToRef(divisors, ref_bin_height);
+}
+
 }
 
 }

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;
+				util3d::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);