heightmap_time_accumulation_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #define BOOST_PARAMETER_MAX_ARITY 7
37 
40 #include <cv_bridge/cv_bridge.h>
42 #include <pcl/common/transforms.h>
43 
44 namespace jsk_pcl_ros
45 {
47  {
48  ConnectionBasedNodelet::onInit();
49  pub_config_ = pnh_->advertise<jsk_recognition_msgs::HeightmapConfig>(
50  "output/config", 1, true);
51  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
52  typename dynamic_reconfigure::Server<Config>::CallbackType f =
53  boost::bind (&HeightmapTimeAccumulation::configCallback, this, _1, _2);
54  srv_->setCallback (f);
55  sub_config_ = pnh_->subscribe(
56  getHeightmapConfigTopic(pnh_->resolveName("input")), 1,
58  if (!pnh_->getParam("center_frame_id", center_frame_id_)) {
59  NODELET_FATAL("no ~center_frame_id is specified");
60  return;
61  }
62  if (!pnh_->getParam("fixed_frame_id", fixed_frame_id_)) {
63  NODELET_FATAL("no ~fixed_frame_id is specified");
64  return;
65  }
66  int tf_queue_size;
67  pnh_->param("tf_queue_size", tf_queue_size, 10);
68  prev_from_center_to_fixed_ = Eigen::Affine3f::Identity();
70  pub_output_ = pnh_->advertise<sensor_msgs::Image>("output", 1, true);
71  sub_previous_pointcloud_ = pnh_->subscribe<sensor_msgs::PointCloud2>(
72  "input/prev_pointcloud", 5,
74  sub_heightmap_.subscribe(*pnh_, "input", 10);
77  *tf_,
79  tf_queue_size));
80  tf_filter_->registerCallback(
81  boost::bind(
83  srv_reset_ = pnh_->advertiseService("reset", &HeightmapTimeAccumulation::resetCallback, this);
85  }
86 
88  {
89 
90  }
92  {
93 
94  }
95 
96 
98  const cv::Mat& heightmap, const std_msgs::Header& header)
99  {
101  header,
103  heightmap).toImageMsg());
104  }
105 
107  const PointType& p, const cv::Mat& map)
108  {
109  if (p.x > max_x_ || p.x < min_x_ ||
110  p.y > max_y_ || p.y < min_y_) {
111  return cv::Point(-1, -1);
112  }
113  const float offsetted_x = p.x - min_x_;
114  const float offsetted_y = p.y - min_y_;
115  const float dx = (max_x_ - min_x_) / map.cols;
116  const float dy = (max_y_ - min_y_) / map.rows;
117  return cv::Point(std::floor(offsetted_x / dx),
118  std::floor(offsetted_y / dy));
119  }
120 
121  bool HeightmapTimeAccumulation::isValidIndex(const cv::Point& index, const cv::Mat& map)
122  {
123  return (index.x >= 0 && index.x < map.cols &&
124  index.y >= 0 && index.y < map.rows);
125  }
126 
127  bool HeightmapTimeAccumulation::isValidCell(const cv::Point& index, const cv::Mat& map)
128  {
129  float v = map.at<cv::Vec2f>(index.y, index.x)[0];
130  return !std::isnan(v) && v != -FLT_MAX;
131  }
132 
134  const sensor_msgs::Image::ConstPtr& msg)
135  {
136  boost::mutex::scoped_lock lock(mutex_);
137  if (!config_) {
138  NODELET_ERROR("no ~input/config is yet available");
139  return;
140  }
141  tf::StampedTransform tf_transform;
142  try {
144  msg->header.stamp,
145  tf_transform);
146  }
147  catch (tf2::TransformException &e) {
148  NODELET_ERROR("Transform error: %s", e.what());
149  return;
150  }
151  Eigen::Affine3f from_center_to_fixed;
152  tf::transformTFToEigen(tf_transform, from_center_to_fixed);
153  cv::Mat new_heightmap = cv_bridge::toCvShare(
155  // Transform prev_cloud_ to current frame
156  Eigen::Affine3f from_prev_to_current
157  = prev_from_center_to_fixed_.inverse() * from_center_to_fixed;
158  pcl::PointCloud<PointType > transformed_pointcloud;
159  pcl::transformPointCloud(prev_cloud_, transformed_pointcloud, from_prev_to_current.inverse());
160 
161  mergedAccmulation(transformed_pointcloud, new_heightmap);
162 
163  publishHeightmap(new_heightmap, msg->header);
164  // prev_from_center_to_fixed_ = from_center_to_fixed;
165  }
166 
168  pcl::PointCloud<PointType > &transformed_pointcloud, cv::Mat &new_heightmap)
169  {
170  for (size_t i = 0; i < transformed_pointcloud.points.size(); i++) {
171  PointType p = transformed_pointcloud.points[i];
172  if (isValidPoint(p)) {
173  cv::Point index = toIndex(p, new_heightmap);
174  if (isValidIndex(index, new_heightmap)) {
175  if (!isValidCell(index, new_heightmap)) {
176  // There is not valid data in current heightmap,
177  new_heightmap.at<cv::Vec2f>(index.y, index.x)[0] = p.z;
178  new_heightmap.at<cv::Vec2f>(index.y, index.x)[1] = (float)(p.intensity);
179  } else {
180  // There is valid data in current heightmap,
181  if (new_heightmap.at<cv::Vec2f>(index.y, index.x)[1] < (float)(p.intensity)) {
182  // heightmap has worth quality than prev_pointcloud
183  new_heightmap.at<cv::Vec2f>(index.y, index.x)[0] = p.z;
184  new_heightmap.at<cv::Vec2f>(index.y, index.x)[1] = (float)(p.intensity);
185  }
186  }
187  }
188  }
189  }
190  }
191 
193  pcl::PointCloud<PointType > &transformed_pointcloud, cv::Mat &new_heightmap)
194  {
195  float offset_new_z = 0.0;
196  float offset_org_z = 0.0;
197  if (use_offset_) { // calc offset
198  double diff_z = 0.0;
199  long cntr = 0;
200  for (size_t i = 0; i < transformed_pointcloud.points.size(); i++) {
201  PointType p = transformed_pointcloud.points[i];
202  if (isValidPoint(p)) {
203  cv::Point index = toIndex(p, new_heightmap);
204  if (isValidIndex(index, new_heightmap)) {
205  if (isValidCell(index, new_heightmap)) {
206  float new_z = new_heightmap.at<cv::Vec2f>(index.y, index.x)[0];
207  float new_q = new_heightmap.at<cv::Vec2f>(index.y, index.x)[1];
208  float org_z = p.z;
209  float org_q = p.intensity;
210  float tmp_diff = (new_z - org_z);
211  if (std::fabs(tmp_diff) < 0.04) {
212  diff_z += tmp_diff;
213  cntr++;
214  }
215  }
216  }
217  }
218  }
219  if(cntr > 0) {
220  diff_z /= cntr;
221  offset_new_z = - diff_z/2.0;
222  offset_org_z = diff_z/2.0;
223  }
224  // add offset to new heightmap
225  for(int yy = 0; yy < new_heightmap.rows; yy++) {
226  for(int xx = 0; xx < new_heightmap.cols; xx++) {
227  new_heightmap.at<cv::Vec2f>(yy, xx)[0] += offset_new_z;
228  }
229  }
230  }
231 
232  //averaging strategy
233  for (size_t i = 0; i < transformed_pointcloud.points.size(); i++) {
234  PointType p = transformed_pointcloud.points[i];
235  if (isValidPoint(p)) {
236  cv::Point index = toIndex(p, new_heightmap);
237  if (isValidIndex(index, new_heightmap)) {
238  p.z += offset_org_z;
239  if (!isValidCell(index, new_heightmap)) {
240  // There is not valid data in current heightmap,
241  new_heightmap.at<cv::Vec2f>(index.y, index.x)[0] = p.z;
242  new_heightmap.at<cv::Vec2f>(index.y, index.x)[1] = (float)(p.intensity);
243  } else {
244  // There is valid data in current heightmap,
245  float new_z = new_heightmap.at<cv::Vec2f>(index.y, index.x)[0];
246  float new_q = new_heightmap.at<cv::Vec2f>(index.y, index.x)[1];
247  float org_z = p.z;
248  float org_q = p.intensity;
249  new_z = (new_z * new_q + org_z * org_q)/(new_q + org_q);
250  new_q = (new_q + org_q)/1.6; // ?? new quality
251  if(new_q > 1.0) new_q = 1.0;
252  new_heightmap.at<cv::Vec2f>(index.y, index.x)[0] = new_z;
253  new_heightmap.at<cv::Vec2f>(index.y, index.x)[1] = new_q;
254  }
255  }
256  }
257  }
258 
259  if (use_bilateral_) {
260  std::vector<cv::Mat> fimages;
261  cv::split(new_heightmap, fimages);
262  cv::Mat res_image;
263  // filter
264  cv::bilateralFilter(fimages[0], res_image, bilateral_filter_size_, bilateral_sigma_color_, bilateral_sigma_space_);
265  {
266  for (size_t j = 0; j < res_image.rows; j++) {
267  for (size_t i = 0; i < res_image.cols; i++) {
268  float ov = fimages[0].at<float>(j, i);
269  if (ov == -FLT_MAX) {
270  res_image.at<float>(j, i) = -FLT_MAX;
271  }
272  }
273  }
274  }
275  // reconstruct images
276  std::vector<cv::Mat> ret_images;
277  ret_images.push_back(res_image);
278  ret_images.push_back(fimages[1]);
279  cv::merge(ret_images, new_heightmap);
280  }
281  }
282 
284  const sensor_msgs::PointCloud2::ConstPtr& msg)
285  {
286  boost::mutex::scoped_lock lock(mutex_);
287 
289  // get transform at subscribed timestamp
290  tf::StampedTransform tf_transform;
291  try {
293  msg->header.stamp,
294  tf_transform);
295  }
296  catch (tf2::TransformException &e) {
297  NODELET_ERROR("Transform error: %s", e.what());
298  return;
299  }
301  }
302 
304  const jsk_recognition_msgs::HeightmapConfig::ConstPtr& msg)
305  {
306  boost::mutex::scoped_lock lock(mutex_);
307  config_ = msg;
308  min_x_ = msg->min_x;
309  max_x_ = msg->max_x;
310  min_y_ = msg->min_y;
311  max_y_ = msg->max_y;
312  pub_config_.publish(msg);
313  }
314 
315  bool HeightmapTimeAccumulation::resetCallback(std_srvs::Empty::Request& req,
316  std_srvs::Empty::Response& res)
317  {
318  boost::mutex::scoped_lock lock(mutex_);
319  prev_from_center_to_fixed_ = Eigen::Affine3f::Identity();
320  prev_cloud_.points.clear();
321  return true;
322  }
323 
324  void HeightmapTimeAccumulation::configCallback(Config& config, uint32_t level)
325  {
326  boost::mutex::scoped_lock lock(mutex_);
327  //min_x_ = config.min_x;
328  //max_x_ = config.max_x;
329  //min_y_ = config.min_y;
330  //max_y_ = config.max_y;
331  use_offset_ = config.use_offset;
332  use_bilateral_ = config.use_bilateral;
333  bilateral_filter_size_ = config.bilateral_filter_size;
334  bilateral_sigma_color_ = config.bilateral_sigma_color;
335  bilateral_sigma_space_ = config.bilateral_sigma_space;
336  }
337 }
338 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
virtual void configTopicCallback(const jsk_recognition_msgs::HeightmapConfig::ConstPtr &config)
virtual bool resetCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
std::string getHeightmapConfigTopic(const std::string &base_topic)
message_filters::Subscriber< sensor_msgs::Image > sub_heightmap_
jsk_recognition_msgs::HeightmapConfig::ConstPtr config_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::HeightmapTimeAccumulation, nodelet::Nodelet)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual bool isValidCell(const cv::Point &index, const cv::Mat &map)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void prevPointCloud(const sensor_msgs::PointCloud2::ConstPtr &msg)
pcl::PointXYZI PointType
float
unsigned int index
virtual bool isValidIndex(const cv::Point &index, const cv::Mat &map)
virtual void configCallback(Config &config, uint32_t level)
virtual void overwriteAccmulation(pcl::PointCloud< PointType > &transformed_pointcloud, cv::Mat &new_heightmap)
virtual void mergedAccmulation(pcl::PointCloud< PointType > &transformed_pointcloud, cv::Mat &new_heightmap)
boost::shared_ptr< ros::NodeHandle > pnh_
virtual cv::Point toIndex(const PointType &p, const cv::Mat &map)
virtual void publishHeightmap(const cv::Mat &heightmap, const std_msgs::Header &header)
virtual void accumulate(const sensor_msgs::Image::ConstPtr &new_heightmap)
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
p
GLfloat v[8][3]
static tf::TransformListener * getInstance()
boost::shared_ptr< tf::MessageFilter< sensor_msgs::Image > > tf_filter_
#define NODELET_FATAL(...)
bool isValidPoint(const pcl::PointXYZ &p)
const std::string TYPE_32FC2


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:46