depth_camera_error_visualization.cpp

Go to the documentation of this file.

// -*- mode: c++ -*-
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, JSK Lab
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/o2r other materials provided
 *     with the distribution.
 *   * Neither the name of the JSK Lab nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#define BOOST_PARAMETER_MAX_ARITY 7

#include <ros/ros.h>
#include <image_geometry/pinhole_camera_model.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/CameraInfo.h>

#include "jsk_pcl_ros/pcl_conversion_util.h"

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/synchronizer.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <jsk_recognition_msgs/PlotData.h>
#include "one_data_stat.h"

using namespace jsk_pcl_ros;

typedef message_filters::sync_policies::ExactTime<
  sensor_msgs::CameraInfo,
  sensor_msgs::Image,
  sensor_msgs::PointCloud2 > SyncPolicy;

ros::Publisher pub_error_image, pub_variance_image, pub_stddev_image, pub_mean_image;

ros::Publisher pub_error_plot, pub_variance_plot, pub_stddev_plot;

boost::mutex mutex;

sensor_msgs::PointCloud2::ConstPtr ground_truth;

std::vector<OneDataStat::Ptr> stats;

void groundTruthCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  boost::mutex::scoped_lock lock(mutex);
  ground_truth = msg;
}

void generateImage(
  const std::vector<OneDataStat::Ptr>& stats,
  cv::Mat& image,
  boost::function<double(OneDataStat&)> accessor)
{
  for (size_t i = 0; i < image.rows; i++) {
    for (size_t j = 0; j < image.cols; j++) {
      size_t index = i * image.cols + j;
      if (stats[index]->count() > 2) {
        image.at<float>(i, j) = accessor(*stats[index]);
      }
    }
  }
}

void publishImage(const cv::Mat& image, ros::Publisher& pub)
{
  std_msgs::Header header;
  header.stamp = ros::Time::now();
  pub.publish(cv_bridge::CvImage(header, sensor_msgs::image_encodings::TYPE_32FC1, image).toImageMsg());
}

void publishPlotData(const std::vector<OneDataStat::Ptr>& stats,
                     ros::Publisher& pub,
                     boost::function<double(OneDataStat&)> accessor)
{
  jsk_recognition_msgs::PlotData msg;
  for (size_t i = 0; i < stats.size(); i++) {
    if (i % 8 == 0) {
      if (stats[i]->count() > 2) {
        msg.xs.push_back(stats[i]->mean());
        msg.ys.push_back(accessor(*stats[i]));
      }
    }
  }
  pub.publish(msg);
}

void dataCallback(
  const sensor_msgs::CameraInfo::ConstPtr& camera_info_msg,
  const sensor_msgs::Image::ConstPtr& left_image_msg,
  const sensor_msgs::PointCloud2::ConstPtr& point_cloud_msg)
{
  boost::mutex::scoped_lock lock(mutex);
  // if (!ground_truth) {
  //   ROS_ERROR("no ground truth pointcloud");
  //   return;
  // }
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg(*point_cloud_msg, *cloud);

  // stats looks not initialized yet
  if (stats.size() == 0) {
    for (size_t i = 0; i < cloud->points.size(); i++) {
      stats.push_back(OneDataStat::Ptr(new OneDataStat));
    }
  }

  
  // Initialize accumurator
  // 1 loop for all the points
  // 2 check the point can have good ground_truth by 3-D -> 2-D projection
  //   based on camera internal parameter
  // 3 compute error from ground truth

  // at this time, we do not implement step 2
  for (size_t i = 0; i < cloud->points.size(); i++) {
    pcl::PointXYZ p = cloud->points[i];
    // We expect organized pointcloud in input/point_cloud message.
    // So we need to check nan.
    if (isnan(p.x) || isnan(p.y) || isnan(p.z)) {
      continue;
    }
    stats[i]->addData(p.z);
  }

  // visualization as image
  cv::Mat mean_image = cv::Mat::zeros(cloud->height, cloud->width, CV_32FC1);
  cv::Mat variance_image = cv::Mat::zeros(cloud->height, cloud->width, CV_32FC1);
  cv::Mat stddev_image = cv::Mat::zeros(cloud->height, cloud->width, CV_32FC1);
  generateImage(stats, mean_image, &mean);
  generateImage(stats, variance_image, &variance);
  generateImage(stats, stddev_image, &stddev);
  publishImage(mean_image, pub_mean_image);
  publishImage(variance_image, pub_variance_image);
  publishImage(stddev_image, pub_stddev_image);
  publishPlotData(stats, pub_variance_plot, &variance);
  publishPlotData(stats, pub_stddev_plot, &stddev);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "depth_camera_error_visualization");
  ros::NodeHandle nh("~");
  // Setup publishers
  pub_error_image = nh.advertise<sensor_msgs::Image>("output/error_image", 1);
  pub_variance_image = nh.advertise<sensor_msgs::Image>("output/variance_image", 1);
  pub_stddev_image = nh.advertise<sensor_msgs::Image>("output/stddev_image", 1);
  pub_mean_image = nh.advertise<sensor_msgs::Image>("output/mean_image", 1);
  pub_error_plot = nh.advertise<jsk_recognition_msgs::PlotData>("output/error_plot", 1);
  pub_variance_plot = nh.advertise<jsk_recognition_msgs::PlotData>("output/variance_plot", 1);
  pub_stddev_plot = nh.advertise<jsk_recognition_msgs::PlotData>("output/stddev_plot", 1);
  //ros::Subscriber sub_ground_truth = nh.subscribe("input/ground_truth", 1, &groundTruthCallback);
  message_filters::Subscriber<sensor_msgs::CameraInfo> sub_camera_info;
  message_filters::Subscriber<sensor_msgs::Image> sub_left_image;
  message_filters::Subscriber<sensor_msgs::PointCloud2> sub_point_cloud;
  sub_camera_info.subscribe(nh, "input/camera_info", 1);
  sub_left_image.subscribe(nh, "input/left_image", 1);
  sub_point_cloud.subscribe(nh, "input/point_cloud", 1);
  message_filters::Synchronizer<SyncPolicy> sync(100);
  sync.connectInput(sub_camera_info, sub_left_image, sub_point_cloud);
  sync.registerCallback(&dataCallback);
  ros::spin();
  return 0;
}