frame_projector.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/core/util3d.h>
34 #include <opencv2/highgui/highgui.hpp>
35 #include <opencv2/imgproc/imgproc.hpp>
36 #include <pcl/common/point_tests.h>
37 
38 using namespace std;
39 using namespace Eigen;
40 
41 namespace clams
42 {
43  FrameProjector::FrameProjector(const rtabmap::CameraModel & model) :
44  model_(model)
45  {
47  }
48 
49  // pcd is in /map frame
50  FrameProjector::RangeIndex FrameProjector::cloudToRangeIndex(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcd) const
51  {
52  int height = model_.imageHeight();
53  int width = model_.imageWidth();
54  RangeIndex ind;
55  if((int)ind.size() != height)
56  ind.resize(height);
57  for(size_t y = 0; y < ind.size(); ++y)
58  if((int)ind[y].size() != width)
59  ind[y].resize(width);
60  for(size_t y = 0; y < ind.size(); ++y) {
61  for(size_t x = 0; x < ind[y].size(); ++x) {
62  ind[y][x].clear();
63  ind[y][x].reserve(10);
64  }
65  }
66 
68 
69  ProjectivePoint ppt;
70  for(size_t i = 0; i < pcd->size(); ++i) {
71  if(!isFinite(pcd->at(i)))
72  continue;
73  ppt = reproject(rtabmap::util3d::transformPoint(pcd->at(i), t));
74  if(ppt.z_ == 0 || !(ppt.u_ >= 0 && ppt.v_ >= 0 && ppt.u_ < width && ppt.v_ < height))
75  continue;
76  ind[ppt.v_][ppt.u_].push_back(ppt.z_);
77  }
78  return ind;
79  }
80 
81  pcl::PointXYZ FrameProjector::project(const ProjectivePoint& ppt) const
82  {
83  UASSERT(ppt.u_ >= 0 && ppt.v_ >= 0 && ppt.u_ < model_.imageWidth() && ppt.v_ < model_.imageHeight());
84 
85  pcl::PointXYZ pt;
86 
87  model_.project(ppt.u_, ppt.v_, ppt.z_, pt.x, pt.y, pt.z);
88  return pt;
89  }
90 
91  ProjectivePoint FrameProjector::reproject(const pcl::PointXYZ & pt) const
92  {
93  UASSERT(isFinite(pt));
94 
95  ProjectivePoint ppt;
96 
97  if(pt.z > 0)
98  {
99  model_.reproject(pt.x, pt.y, pt.z, ppt.u_, ppt.v_);
100  ppt.z_ = pt.z;
101  }
102 
103  return ppt;
104  }
105 
107  const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
108  const rtabmap::Transform & transform,
109  const cv::Mat& measurement,
110  double coneRadius,
111  double coneStdevThresh) const
112  {
113  cv::Mat estimate = cv::Mat::zeros(measurement.size(), CV_32FC1);
114 
115  // -- Get the depth index.
116  pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = rtabmap::util3d::transformPointCloud(map, transform);
117  RangeIndex rindex = cloudToRangeIndex(transformed);
118 
119  // -- Compute the edge-of-map mask.
120  cv::Mat naive_mapframe = rtabmap::util3d::projectCloudToCamera(model_.imageSize(), model_.K(), transformed, model_.localTransform());
121  const cv::Mat& measurement_depth = measurement;
122  const cv::Mat& naive_mapdepth = naive_mapframe;
123  cv::Mat1b mask(measurement_depth.rows, measurement_depth.cols);
124  mask = 0;
125  for(int y = 0; y < mask.rows; ++y)
126  for(int x = 0; x < mask.cols; ++x)
127  if(naive_mapdepth.at<float>(y, x) != 0)
128  mask(y, x) = 255;
129 
130  cv::dilate(mask, mask, cv::Mat(), cv::Point(-1, -1), 4);
131  cv::erode(mask, mask, cv::Mat(), cv::Point(-1, -1), 15);
132 
133  bool isInMM = measurement_depth.type() == CV_16UC1;
134 
135  // -- Main loop: for all points in the image...
136  ProjectivePoint ppt;
137  for(ppt.v_ = 0; ppt.v_ < measurement_depth.rows; ++ppt.v_) {
138  for(ppt.u_ = 0; ppt.u_ < measurement_depth.cols; ++ppt.u_) {
139 
140  float value = isInMM?float(measurement_depth.at<unsigned short>(ppt.v_, ppt.u_))*0.001f:measurement_depth.at<float>(ppt.v_, ppt.u_);
141 
142  // -- Reject points with no data.
143  if(value == 0)
144  continue;
145  if(naive_mapdepth.at<float>(ppt.v_, ppt.u_) == 0)
146  continue;
147 
148  // -- Reject points on the edge of the map.
149  if(mask(ppt.v_, ppt.u_) == 0)
150  continue;
151 
152  // -- Find nearby points in the cone to get a good estimate of the map depth.
153  double mean = 0;
154  double stdev = 0;
155  //double stdev_thresh = numeric_limits<double>::max();
156  bool valid = coneFit(
157  naive_mapdepth.size(),
158  rindex,
159  ppt.u_,
160  ppt.v_,
161  coneRadius,
162  value,
163  &mean,
164  &stdev);
165  if(!valid)
166  continue;
167  if(stdev > coneStdevThresh)
168  continue;
169 
170  estimate.at<float>(ppt.v_, ppt.u_) = mean;
171  }
172  }
173  return estimate;
174  }
175 
176  bool FrameProjector::coneFit(const cv::Size& imageSize, const RangeIndex& rindex,
177  int uc, int vc, double radius, double measurement_depth,
178  double* mean, double* stdev) const
179  {
180  pcl::PointXYZ pt_center, pt_ul, pt_lr;
181  ProjectivePoint ppt, ppt_ul, ppt_lr;
182  ppt.u_ = uc;
183  ppt.v_ = vc;
184  ppt.z_ = measurement_depth;
185  pt_center = project(ppt);
186 
187  pt_ul = pt_center;
188  pt_lr = pt_center;
189  pt_ul.x -= radius;
190  pt_ul.y -= radius;
191  pt_lr.x += radius;
192  pt_lr.y += radius;
193 
194  if(!pcl::isFinite(pt_ul) || !pcl::isFinite(pt_lr))
195  {
196  return false;
197  }
198 
199  ppt_ul = reproject(pt_ul);
200  ppt_lr = reproject(pt_lr);
201  if(ppt_ul.z_ == 0 || !(ppt_ul.u_ >= 0 && ppt_ul.v_ >= 0 && ppt_ul.u_ < imageSize.width && ppt_ul.v_ < imageSize.height))
202  return false;
203  if(ppt_lr.z_ == 0 || !(ppt_lr.u_ >= 0 && ppt_lr.v_ >= 0 && ppt_lr.u_ < imageSize.width && ppt_lr.v_ < imageSize.height))
204  return false;
205 
206  int min_u = ppt_ul.u_;
207  int max_u = ppt_lr.u_;
208  int min_v = ppt_ul.v_;
209  int max_v = ppt_lr.v_;
210 
211  *mean = 0;
212  double num = 0;
213  for(ppt.u_ = min_u; ppt.u_ <= max_u; ++ppt.u_) {
214  for(ppt.v_ = min_v; ppt.v_ <= max_v; ++ppt.v_) {
215  const vector<double>& vals = rindex[ppt.v_][ppt.u_];
216  for(size_t i = 0; i < vals.size(); ++i) {
217  double mult = vals[i] / measurement_depth;
218  if(mult > MIN_MULT && mult < MAX_MULT) {
219  *mean += vals[i];
220  ++num;
221  }
222  }
223  }
224  }
225  if(num == 0)
226  return false;
227  *mean /= num;
228 
229  double var = 0;
230  for(ppt.u_ = min_u; ppt.u_ <= max_u; ++ppt.u_) {
231  for(ppt.v_ = min_v; ppt.v_ <= max_v; ++ppt.v_) {
232  const vector<double>& vals = rindex[ppt.v_][ppt.u_];
233  for(size_t i = 0; i < vals.size(); ++i) {
234  double mult = vals[i] / measurement_depth;
235  if(mult > MIN_MULT && mult < MAX_MULT)
236  var += (vals[i] - *mean) * (vals[i] - *mean);
237  }
238  }
239  }
240  var /= num;
241 
242  *stdev = sqrt(var);
243  return true;
244  }
245 
246 } // namespace clams
247 
GLM_FUNC_DECL genIType mask(genIType const &count)
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3108
int imageWidth() const
Definition: CameraModel.h:120
pcl::PointXYZ project(const ProjectivePoint &ppt) const
const cv::Size & imageSize() const
Definition: CameraModel.h:119
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
double stdev(const Eigen::VectorXd &vec)
#define MAX_MULT
cv::Mat estimateMapDepth(const pcl::PointCloud< pcl::PointXYZ >::Ptr &map, const rtabmap::Transform &transform, const cv::Mat &measurement, double coneRadius=0.02, double coneStdevThresh=0.03) const
#define UASSERT(condition)
rtabmap::CameraModel model_
void reproject(float x, float y, float z, float &u, float &v) const
bool coneFit(const cv::Size &imageSize, const RangeIndex &rindex, int uc, int vc, double radius, double measurement_depth, double *mean, double *stdev) const
bool isValidForReprojection() const
Definition: CameraModel.h:88
#define MIN_MULT
void project(float u, float v, float depth, float &x, float &y, float &z) const
std::vector< std::vector< std::vector< double > > > RangeIndex
ULogger class and convenient macros.
RangeIndex cloudToRangeIndex(const pcl::PointCloud< pcl::PointXYZ >::Ptr &pcd) const
cv::Mat RTABMAP_EXP projectCloudToCamera(const cv::Size &imageSize, const cv::Mat &cameraMatrixK, const cv::Mat &laserScan, const rtabmap::Transform &cameraTransform)
Definition: util3d.cpp:2745
Transform inverse() const
Definition: Transform.cpp:178
cv::Mat K() const
Definition: CameraModel.h:110
ProjectivePoint reproject(const pcl::PointXYZ &pt) const
int imageHeight() const
Definition: CameraModel.h:121
const Transform & localTransform() const
Definition: CameraModel.h:116


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