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: using appropriate bin_width and bin_height for all cameras

Browse Source
This commit is contained in:
Anton Onishchenko
2017-05-10 13:54:11 +03:00
parent 8eab31c469
commit df40b0b8d3
3 changed files with 62 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
corelib/include/rtabmap/core/util3d.h
View File

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

50
corelib/src/util3d.cpp
View File

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

4
guilib/src/DepthCalibrationDialog.cpp
View File

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