discrete_depth_distortion_model.h
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 
30 #ifndef DISCRETE_DEPTH_DISTORTION_MODEL_H
31 #define DISCRETE_DEPTH_DISTORTION_MODEL_H
32 
33 #include <assert.h>
34 #include <vector>
35 #include <set>
36 #include <Eigen/Core>
37 #include <opencv2/opencv.hpp>
38 #include <rtabmap/utilite/UMutex.h>
39 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
40 
41 namespace clams
42 {
44  {
45  public:
46  DiscreteFrustum(int smoothing = 1, double bin_depth = 1.0, double max_dist = 10.0);
49  void addExample(double ground_truth, double measurement);
50  int index(double z) const;
51  void undistort(double* z) const;
52  void interpolatedUndistort(double* z) const;
53  void serialize(std::ostream& out, bool ascii) const;
54  void deserialize(std::istream& in, bool ascii);
55 
56  protected:
57  double max_dist_;
58  int num_bins_;
59  double bin_depth_;
60  Eigen::VectorXf counts_;
61  Eigen::VectorXf total_numerators_;
62  Eigen::VectorXf total_denominators_;
63  Eigen::VectorXf multipliers_;
64 
66  };
67 
69  {
70  public:
71  // returns all divisors of num
72  static std::set<size_t> getDivisors(const size_t &num);
73 
74  // returns divisor from divisors closest to ref
75  static size_t getClosestToRef(const std::set<size_t> &divisors, const double &ref);
76 
77  // sets bin_width and bin_height to appropriate values
78  static void getBinSize(const size_t &width, const size_t &height, size_t &bin_width, size_t &bin_height);
79 
80  public:
82  width_(0),
83  height_(0),
84  bin_width_(0),
85  bin_height_(0),
86  bin_depth_(0),
87  num_bins_x_(0),
88  num_bins_y_(0),
89  training_samples_(0)
90  {}
92  DiscreteDepthDistortionModel(int width, int height, int bin_width = 8, int bin_height = 6, double bin_depth = 2.0, int smoothing = 1, double max_depth = 10.0);
95  void undistort(cv::Mat & depth) const;
98  size_t accumulate(const cv::Mat& ground_truth, const cv::Mat& measurement);
99  void addExample(int v, int u, double ground_truth, double measurement);
100  void save(const std::string& path) const;
101  void load(const std::string& path);
102  void serialize(std::ostream& out, bool ascii) const;
103  void deserialize(std::istream& in, bool ascii);
104  cv::Mat visualize(const std::string& path = "") const;
105 
106  int getWidth() const {return width_;}
107  int getHeight() const {return height_;}
108  size_t getTrainingSamples() const {return training_samples_;}
109  bool isValid() const
110  {
111  return !frustums_.empty();
112  }
113 
114  protected:
116  int width_;
118  int height_;
124  double bin_depth_;
128  std::vector< std::vector<DiscreteFrustum*> > frustums_;
129 
131 
132  void deleteFrustums();
133  DiscreteFrustum& frustum(int y, int x);
134  const DiscreteFrustum& frustum(int y, int x) const;
135 
137  };
138 
139 } // namespace clams
140 
141 #endif // DISCRETE_DEPTH_DISTORTION_MODEL_H
void serialize(const Eigen::Matrix< S, T, U > &mat, std::ostream &strm)
std::vector< std::vector< DiscreteFrustum * > > frustums_
frustums_[y][x]
#define RTABMAP_EXP
Definition: RtabmapExp.h:38
ref
Definition: UMutex.h:54
int bin_height_
Height of each bin in pixels.
NearestNeighbourSearch< T >::Matrix load(const char *fileName)
double bin_depth_
Depth of each bin in meters.
void deserialize(std::istream &strm, Eigen::Matrix< S, T, U > *mat)
GLM_FUNC_DECL detail::tmat4x4< T, defaultp > frustum(T const &left, T const &right, T const &bottom, T const &top, T const &near, T const &far)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:28