depth_calibration_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: C++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, 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 #include "jsk_pcl_ros/depth_calibration.h"
00037 #include <jsk_topic_tools/rosparam_utils.h>
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <sensor_msgs/image_encodings.h>
00040 
00041 namespace jsk_pcl_ros
00042 {
00043   void DepthCalibration::onInit()
00044   {
00045     DiagnosticNodelet::onInit();
00046     if (pnh_->hasParam("coefficients2")) {
00047       jsk_topic_tools::readVectorParameter(
00048         *pnh_, "coefficients2", coefficients2_);
00049     }
00050     else {
00051       coefficients2_.assign(5, 0);
00052     }
00053     if (pnh_->hasParam("coefficients1")) {
00054       jsk_topic_tools::readVectorParameter(
00055         *pnh_, "coefficients1", coefficients1_);
00056     }
00057     else {
00058       coefficients1_.assign(5, 0);
00059       coefficients1_[4] = 1.0;
00060     }
00061     if (pnh_->hasParam("coefficients0")) {
00062       jsk_topic_tools::readVectorParameter(
00063         *pnh_, "coefficients0", coefficients0_);
00064     }
00065     else {
00066       coefficients0_.assign(5, 0);
00067     }
00068     pnh_->param("use_abs", use_abs_, false);
00069     pnh_->param("uv_scale", uv_scale_, 1.0);
00070     printModel();
00071     set_calibration_parameter_srv_ = pnh_->advertiseService(
00072       "set_calibration_parameter",
00073       &DepthCalibration::setCalibrationParameter,
00074       this);
00075     pub_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
00076     onInitPostProcess();
00077   }
00078   
00079   void DepthCalibration::printModel()
00080   {
00081     NODELET_INFO("C2(u, v) = %fu^2 + %fu + %fv^2 + %fv + %f",
00082              coefficients2_[0], coefficients2_[1], coefficients2_[2], coefficients2_[3], coefficients2_[4]);
00083     NODELET_INFO("C1(u, v) = %fu^2 + %fu + %fv^2 + %fv + %f",
00084              coefficients1_[0], coefficients1_[1], coefficients1_[2], coefficients1_[3], coefficients1_[4]);
00085     NODELET_INFO("C0(u, v) = %fu^2 + %fu + %fv^2 + %fv + %f",
00086              coefficients0_[0], coefficients0_[1], coefficients0_[2], coefficients0_[3], coefficients0_[4]);
00087     if (use_abs_) {
00088       NODELET_INFO("use_abs: True");
00089     }
00090     else {
00091       NODELET_INFO("use_abs: False");
00092     }
00093   }
00094 
00095   bool DepthCalibration::setCalibrationParameter(
00096     jsk_recognition_msgs::SetDepthCalibrationParameter::Request& req,
00097     jsk_recognition_msgs::SetDepthCalibrationParameter::Response& res)
00098   {
00099     boost::mutex::scoped_lock lock(mutex_);
00100     coefficients2_ = req.parameter.coefficients2;
00101     coefficients1_ = req.parameter.coefficients1;
00102     coefficients0_ = req.parameter.coefficients0;
00103     use_abs_ = req.parameter.use_abs;
00104     printModel();
00105     return true;
00106   }
00107 
00108   void DepthCalibration::subscribe()
00109   {
00110       sub_input_.subscribe(*pnh_, "input", 1);
00111       sub_camera_info_.subscribe(*pnh_, "camera_info", 1);
00112       sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00113       sync_->connectInput(sub_input_, sub_camera_info_);
00114       sync_->registerCallback(boost::bind(&DepthCalibration::calibrate, this, _1, _2));
00115   }
00116   
00117   void DepthCalibration::unsubscribe()
00118   {
00119       sub_input_.unsubscribe();
00120       sub_camera_info_.unsubscribe();
00121   }
00122 
00123   void DepthCalibration::calibrate(
00124       const sensor_msgs::Image::ConstPtr& msg,
00125       const sensor_msgs::CameraInfo::ConstPtr& camera_info)
00126   {
00127     boost::mutex::scoped_lock lock(mutex_);
00128     cv_bridge::CvImagePtr cv_ptr;
00129     try
00130     {
00131       cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
00132     }
00133     catch (cv_bridge::Exception& e)
00134     {
00135       NODELET_ERROR("cv_bridge exception: %s", e.what());
00136       return;
00137     }
00138     cv::Mat image = cv_ptr->image;
00139     cv::Mat output_image = image.clone();
00140     double cu = camera_info->P[2];
00141     double cv = camera_info->P[6];
00142     for(int v = 0; v < image.rows; v++) {
00143       for(int u = 0; u < image.cols; u++) {
00144         float z = image.at<float>(v, u);
00145         if (std::isnan(z)) {
00146           output_image.at<float>(v, u) = z;
00147         }
00148         else {
00149           output_image.at<float>(v, u) = applyModel(z, u, v, cu, cv);
00150         }
00151         //NODELET_INFO("z: %f", z);
00152       }
00153     }
00154     sensor_msgs::Image::Ptr ros_image = cv_bridge::CvImage(msg->header, "32FC1", output_image).toImageMsg();
00155     pub_.publish(ros_image);
00156   }
00157 
00158   void DepthCalibration::updateDiagnostic(
00159       diagnostic_updater::DiagnosticStatusWrapper &stat)
00160   {
00161   }
00162 
00163 }
00164 #include <pluginlib/class_list_macros.h>
00165 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::DepthCalibration,
00166                           nodelet::Nodelet);


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