diff --git a/corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h b/corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h index bfc974f1..ade192b1 100644 --- a/corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h +++ b/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 getDivisors(const size_t &num); + + // returns divisor from divisors closest to ref + static size_t getClosestToRef(const std::set &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), diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index e88cadb3..65745d41 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -319,15 +319,6 @@ pcl::PointCloud::Ptr RTABMAP_EXP loadCloud( int downsampleStep = 1, float voxelSize = 0.0f); -// returns all divisors of num -std::set get_divisors(const size_t &num); - -// returns divisor from divisors closest to ref -size_t GetClosestToRef(const std::set &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 diff --git a/corelib/src/clams/discrete_depth_distortion_model.cpp b/corelib/src/clams/discrete_depth_distortion_model.cpp index 777ad803..21d25af8 100644 --- a/corelib/src/clams/discrete_depth_distortion_model.cpp +++ b/corelib/src/clams/discrete_depth_distortion_model.cpp @@ -153,6 +153,57 @@ namespace clams UDEBUG("Frustum: multipliers=%d", multipliers_.rows()); } + std::set DiscreteDepthDistortionModel::getDivisors(const size_t &num) + { + std::set 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 &divisors, const double &ref) + { + std::set::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(height); + std::set 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; diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index 688e78fe..dceef1bf 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -2275,56 +2275,6 @@ pcl::PointCloud::Ptr loadCloud( return util3d::transformPointCloud(cloud, transform); } -std::set get_divisors(const size_t &num) -{ - std::set divisors; - for(size_t i = 1; i <= num; i++) - { - if(num % i == 0) - { - divisors.insert(i); - } - } - return divisors; -} - -size_t GetClosestToRef(const std::set &divisors, const double &ref) -{ - std::set::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(height); - std::set 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); -} - } } diff --git a/guilib/src/DepthCalibrationDialog.cpp b/guilib/src/DepthCalibrationDialog.cpp index 57bfc726..53faec50 100644 --- a/guilib/src/DepthCalibrationDialog.cpp +++ b/guilib/src/DepthCalibrationDialog.cpp @@ -392,7 +392,7 @@ void DepthCalibrationDialog::calibrate( if(_model == 0) { size_t bin_width, bin_height; - util3d::GetBinSize(imageSize.width, imageSize.height, 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);