heightmap_time_accumulation_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 
00038 #include "jsk_pcl_ros/heightmap_time_accumulation.h"
00039 #include "jsk_pcl_ros/tf_listener_singleton.h"
00040 #include <cv_bridge/cv_bridge.h>
00041 #include <sensor_msgs/image_encodings.h>
00042 #include <pcl/common/transforms.h>
00043 
00044 namespace jsk_pcl_ros
00045 {
00046   void HeightmapTimeAccumulation::onInit()
00047   {
00048     ConnectionBasedNodelet::onInit();
00049     pub_config_ = pnh_->advertise<jsk_recognition_msgs::HeightmapConfig>(
00050       "output/config", 1, true);
00051     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00052     typename dynamic_reconfigure::Server<Config>::CallbackType f =
00053       boost::bind (&HeightmapTimeAccumulation::configCallback, this, _1, _2);
00054     srv_->setCallback (f);
00055     sub_config_ = pnh_->subscribe(
00056       getHeightmapConfigTopic(pnh_->resolveName("input")), 1,
00057       &HeightmapTimeAccumulation::configTopicCallback, this);
00058     if (!pnh_->getParam("center_frame_id", center_frame_id_)) {
00059       NODELET_FATAL("no ~center_frame_id is specified");
00060       return;
00061     }
00062     if (!pnh_->getParam("fixed_frame_id", fixed_frame_id_)) {
00063       NODELET_FATAL("no ~fixed_frame_id is specified");
00064       return;
00065     }
00066     int tf_queue_size;
00067     pnh_->param("tf_queue_size", tf_queue_size, 10);
00068     prev_from_center_to_fixed_ = Eigen::Affine3f::Identity();
00069     tf_ = TfListenerSingleton::getInstance();
00070     pub_output_ = pnh_->advertise<sensor_msgs::Image>("output", 1, true);
00071     sub_previous_pointcloud_ = pnh_->subscribe<sensor_msgs::PointCloud2>(
00072       "input/prev_pointcloud", 5, 
00073       &HeightmapTimeAccumulation::prevPointCloud, this);
00074     sub_heightmap_.subscribe(*pnh_, "input", 10);
00075     tf_filter_.reset(new tf::MessageFilter<sensor_msgs::Image>(
00076                        sub_heightmap_,
00077                        *tf_,
00078                        fixed_frame_id_,
00079                        tf_queue_size));
00080     tf_filter_->registerCallback(
00081       boost::bind(
00082         &HeightmapTimeAccumulation::accumulate, this, _1));
00083     srv_reset_ = pnh_->advertiseService("reset", &HeightmapTimeAccumulation::resetCallback, this);
00084     onInitPostProcess();
00085   }
00086   
00087   void HeightmapTimeAccumulation::subscribe()
00088   {
00089 
00090   }
00091   void HeightmapTimeAccumulation::unsubscribe()
00092   {
00093 
00094   }
00095 
00096 
00097   void HeightmapTimeAccumulation::publishHeightmap(
00098     const cv::Mat& heightmap, const std_msgs::Header& header)
00099   {
00100     pub_output_.publish(cv_bridge::CvImage(
00101                           header,
00102                           sensor_msgs::image_encodings::TYPE_32FC2,
00103                           heightmap).toImageMsg());
00104   }
00105 
00106   cv::Point HeightmapTimeAccumulation::toIndex(
00107     const PointType& p, const cv::Mat& map)
00108   {
00109     if (p.x > max_x_ || p.x < min_x_ ||
00110         p.y > max_y_ || p.y < min_y_) {
00111       return cv::Point(-1, -1);
00112     }
00113     const float offsetted_x = p.x - min_x_;
00114     const float offsetted_y = p.y - min_y_;
00115     const float dx = (max_x_ - min_x_) / map.cols;
00116     const float dy = (max_y_ - min_y_) / map.rows;
00117     return cv::Point(std::floor(offsetted_x / dx),
00118                      std::floor(offsetted_y / dy));
00119   }
00120 
00121   bool HeightmapTimeAccumulation::isValidIndex(const cv::Point& index, const cv::Mat& map)
00122   {
00123     return (index.x >= 0 && index.x < map.cols && 
00124             index.y >= 0 && index.y < map.rows);
00125   }
00126 
00127   bool HeightmapTimeAccumulation::isValidCell(const cv::Point& index, const cv::Mat& map)
00128   {
00129     float v = map.at<cv::Vec2f>(index.y, index.x)[0];
00130     return !std::isnan(v) && v != -FLT_MAX;
00131   }
00132 
00133   void HeightmapTimeAccumulation::accumulate(
00134     const sensor_msgs::Image::ConstPtr& msg)
00135   {
00136     boost::mutex::scoped_lock lock(mutex_);
00137     if (!config_) {
00138       NODELET_ERROR("no ~input/config is yet available");
00139       return;
00140     }
00141     tf::StampedTransform tf_transform;
00142     tf_->lookupTransform(fixed_frame_id_, center_frame_id_,
00143                          msg->header.stamp,
00144                          tf_transform);
00145     Eigen::Affine3f from_center_to_fixed;
00146     tf::transformTFToEigen(tf_transform, from_center_to_fixed);
00147     cv::Mat new_heightmap = cv_bridge::toCvShare(
00148       msg, sensor_msgs::image_encodings::TYPE_32FC2)->image;
00149     // Transform prev_cloud_ to current frame
00150     Eigen::Affine3f from_prev_to_current
00151       = prev_from_center_to_fixed_.inverse() * from_center_to_fixed;
00152     pcl::PointCloud<PointType > transformed_pointcloud;
00153     pcl::transformPointCloud(prev_cloud_, transformed_pointcloud, from_prev_to_current.inverse());
00154 
00155     mergedAccmulation(transformed_pointcloud, new_heightmap);
00156 
00157     publishHeightmap(new_heightmap, msg->header);
00158     // prev_from_center_to_fixed_ = from_center_to_fixed;
00159   }
00160 
00161   void HeightmapTimeAccumulation::overwriteAccmulation(
00162     pcl::PointCloud<PointType > &transformed_pointcloud, cv::Mat &new_heightmap)
00163   {
00164     for (size_t i = 0; i < transformed_pointcloud.points.size(); i++) {
00165       PointType p = transformed_pointcloud.points[i];
00166       if (isValidPoint(p)) {
00167         cv::Point index = toIndex(p, new_heightmap);
00168         if (isValidIndex(index, new_heightmap)) {
00169           if (!isValidCell(index, new_heightmap)) {
00170             // There is not valid data in current heightmap,
00171             new_heightmap.at<cv::Vec2f>(index.y, index.x)[0] = p.z;
00172             new_heightmap.at<cv::Vec2f>(index.y, index.x)[1] = (float)(p.intensity);
00173           } else {
00174             // There is valid data in current heightmap,
00175             if (new_heightmap.at<cv::Vec2f>(index.y, index.x)[1] < (float)(p.intensity)) {
00176               // heightmap has worth quality than prev_pointcloud
00177               new_heightmap.at<cv::Vec2f>(index.y, index.x)[0] = p.z;
00178               new_heightmap.at<cv::Vec2f>(index.y, index.x)[1] = (float)(p.intensity);
00179             }
00180           }
00181         }
00182       }
00183     }
00184   }
00185 
00186   void HeightmapTimeAccumulation::mergedAccmulation(
00187     pcl::PointCloud<PointType > &transformed_pointcloud, cv::Mat &new_heightmap)
00188   {
00189     float offset_new_z = 0.0;
00190     float offset_org_z = 0.0;
00191     if (use_offset_) { // calc offset
00192       double diff_z = 0.0;
00193       long cntr = 0;
00194       for (size_t i = 0; i < transformed_pointcloud.points.size(); i++) {
00195         PointType p = transformed_pointcloud.points[i];
00196         if (isValidPoint(p)) {
00197           cv::Point index = toIndex(p, new_heightmap);
00198           if (isValidIndex(index, new_heightmap)) {
00199             if (isValidCell(index, new_heightmap)) {
00200               float new_z = new_heightmap.at<cv::Vec2f>(index.y, index.x)[0];
00201               float new_q = new_heightmap.at<cv::Vec2f>(index.y, index.x)[1];
00202               float org_z = p.z;
00203               float org_q = p.intensity;
00204               float tmp_diff = (new_z - org_z);
00205               if (std::fabs(tmp_diff) < 0.04) {
00206                 diff_z += tmp_diff;
00207                 cntr++;
00208               }
00209             }
00210           }
00211         }
00212       }
00213       if(cntr > 0) {
00214         diff_z /= cntr;
00215         offset_new_z = - diff_z/2.0;
00216         offset_org_z =   diff_z/2.0;
00217       }
00218       // add offset to new heightmap
00219       for(int yy = 0; yy < new_heightmap.rows; yy++) {
00220         for(int xx = 0; xx < new_heightmap.cols; xx++) {
00221           new_heightmap.at<cv::Vec2f>(yy, xx)[0] += offset_new_z;
00222         }
00223       }
00224     }
00225 
00226     //averaging strategy
00227     for (size_t i = 0; i < transformed_pointcloud.points.size(); i++) {
00228       PointType p = transformed_pointcloud.points[i];
00229       if (isValidPoint(p)) {
00230         cv::Point index = toIndex(p, new_heightmap);
00231         if (isValidIndex(index, new_heightmap)) {
00232           p.z += offset_org_z;
00233           if (!isValidCell(index, new_heightmap)) {
00234             // There is not valid data in current heightmap,
00235             new_heightmap.at<cv::Vec2f>(index.y, index.x)[0] = p.z;
00236             new_heightmap.at<cv::Vec2f>(index.y, index.x)[1] = (float)(p.intensity);
00237           } else {
00238             // There is valid data in current heightmap,
00239             float new_z = new_heightmap.at<cv::Vec2f>(index.y, index.x)[0];
00240             float new_q = new_heightmap.at<cv::Vec2f>(index.y, index.x)[1];
00241             float org_z = p.z;
00242             float org_q = p.intensity;
00243             new_z = (new_z * new_q + org_z * org_q)/(new_q + org_q);
00244             new_q = (new_q + org_q)/1.6; // ?? new quality
00245             if(new_q > 1.0) new_q = 1.0;
00246             new_heightmap.at<cv::Vec2f>(index.y, index.x)[0] = new_z;
00247             new_heightmap.at<cv::Vec2f>(index.y, index.x)[1] = new_q;
00248           }
00249         }
00250       }
00251     }
00252 
00253     if (use_bilateral_) {
00254       std::vector<cv::Mat> fimages;
00255       cv::split(new_heightmap, fimages);
00256       cv::Mat res_image;
00257       // filter
00258       cv::bilateralFilter(fimages[0], res_image, bilateral_filter_size_, bilateral_sigma_color_, bilateral_sigma_space_);
00259       {
00260         for (size_t j = 0; j < res_image.rows; j++) {
00261           for (size_t i = 0; i < res_image.cols; i++) {
00262             float ov = fimages[0].at<float>(j, i);
00263             if (ov == -FLT_MAX) {
00264               res_image.at<float>(j, i) = -FLT_MAX;
00265             }
00266           }
00267         }
00268       }
00269       // reconstruct images
00270       std::vector<cv::Mat> ret_images;
00271       ret_images.push_back(res_image);
00272       ret_images.push_back(fimages[1]);
00273       cv::merge(ret_images, new_heightmap);
00274     }
00275   }
00276 
00277   void HeightmapTimeAccumulation::prevPointCloud(
00278     const sensor_msgs::PointCloud2::ConstPtr& msg)
00279   {
00280     boost::mutex::scoped_lock lock(mutex_);
00281 
00282     pcl::fromROSMsg(*msg, prev_cloud_);
00283     // get transform at subscribed timestamp
00284     tf::StampedTransform tf_transform;
00285     tf_->lookupTransform(fixed_frame_id_, center_frame_id_,
00286                          msg->header.stamp,
00287                          tf_transform);
00288     tf::transformTFToEigen(tf_transform, prev_from_center_to_fixed_);
00289   }
00290 
00291   void HeightmapTimeAccumulation::configTopicCallback(
00292     const jsk_recognition_msgs::HeightmapConfig::ConstPtr& msg)
00293   {
00294     boost::mutex::scoped_lock lock(mutex_);
00295     config_ = msg;
00296     min_x_ = msg->min_x;
00297     max_x_ = msg->max_x;
00298     min_y_ = msg->min_y;
00299     max_y_ = msg->max_y;
00300     pub_config_.publish(msg);
00301   }
00302 
00303   bool HeightmapTimeAccumulation::resetCallback(std_srvs::Empty::Request& req,
00304                                                 std_srvs::Empty::Response& res)
00305   {
00306     boost::mutex::scoped_lock lock(mutex_);
00307     prev_from_center_to_fixed_ = Eigen::Affine3f::Identity();
00308     prev_cloud_.points.clear();
00309     return true;
00310   }
00311 
00312   void HeightmapTimeAccumulation::configCallback(Config& config, uint32_t level)
00313   {
00314     boost::mutex::scoped_lock lock(mutex_);
00315     //min_x_ = config.min_x;
00316     //max_x_ = config.max_x;
00317     //min_y_ = config.min_y;
00318     //max_y_ = config.max_y;
00319     use_offset_    = config.use_offset;
00320     use_bilateral_ = config.use_bilateral;
00321     bilateral_filter_size_ = config.bilateral_filter_size;
00322     bilateral_sigma_color_ = config.bilateral_sigma_color;
00323     bilateral_sigma_space_ = config.bilateral_sigma_space;
00324   }
00325 }
00326 
00327 #include <pluginlib/class_list_macros.h>
00328 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::HeightmapTimeAccumulation, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:49