discrete_depth_distortion_model.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the <organization> nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 RTAB-Map integration: Mathieu Labbe
28 */
29 
33 #include <rtabmap/utilite/UMath.h>
34 #include <rtabmap/utilite/UFile.h>
36 
37 using namespace std;
38 using namespace Eigen;
39 
40 namespace clams
41 {
42 
43  DiscreteFrustum::DiscreteFrustum(int smoothing, double bin_depth, double max_dist) :
44  max_dist_(max_dist),
45  bin_depth_(bin_depth)
46  {
48  UASSERT(bin_depth_>0.0);
49  UASSERT(smoothing>=1);
51  counts_ = VectorXf::Ones(num_bins_) * smoothing;
52  total_numerators_ = VectorXf::Ones(num_bins_) * smoothing;
53  total_denominators_ = VectorXf::Ones(num_bins_) * smoothing;
54  multipliers_ = VectorXf::Ones(num_bins_);
55  }
56 
57  void DiscreteFrustum::addExample(double ground_truth, double measurement)
58  {
59  double mult = ground_truth / measurement;
60  if(mult > MAX_MULT || mult < MIN_MULT)
61  return;
62 
63  int idx = min(num_bins_ - 1, (int)floor(measurement / bin_depth_));
64  UASSERT(idx >= 0);
65 
66  total_numerators_(idx) += ground_truth * ground_truth;
67  total_denominators_(idx) += ground_truth * measurement;
68  ++counts_(idx);
70  }
71 
72  inline int DiscreteFrustum::index(double z) const
73  {
74  return min(num_bins_ - 1, (int)floor(z / bin_depth_));
75  }
76 
77  inline void DiscreteFrustum::undistort(double* z) const
78  {
79  *z *= multipliers_.coeffRef(index(*z));
80  }
81 
83  {
84  int idx = index(*z);
85  double start = bin_depth_ * idx;
86  int idx1;
87  if(*z - start < bin_depth_ / 2)
88  idx1 = idx;
89  else
90  idx1 = idx + 1;
91  int idx0 = idx1 - 1;
92  if(idx0 < 0 || idx1 >= num_bins_ || counts_(idx0) < 50 || counts_(idx1) < 50) {
93  undistort(z);
94  return;
95  }
96 
97  double z0 = (idx0 + 1) * bin_depth_ - bin_depth_ * 0.5;
98  double coeff1 = (*z - z0) / bin_depth_;
99  double coeff0 = 1.0 - coeff1;
100  double mult = coeff0 * multipliers_.coeffRef(idx0) + coeff1 * multipliers_.coeffRef(idx1);
101  *z *= mult;
102  }
103 
104  void DiscreteFrustum::serialize(std::ostream& out, bool ascii) const
105  {
106  if(ascii)
107  {
115  }
116  else
117  {
125  }
126  }
127 
128  void DiscreteFrustum::deserialize(std::istream& in, bool ascii)
129  {
130  if(ascii)
131  {
139  }
140  else
141  {
149  }
150  UDEBUG("Frustum: max_dist=%f", max_dist_);
151  UDEBUG("Frustum: num_bins=%d", num_bins_);
152  UDEBUG("Frustum: bin_depth=%f", bin_depth_);
153  UDEBUG("Frustum: counts=%d", counts_.rows());
154  UDEBUG("Frustum: total_numerators=%d", total_numerators_.rows());
155  UDEBUG("Frustum: total_denominators=%d", total_denominators_.rows());
156  UDEBUG("Frustum: multipliers=%d", multipliers_.rows());
157  }
158 
159  std::set<size_t> DiscreteDepthDistortionModel::getDivisors(const size_t &num)
160  {
161  std::set<size_t> divisors;
162  for(size_t i = 1; i <= num; i++)
163  {
164  if(num % i == 0)
165  {
166  divisors.insert(i);
167  }
168  }
169  return divisors;
170  }
171 
172  size_t DiscreteDepthDistortionModel::getClosestToRef(const std::set<size_t> &divisors, const double &ref)
173  {
174  std::set<size_t>::iterator low, prev;
175  low = divisors.lower_bound(ref);
176  if(low == divisors.end())
177  {
178  return *(--divisors.end());
179  }
180  else if(low == divisors.begin())
181  {
182  return *low;
183  }
184  else
185  {
186  prev = low;
187  --prev;
188  if((ref - *prev) <= (*low - ref))
189  {
190  return *prev;
191  }
192  else
193  {
194  return *low;
195  }
196  }
197  }
198 
199  void DiscreteDepthDistortionModel::getBinSize(const size_t &width, const size_t &height, size_t &bin_width, size_t &bin_height) {
200  double ratio = width / static_cast<double>(height);
201  std::set<size_t> divisors = getDivisors(width);
202  double ref_bin_width = 8;
203  bin_width = getClosestToRef(divisors, ref_bin_width);
204  divisors = getDivisors(height);
205  double ref_bin_height = ref_bin_width / ratio;
206  bin_height = getClosestToRef(divisors, ref_bin_height);
207  }
208 
209 
211  {
212  *this = other;
213  }
214 
216  {
217  width_ = other.width_;
218  height_ = other.height_;
219  bin_width_ = other.bin_width_;
220  bin_height_ = other.bin_height_;
221  bin_depth_ = other.bin_depth_;
222  num_bins_x_ = other.num_bins_x_;
223  num_bins_y_ = other.num_bins_y_;
224  training_samples_ = other.training_samples_;
225 
226  frustums_ = other.frustums_;
227  for(size_t i = 0; i < frustums_.size(); ++i)
228  for(size_t j = 0; j < frustums_[i].size(); ++j)
229  frustums_[i][j] = new DiscreteFrustum(*other.frustums_[i][j]);
230 
231  return *this;
232  }
233 
235  int bin_width, int bin_height,
236  double bin_depth,
237  int smoothing,
238  double max_depth) :
239  width_(width),
240  height_(height),
241  bin_width_(bin_width),
242  bin_height_(bin_height),
243  bin_depth_(bin_depth)
244  {
245  UASSERT(width_ % bin_width_ == 0);
246  UASSERT(height_ % bin_height_ == 0);
247 
250 
251  frustums_.resize(num_bins_y_);
252  for(size_t i = 0; i < frustums_.size(); ++i) {
253  frustums_[i].resize(num_bins_x_, NULL);
254  for(size_t j = 0; j < frustums_[i].size(); ++j)
255  frustums_[i][j] = new DiscreteFrustum(smoothing, bin_depth, max_depth);
256  }
257 
258  training_samples_ = 0;
259  }
260 
262  {
263  UDEBUG("");
264  for(size_t y = 0; y < frustums_.size(); ++y)
265  for(size_t x = 0; x < frustums_[y].size(); ++x)
266  if(frustums_[y][x])
267  delete frustums_[y][x];
268  training_samples_ = 0;
269  }
270 
272  {
273  deleteFrustums();
274  }
275 
276  void DiscreteDepthDistortionModel::undistort(cv::Mat & depth) const
277  {
278  UASSERT(width_ == depth.cols);
279  UASSERT(height_ ==depth.rows);
280  UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
281  if(depth.type() == CV_32FC1)
282  {
283  #pragma omp parallel for
284  for(int v = 0; v < height_; ++v) {
285  for(int u = 0; u < width_; ++u) {
286  float & z = depth.at<float>(v, u);
287  if(uIsNan(z) || z == 0.0f)
288  continue;
289  double zf = z;
290  frustum(v, u).interpolatedUndistort(&zf);
291  z = zf;
292  }
293  }
294  }
295  else
296  {
297  #pragma omp parallel for
298  for(int v = 0; v < height_; ++v) {
299  for(int u = 0; u < width_; ++u) {
300  unsigned short & z = depth.at<unsigned short>(v, u);
301  if(uIsNan(z) || z == 0)
302  continue;
303  double zf = z * 0.001;
304  frustum(v, u).interpolatedUndistort(&zf);
305  z = zf*1000;
306  }
307  }
308  }
309  }
310 
311  void DiscreteDepthDistortionModel::addExample(int v, int u, double ground_truth, double measurement)
312  {
313  frustum(v, u).addExample(ground_truth, measurement);
314  }
315 
316  size_t DiscreteDepthDistortionModel::accumulate(const cv::Mat& ground_truth,
317  const cv::Mat& measurement)
318  {
319  UASSERT(width_ == ground_truth.cols);
320  UASSERT(height_ == ground_truth.rows);
321  UASSERT(width_ == measurement.cols);
322  UASSERT(height_ == measurement.rows);
323  UASSERT(ground_truth.type() == CV_16UC1 || ground_truth.type() == CV_32FC1);
324  UASSERT(measurement.type() == CV_16UC1 || measurement.type() == CV_32FC1);
325 
326  bool isGroundTruthInMM = ground_truth.type()==CV_16UC1;
327  bool isMeasurementInMM = measurement.type()==CV_16UC1;
328 
329  size_t num_training_examples = 0;
330  for(int v = 0; v < height_; ++v) {
331  for(int u = 0; u < width_; ++u) {
332  float gt = isGroundTruthInMM?float(ground_truth.at<unsigned short>(v,u))*0.001:ground_truth.at<float>(v,u);
333  if(gt == 0)
334  continue;
335  float meas = isMeasurementInMM?float(measurement.at<unsigned short>(v,u))*0.001:measurement.at<float>(v,u);
336  if(meas == 0)
337  continue;
338 
339  UScopeMutex sm(mutex_);
340  frustum(v, u).addExample(gt, meas);
341  ++num_training_examples;
342  }
343  }
344 
345  training_samples_ += num_training_examples;
346 
347  return num_training_examples;
348  }
349 
350  void DiscreteDepthDistortionModel::load(const std::string& path)
351  {
352  bool ascii = UFile::getExtension(path).compare("txt") == 0;
353  ifstream f;
354  f.open(path.c_str(), ascii ? ios::in : ios::in | ios::binary);
355  if(!f.is_open()) {
356  cerr << "Failed to open " << path << endl;
357  assert(f.is_open());
358  }
359  deserialize(f, ascii);
360  f.close();
361  }
362 
363  void DiscreteDepthDistortionModel::save(const std::string& path) const
364  {
365  bool ascii = UFile::getExtension(path).compare("txt") == 0;
366  ofstream f;
367  f.open(path.c_str(), ascii?ios::out: ios::out | ios::binary);
368  if(!f.is_open()) {
369  cerr << "Failed to open " << path << endl;
370  assert(f.is_open());
371  }
372  serialize(f, ascii);
373  f.close();
374  }
375 
376  void DiscreteDepthDistortionModel::serialize(std::ostream& out, bool ascii) const
377  {
378  out << "DiscreteDepthDistortionModel v01" << endl;
379  if(ascii)
380  {
389  }
390  else
391  {
400  }
401 
402 
403  for(int y = 0; y < num_bins_y_; ++y)
404  for(int x = 0; x < num_bins_x_; ++x)
405  frustums_[y][x]->serialize(out, ascii);
406  }
407 
408  void DiscreteDepthDistortionModel::deserialize(std::istream& in, bool ascii)
409  {
410  UDEBUG("");
411  string buf;
412  getline(in, buf);
413  UDEBUG("buf=%s", buf.c_str());
414  assert(buf == "DiscreteDepthDistortionModel v01");
415  if(ascii)
416  {
425  }
426  else
427  {
436  }
437  UINFO("Distortion Model: width=%d", width_);
438  UINFO("Distortion Model: height=%d", height_);
439  UINFO("Distortion Model: bin_width=%d", bin_width_);
440  UINFO("Distortion Model: bin_height=%d", bin_height_);
441  UINFO("Distortion Model: bin_depth=%f", bin_depth_);
442  UINFO("Distortion Model: num_bins_x=%d", num_bins_x_);
443  UINFO("Distortion Model: num_bins_y=%d", num_bins_y_);
444  UINFO("Distortion Model: training_samples=%d", training_samples_);
445  deleteFrustums();
446  frustums_.resize(num_bins_y_);
447  for(size_t y = 0; y < frustums_.size(); ++y) {
448  frustums_[y].resize(num_bins_x_, NULL);
449  for(size_t x = 0; x < frustums_[y].size(); ++x) {
450  UDEBUG("Distortion Model: Frustum[%d][%d]", y, x);
451  frustums_[y][x] = new DiscreteFrustum;
452  frustums_[y][x]->deserialize(in, ascii);
453  }
454  }
455  UDEBUG("");
456  }
457 
459  {
460  UASSERT(x >= 0 && x < width_);
461  UASSERT(y >= 0 && y < height_);
462  int xidx = x / bin_width_;
463  int yidx = y / bin_height_;
464  return (*frustums_[yidx][xidx]);
465  }
466 
468  {
469  UASSERT(x >= 0 && x < width_);
470  UASSERT(y >= 0 && y < height_);
471  int xidx = x / bin_width_;
472  int yidx = y / bin_height_;
473  return (*frustums_[yidx][xidx]);
474  }
475 
476 } // namespace clams
#define NULL
void serializeScalarASCII(T val, std::ostream &strm)
void deserializeASCII(std::istream &strm, Eigen::Matrix< S, T, U > *mat)
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
void serialize(const Eigen::Matrix< S, T, U > &mat, std::ostream &strm)
f
static size_t getClosestToRef(const std::set< size_t > &divisors, const double &ref)
Basic mathematics functions.
void deserialize(std::istream &in, bool ascii)
#define MAX_MULT
std::string getExtension()
Definition: UFile.h:140
std::vector< std::vector< DiscreteFrustum * > > frustums_
frustums_[y][x]
void deserialize(std::istream &in, bool ascii)
#define UASSERT(condition)
static void getBinSize(const size_t &width, const size_t &height, size_t &bin_width, size_t &bin_height)
GLM_FUNC_DECL genType floor(genType const &x)
void interpolatedUndistort(double *z) const
void serializeScalar(T val, std::ostream &strm)
void addExample(int v, int u, double ground_truth, double measurement)
#define MIN_MULT
DiscreteFrustum(int smoothing=1, double bin_depth=1.0, double max_dist=10.0)
void deserializeScalarASCII(std::istream &strm, T *val)
void serializeASCII(const Eigen::Matrix< S, T, U > &mat, std::ostream &strm)
void serialize(std::ostream &out, bool ascii) const
size_t accumulate(const cv::Mat &ground_truth, const cv::Mat &measurement)
#define UDEBUG(...)
int bin_height_
Height of each bin in pixels.
DiscreteDepthDistortionModel & operator=(const DiscreteDepthDistortionModel &other)
ULogger class and convenient macros.
bool uIsNan(const T &value)
Definition: UMath.h:42
void deserializeScalar(std::istream &strm, T *val)
static std::set< size_t > getDivisors(const size_t &num)
GLM_FUNC_DECL genType ceil(genType const &x)
double bin_depth_
Depth of each bin in meters.
void deserialize(std::istream &strm, Eigen::Matrix< S, T, U > *mat)
void addExample(double ground_truth, double measurement)
void serialize(std::ostream &out, bool ascii) const
#define UINFO(...)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58