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  {
46  UASSERT(model.isValidForReprojection());
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();
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 
ind
std::vector< int > ind
clams::ProjectivePoint::u_
int u_
Definition: frame_projector.h:55
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:120
Eigen
glm::mask
GLM_FUNC_DECL genIType mask(genIType const &count)
MIN_MULT
#define MIN_MULT
Definition: frame_projector.h:39
size
Index size
rtabmap::CameraModel
Definition: CameraModel.h:38
clams::FrameProjector::model_
rtabmap::CameraModel model_
Definition: frame_projector.h:90
clams::FrameProjector::cloudToRangeIndex
RangeIndex cloudToRangeIndex(const pcl::PointCloud< pcl::PointXYZ >::Ptr &pcd) const
Definition: frame_projector.cpp:50
y
Matrix3f y
clams::FrameProjector::coneFit
bool coneFit(const cv::Size &imageSize, const RangeIndex &rindex, int uc, int vc, double radius, double measurement_depth, double *mean, double *stdev) const
Definition: frame_projector.cpp:176
rtabmap::CameraModel::K
cv::Mat K() const
Definition: CameraModel.h:110
isFinite
const EIGEN_DEVICE_FUNC IsFiniteReturnType isFinite() const
util3d.h
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
clams::FrameProjector::RangeIndex
std::vector< std::vector< std::vector< double > > > RangeIndex
Definition: frame_projector.h:66
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::CameraModel::project
void project(float u, float v, float depth, float &x, float &y, float &z) const
Definition: CameraModel.cpp:766
util3d_transforms.h
clams::ProjectivePoint::z_
float z_
Definition: frame_projector.h:57
rtabmap::util3d::projectCloudToCamera
cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera(const cv::Size &imageSize, const cv::Mat &cameraMatrixK, const cv::Mat &laserScan, const rtabmap::Transform &cameraTransform)
Definition: util3d.cpp:2693
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
clams::ProjectivePoint::v_
int v_
Definition: frame_projector.h:56
UASSERT
#define UASSERT(condition)
x
x
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
clams
Definition: discrete_depth_distortion_model.h:41
eigen_extensions::stdev
double stdev(const Eigen::VectorXd &vec)
Definition: eigen_extensions.h:22
ULogger.h
ULogger class and convenient macros.
clams::FrameProjector::estimateMapDepth
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
Definition: frame_projector.cpp:106
rtabmap::Transform
Definition: Transform.h:41
mean
Point3 mean(const CONTAINER &points)
std
clams::FrameProjector::project
pcl::PointXYZ project(const ProjectivePoint &ppt) const
Definition: frame_projector.cpp:81
MAX_MULT
#define MAX_MULT
Definition: frame_projector.h:38
clams::FrameProjector::reproject
ProjectivePoint reproject(const pcl::PointXYZ &pt) const
Definition: frame_projector.cpp:91
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:119
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:121
float
float
transform
EIGEN_DONT_INLINE void transform(const Quaternion< Scalar > &t, Data &data)
t
Point2 t(10, 10)
clams::ProjectivePoint
Definition: frame_projector.h:47
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
trace.model
model
Definition: trace.py:4
rtabmap::CameraModel::reproject
void reproject(float x, float y, float z, float &u, float &v) const
Definition: CameraModel.cpp:781
value
value
i
int i
frame_projector.h


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:10