34 #include <opencv2/highgui/highgui.hpp> 35 #include <opencv2/imgproc/imgproc.hpp> 36 #include <pcl/common/point_tests.h> 39 using namespace Eigen;
55 if((
int)ind.size() != height)
57 for(
size_t y = 0; y < ind.size(); ++y)
58 if((
int)ind[y].size() != width)
60 for(
size_t y = 0; y < ind.size(); ++y) {
61 for(
size_t x = 0; x < ind[y].size(); ++x) {
63 ind[y][x].reserve(10);
70 for(
size_t i = 0; i < pcd->size(); ++i) {
74 if(ppt.
z_ == 0 || !(ppt.
u_ >= 0 && ppt.
v_ >= 0 && ppt.
u_ < width && ppt.
v_ < height))
76 ind[ppt.
v_][ppt.
u_].push_back(ppt.
z_);
107 const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
109 const cv::Mat& measurement,
111 double coneStdevThresh)
const 113 cv::Mat estimate = cv::Mat::zeros(measurement.size(), CV_32FC1);
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);
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)
130 cv::dilate(mask, mask, cv::Mat(), cv::Point(-1, -1), 4);
131 cv::erode(mask, mask, cv::Mat(), cv::Point(-1, -1), 15);
133 bool isInMM = measurement_depth.type() == CV_16UC1;
137 for(ppt.
v_ = 0; ppt.
v_ < measurement_depth.rows; ++ppt.
v_) {
138 for(ppt.
u_ = 0; ppt.
u_ < measurement_depth.cols; ++ppt.
u_) {
140 float value = isInMM?float(measurement_depth.at<
unsigned short>(ppt.
v_, ppt.
u_))*0.001f:measurement_depth.at<
float>(ppt.
v_, ppt.
u_);
145 if(naive_mapdepth.at<
float>(ppt.
v_, ppt.
u_) == 0)
157 naive_mapdepth.size(),
167 if(stdev > coneStdevThresh)
170 estimate.at<
float>(ppt.
v_, ppt.
u_) = mean;
177 int uc,
int vc,
double radius,
double measurement_depth,
178 double* mean,
double*
stdev)
const 180 pcl::PointXYZ pt_center, pt_ul, pt_lr;
184 ppt.
z_ = measurement_depth;
201 if(ppt_ul.
z_ == 0 || !(ppt_ul.
u_ >= 0 && ppt_ul.
v_ >= 0 && ppt_ul.
u_ < imageSize.width && ppt_ul.
v_ < imageSize.height))
203 if(ppt_lr.
z_ == 0 || !(ppt_lr.
u_ >= 0 && ppt_lr.
v_ >= 0 && ppt_lr.
u_ < imageSize.width && ppt_lr.
v_ < imageSize.height))
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_;
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;
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;
236 var += (vals[i] - *mean) * (vals[i] - *mean);
GLM_FUNC_DECL genIType mask(genIType const &count)
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
pcl::PointXYZ project(const ProjectivePoint &ppt) const
const cv::Size & imageSize() const
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)
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
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)
ProjectivePoint reproject(const pcl::PointXYZ &pt) const
const Transform & localTransform() const