i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

Depth calibration: move GetBinSize to clams namespace

Browse Source
This commit is contained in:
Anton Onishchenko
2017-05-11 11:38:42 +03:00
parent df40b0b8d3
commit 197bb2179f
5 changed files with 62 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
corelib/src/clams/discrete_depth_distortion_model.cpp
View File

@@ -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;

50
corelib/src/util3d.cpp
View File

@@ -2275,56 +2275,6 @@ 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);
}
}
}