depth_camera_error_visualization.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 <ros/ros.h>
00039 #include <image_geometry/pinhole_camera_model.h>
00040 #include <sensor_msgs/Image.h>
00041 #include <sensor_msgs/PointCloud2.h>
00042 #include <sensor_msgs/CameraInfo.h>
00043 
00044 #include "jsk_pcl_ros/pcl_conversion_util.h"
00045 
00046 #include <message_filters/subscriber.h>
00047 #include <message_filters/time_synchronizer.h>
00048 #include <message_filters/synchronizer.h>
00049 #include <cv_bridge/cv_bridge.h>
00050 #include <sensor_msgs/image_encodings.h>
00051 #include <jsk_recognition_msgs/PlotData.h>
00052 #include "one_data_stat.h"
00053 
00054 using namespace jsk_pcl_ros;
00055 
00065 typedef message_filters::sync_policies::ExactTime<
00066   sensor_msgs::CameraInfo,
00067   sensor_msgs::Image,
00068   sensor_msgs::PointCloud2 > SyncPolicy;
00069 
00074 ros::Publisher pub_error_image, pub_variance_image, pub_stddev_image, pub_mean_image;
00075 
00080 ros::Publisher pub_error_plot, pub_variance_plot, pub_stddev_plot;
00081 
00086 boost::mutex mutex;
00087 
00092 sensor_msgs::PointCloud2::ConstPtr ground_truth;
00093 
00098 std::vector<OneDataStat::Ptr> stats;
00099 
00100 void groundTruthCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
00101 {
00102   boost::mutex::scoped_lock lock(mutex);
00103   ground_truth = msg;
00104 }
00105 
00110 void generateImage(
00111   const std::vector<OneDataStat::Ptr>& stats,
00112   cv::Mat& image,
00113   boost::function<double(OneDataStat&)> accessor)
00114 {
00115   for (size_t i = 0; i < image.rows; i++) {
00116     for (size_t j = 0; j < image.cols; j++) {
00117       size_t index = i * image.cols + j;
00118       if (stats[index]->count() > 2) {
00119         image.at<float>(i, j) = accessor(*stats[index]);
00120       }
00121     }
00122   }
00123 }
00124 
00125 void publishImage(const cv::Mat& image, ros::Publisher& pub)
00126 {
00127   std_msgs::Header header;
00128   header.stamp = ros::Time::now();
00129   pub.publish(cv_bridge::CvImage(header, sensor_msgs::image_encodings::TYPE_32FC1, image).toImageMsg());
00130 }
00131 
00132 void publishPlotData(const std::vector<OneDataStat::Ptr>& stats,
00133                      ros::Publisher& pub,
00134                      boost::function<double(OneDataStat&)> accessor)
00135 {
00136   jsk_recognition_msgs::PlotData msg;
00137   for (size_t i = 0; i < stats.size(); i++) {
00138     if (i % 8 == 0) {
00139       if (stats[i]->count() > 2) {
00140         msg.xs.push_back(stats[i]->mean());
00141         msg.ys.push_back(accessor(*stats[i]));
00142       }
00143     }
00144   }
00145   pub.publish(msg);
00146 }
00147 
00148 void dataCallback(
00149   const sensor_msgs::CameraInfo::ConstPtr& camera_info_msg,
00150   const sensor_msgs::Image::ConstPtr& left_image_msg,
00151   const sensor_msgs::PointCloud2::ConstPtr& point_cloud_msg)
00152 {
00153   boost::mutex::scoped_lock lock(mutex);
00154   // if (!ground_truth) {
00155   //   ROS_ERROR("no ground truth pointcloud");
00156   //   return;
00157   // }
00158   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00159   pcl::fromROSMsg(*point_cloud_msg, *cloud);
00160 
00161   // stats looks not initialized yet
00162   if (stats.size() == 0) {
00163     for (size_t i = 0; i < cloud->points.size(); i++) {
00164       stats.push_back(OneDataStat::Ptr(new OneDataStat));
00165     }
00166   }
00167 
00168   
00169   // Initialize accumurator
00170   // 1 loop for all the points
00171   // 2 check the point can have good ground_truth by 3-D -> 2-D projection
00172   //   based on camera internal parameter
00173   // 3 compute error from ground truth
00174 
00175   // at this time, we do not implement step 2
00176   for (size_t i = 0; i < cloud->points.size(); i++) {
00177     pcl::PointXYZ p = cloud->points[i];
00178     // We expect organized pointcloud in input/point_cloud message.
00179     // So we need to check nan.
00180     if (std::isnan(p.x) || std::isnan(p.y) || std::isnan(p.z)) {
00181       continue;
00182     }
00183     stats[i]->addData(p.z);
00184   }
00185 
00186   // visualization as image
00187   cv::Mat mean_image = cv::Mat::zeros(cloud->height, cloud->width, CV_32FC1);
00188   cv::Mat variance_image = cv::Mat::zeros(cloud->height, cloud->width, CV_32FC1);
00189   cv::Mat stddev_image = cv::Mat::zeros(cloud->height, cloud->width, CV_32FC1);
00190   generateImage(stats, mean_image, &mean);
00191   generateImage(stats, variance_image, &variance);
00192   generateImage(stats, stddev_image, &stddev);
00193   publishImage(mean_image, pub_mean_image);
00194   publishImage(variance_image, pub_variance_image);
00195   publishImage(stddev_image, pub_stddev_image);
00196   publishPlotData(stats, pub_variance_plot, &variance);
00197   publishPlotData(stats, pub_stddev_plot, &stddev);
00198 }
00199 
00200 int main(int argc, char** argv)
00201 {
00202   ros::init(argc, argv, "depth_camera_error_visualization");
00203   ros::NodeHandle nh("~");
00204   // Setup publishers
00205   pub_error_image = nh.advertise<sensor_msgs::Image>("output/error_image", 1);
00206   pub_variance_image = nh.advertise<sensor_msgs::Image>("output/variance_image", 1);
00207   pub_stddev_image = nh.advertise<sensor_msgs::Image>("output/stddev_image", 1);
00208   pub_mean_image = nh.advertise<sensor_msgs::Image>("output/mean_image", 1);
00209   pub_error_plot = nh.advertise<jsk_recognition_msgs::PlotData>("output/error_plot", 1);
00210   pub_variance_plot = nh.advertise<jsk_recognition_msgs::PlotData>("output/variance_plot", 1);
00211   pub_stddev_plot = nh.advertise<jsk_recognition_msgs::PlotData>("output/stddev_plot", 1);
00212   //ros::Subscriber sub_ground_truth = nh.subscribe("input/ground_truth", 1, &groundTruthCallback);
00213   message_filters::Subscriber<sensor_msgs::CameraInfo> sub_camera_info;
00214   message_filters::Subscriber<sensor_msgs::Image> sub_left_image;
00215   message_filters::Subscriber<sensor_msgs::PointCloud2> sub_point_cloud;
00216   sub_camera_info.subscribe(nh, "input/camera_info", 1);
00217   sub_left_image.subscribe(nh, "input/left_image", 1);
00218   sub_point_cloud.subscribe(nh, "input/point_cloud", 1);
00219   message_filters::Synchronizer<SyncPolicy> sync(100);
00220   sync.connectInput(sub_camera_info, sub_left_image, sub_point_cloud);
00221   sync.registerCallback(&dataCallback);
00222   ros::spin();
00223   return 0;
00224 }


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