36 #define BOOST_PARAMETER_MAX_ARITY 7 42 #include <pcl/common/transforms.h> 48 ConnectionBasedNodelet::onInit();
50 "output/config", 1,
true);
51 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
52 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
54 srv_->setCallback (f);
67 pnh_->param(
"tf_queue_size", tf_queue_size, 10);
70 pub_output_ = pnh_->advertise<sensor_msgs::Image>(
"output", 1,
true);
72 "input/prev_pointcloud", 5,
98 const cv::Mat& heightmap,
const std_msgs::Header&
header)
103 heightmap).toImageMsg());
111 return cv::Point(-1, -1);
113 const float offsetted_x = p.x -
min_x_;
114 const float offsetted_y = p.y -
min_y_;
117 return cv::Point(std::floor(offsetted_x / dx),
118 std::floor(offsetted_y / dy));
123 return (index.x >= 0 && index.x < map.cols &&
124 index.y >= 0 && index.y < map.rows);
129 float v = map.at<cv::Vec2f>(index.y, index.x)[0];
130 return !std::isnan(v) && v != -FLT_MAX;
134 const sensor_msgs::Image::ConstPtr&
msg)
151 Eigen::Affine3f from_center_to_fixed;
156 Eigen::Affine3f from_prev_to_current
158 pcl::PointCloud<PointType > transformed_pointcloud;
159 pcl::transformPointCloud(
prev_cloud_, transformed_pointcloud, from_prev_to_current.inverse());
168 pcl::PointCloud<PointType > &transformed_pointcloud, cv::Mat &new_heightmap)
170 for (
size_t i = 0; i < transformed_pointcloud.points.size(); i++) {
171 PointType p = transformed_pointcloud.points[i];
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);
181 if (new_heightmap.at<cv::Vec2f>(index.y, index.x)[1] < (
float)(p.intensity)) {
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);
193 pcl::PointCloud<PointType > &transformed_pointcloud, cv::Mat &new_heightmap)
195 float offset_new_z = 0.0;
196 float offset_org_z = 0.0;
200 for (
size_t i = 0; i < transformed_pointcloud.points.size(); i++) {
201 PointType p = transformed_pointcloud.points[i];
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];
209 float org_q = p.intensity;
210 float tmp_diff = (new_z - org_z);
211 if (std::fabs(tmp_diff) < 0.04) {
221 offset_new_z = - diff_z/2.0;
222 offset_org_z = diff_z/2.0;
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;
233 for (
size_t i = 0; i < transformed_pointcloud.points.size(); i++) {
234 PointType p = transformed_pointcloud.points[i];
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);
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];
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;
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;
260 std::vector<cv::Mat> fimages;
261 cv::split(new_heightmap, fimages);
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;
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);
284 const sensor_msgs::PointCloud2::ConstPtr&
msg)
304 const jsk_recognition_msgs::HeightmapConfig::ConstPtr&
msg)
316 std_srvs::Empty::Response&
res)
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)
ros::Publisher pub_output_
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)
int bilateral_filter_size_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
ros::ServiceServer srv_reset_
virtual void prevPointCloud(const sensor_msgs::PointCloud2::ConstPtr &msg)
pcl::PointCloud< PointType > prev_cloud_
double bilateral_sigma_space_
tf::TransformListener * tf_
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)
virtual cv::Point toIndex(const PointType &p, const cv::Mat &map)
ros::Subscriber sub_config_
virtual void publishHeightmap(const cv::Mat &heightmap, const std_msgs::Header &header)
double bilateral_sigma_color_
HeightmapTimeAccumulationConfig Config
virtual void accumulate(const sensor_msgs::Image::ConstPtr &new_heightmap)
virtual void unsubscribe()
std::string center_frame_id_
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)
ros::Publisher pub_config_
std::string fixed_frame_id_
static tf::TransformListener * getInstance()
Eigen::Affine3f prev_from_center_to_fixed_
boost::shared_ptr< tf::MessageFilter< sensor_msgs::Image > > tf_filter_
#define NODELET_FATAL(...)
bool isValidPoint(const pcl::PointXYZ &p)
const std::string TYPE_32FC2
ros::Subscriber sub_previous_pointcloud_