fisheye_to_panorama.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 #include "jsk_perception/fisheye_to_panorama.h"
00037 #include <jsk_topic_tools/log_utils.h>
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <sensor_msgs/Image.h>
00040 #include <sensor_msgs/image_encodings.h>
00041 #include <algorithm>
00042 #include <math.h> 
00043 #include <boost/assign.hpp>
00044 
00045 #define PI 3.141592
00046 
00047 namespace jsk_perception
00048 {
00049   void FisheyeToPanorama::onInit()
00050   {
00051     DiagnosticNodelet::onInit();
00052     pnh_->param("use_panorama", use_panorama_, false);
00053     pnh_->param("simple_panorama", simple_panorama_, false);
00054     pub_undistorted_image_ = advertise<sensor_msgs::Image>(
00055       *pnh_, "output", 1);
00056     if(use_panorama_ && simple_panorama_)
00057       pub_undistorted_bilinear_image_ = advertise<sensor_msgs::Image>(*pnh_, "output_bilinear", 1);
00058 
00059     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00060     dynamic_reconfigure::Server<Config>::CallbackType f =
00061       boost::bind (&FisheyeToPanorama::configCallback, this, _1, _2);
00062     srv_->setCallback (f);
00063 
00064     scale_ = 0.5;
00065     upside_down_ = false;
00066     offset_degree_ = 180.0;
00067     onInitPostProcess();
00068   }
00069 
00070   void FisheyeToPanorama::configCallback(Config &new_config, uint32_t level)
00071   {
00072     max_degree_ = new_config.degree;
00073     scale_ = new_config.scale;
00074     upside_down_ = new_config.upside_down;
00075     offset_degree_ = new_config.offset_degree;
00076   }
00077 
00078 
00079   void FisheyeToPanorama::subscribe()
00080   {
00081     sub_image_ = pnh_->subscribe("input", 1, &FisheyeToPanorama::rectify, this);
00082     ros::V_string names = boost::assign::list_of("~input");
00083     jsk_topic_tools::warnNoRemap(names);
00084   }
00085 
00086   void FisheyeToPanorama::unsubscribe()
00087   {
00088     sub_image_.shutdown();
00089   }
00090 
00091 
00092   void FisheyeToPanorama::rectify(const sensor_msgs::Image::ConstPtr& image_msg)
00093   {
00094     cv::Mat distorted = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8)->image;
00095     int l = distorted.rows / 2;
00096 
00097     if(use_panorama_){
00098       float min_degree = 30;
00099       float min_radian = min_degree * 3.14159265 /180.0;
00100       float tan_min_radian = tan(min_radian);
00101       const float K = 341.656050955;// x = K * theta
00102       if(simple_panorama_){
00103         cv::Mat undistorted(l, l*4, CV_8UC3);
00104         cv::Mat undistorted_bilinear(l, l*4, CV_8UC3);
00105         for(int i = 0; i < undistorted.rows; ++i){
00106           for(int j = 0; j < undistorted.cols; ++j){
00107         
00108             double radius = l - i;
00109             double theta = 2.0 * PI * (double)(-j) / (double)(4.0 * l);
00110             double fTrueX = radius * cos(theta);
00111             double fTrueY = radius * sin(theta);
00112 
00113             int x = (int)round(fTrueX) + l;
00114             int y = l - (int)round(fTrueY);
00115             if (x >= 0 && x < (2 * l) && y >= 0 && y < (2 * l))
00116               {
00117                 for(int c = 0; c < undistorted.channels(); ++c)
00118                   undistorted.data[ i * undistorted.step + j * undistorted.elemSize() + c ]
00119                     = distorted.data[x * distorted.step + y * distorted.elemSize() + c];
00120               }
00121 
00122             fTrueX = fTrueX + (double)l;
00123             fTrueY = (double)l - fTrueY;
00124 
00125             int iFloorX = (int)floor(fTrueX);
00126             int iFloorY = (int)floor(fTrueY);
00127             int iCeilingX = (int)ceil(fTrueX);
00128             int iCeilingY = (int)ceil(fTrueY);
00129 
00130             if (iFloorX < 0 || iCeilingX < 0 ||
00131                 iFloorX >= (2 * l) || iCeilingX >= (2 * l) ||
00132                 iFloorY < 0 || iCeilingY < 0 ||
00133                 iFloorY >= (2 * l) || iCeilingY >= (2 * l)) continue;
00134 
00135             double fDeltaX = fTrueX - (double)iFloorX;
00136             double fDeltaY = fTrueY - (double)iFloorY;
00137 
00138             cv::Mat clrTopLeft(1,1, CV_8UC3), clrTopRight(1,1, CV_8UC3), clrBottomLeft(1,1, CV_8UC3), clrBottomRight(1,1, CV_8UC3);
00139             for(int c = 0; c < undistorted.channels(); ++c){
00140               clrTopLeft.data[ c ] = distorted.data[iFloorX * distorted.step + iFloorY * distorted.elemSize() + c];
00141               clrTopRight.data[ c ] = distorted.data[iCeilingX * distorted.step + iFloorY * distorted.elemSize() + c];
00142               clrBottomLeft.data[ c ] = distorted.data[iFloorX * distorted.step + iCeilingY * distorted.elemSize() + c];
00143               clrBottomRight.data[ c ] = distorted.data[iCeilingX * distorted.step + iCeilingY * distorted.elemSize() + c];
00144             }
00145 
00146             double fTop0 = interpolate(fDeltaX, clrTopLeft.data[0], clrTopRight.data[0]);
00147             double fTop1 = interpolate(fDeltaX, clrTopLeft.data[1], clrTopRight.data[1]);
00148             double fTop2 = interpolate(fDeltaX, clrTopLeft.data[2], clrTopRight.data[2]);
00149             double fBottom0 = interpolate(fDeltaX, clrBottomLeft.data[0], clrBottomRight.data[0]);
00150             double fBottom1 = interpolate(fDeltaX, clrBottomLeft.data[1], clrBottomRight.data[1]);
00151             double fBottom2 = interpolate(fDeltaX, clrBottomLeft.data[2], clrBottomRight.data[2]);
00152 
00153             int i0 = (int)round(interpolate(fDeltaY, fTop0, fBottom0));
00154             int i1 = (int)round(interpolate(fDeltaY, fTop1, fBottom1));
00155             int i2 = (int)round(interpolate(fDeltaY, fTop2, fBottom2));
00156 
00157             i0 = std::min(255, std::max(i0, 0));
00158             i1 = std::min(255, std::max(i1, 0));
00159             i2 = std::min(255, std::max(i2, 0));
00160 
00161             undistorted_bilinear.data[ i * undistorted_bilinear.step + j * undistorted_bilinear.elemSize() + 0] = i0;
00162             undistorted_bilinear.data[ i * undistorted_bilinear.step + j * undistorted_bilinear.elemSize() + 1] = i1;
00163             undistorted_bilinear.data[ i * undistorted_bilinear.step + j * undistorted_bilinear.elemSize() + 2] = i2;
00164           }
00165         }
00166 
00167         pub_undistorted_image_.publish(
00168                                        cv_bridge::CvImage(
00169                                                           image_msg->header,
00170                                                           image_msg->encoding,
00171                                                           undistorted).toImageMsg());
00172         pub_undistorted_bilinear_image_.publish(cv_bridge::CvImage(
00173                                                                    image_msg->header,
00174                                                                    image_msg->encoding,
00175                                                                    undistorted_bilinear).toImageMsg());
00176       }else{
00177         cv::Mat undistorted(int(l * 1.0 / tan_min_radian*scale_), int(l *  2.0 * PI * scale_), CV_8UC3);
00178         int center_x = distorted.rows/2, center_y = distorted.cols/2;
00179 
00180         int offset_jndex = offset_degree_ / 180.0 * PI * l * scale_;
00181         for(int i = 0; i < undistorted.rows; ++i){
00182           for(int j = 0; j < undistorted.cols; ++j){
00183             float phi = PI / 2;
00184             if(i)
00185               phi = atan(l * 1.0 /i*scale_) + 0.5;
00186             float theta = (j-int(undistorted.cols/2))/scale_ * 1.0/l;
00187             int x = K * phi * cos(theta) + center_x;
00188             int y = K * phi * sin(theta) + center_y;
00189             for(int c = 0; c < undistorted.channels(); ++c){
00190               int index = undistorted.rows - 1 - i;
00191               if( upside_down_ )
00192                 index = i;
00193               int jndex = j + offset_jndex;
00194               if(jndex > undistorted.cols - 1)
00195                 jndex -= undistorted.cols - 1;
00196               undistorted.data[ index  * undistorted.step + jndex * undistorted.elemSize() + c ]
00197                 = distorted.data[ x * distorted.step + y * distorted.elemSize() + c];
00198             }
00199           }
00200         }
00201         pub_undistorted_image_.publish(
00202                                        cv_bridge::CvImage(
00203                                                           image_msg->header,
00204                                                           image_msg->encoding,
00205                                                           undistorted).toImageMsg());
00206       }
00207     }else{
00208       float max_degree = max_degree_;
00209       float max_radian = max_degree * 3.14159265 /180.0;
00210       float tan_max_radian = tan(max_radian);
00211       const float K = 341.656050955;
00212       const float absolute_max_degree = 85;
00213       const float absolute_max_radian = absolute_max_degree * 3.14159265 /180.0;
00214       float max_radius = max_radian * K;
00215       cv::Mat undistorted(int(l * tan_max_radian * 2 * scale_), int(l * tan_max_radian * 2 * scale_), CV_8UC3);
00216       int center_x = distorted.rows/2, center_y = distorted.cols/2;
00217       int un_center_x = undistorted.rows/(2 * scale_), un_center_y = undistorted.cols/(2*scale_);
00218 
00219       for(int i = 0; i < undistorted.rows; ++i){
00220         for(int j = 0; j < undistorted.cols; ++j){
00221           int diff_x = i/scale_-un_center_x, diff_y = j/scale_-un_center_y;
00222           float radius = sqrt(pow(diff_x, 2) + pow(diff_y, 2));
00223           float radian = atan(radius/l);
00224           if( radian < absolute_max_radian ){
00225             float multi = 0, new_x = center_x, new_y = center_y;
00226             if(radius){
00227               // float new_radius = radian * K; 
00228               multi = radian * K / radius;
00229               new_x += diff_x * multi;
00230               new_y += diff_y * multi;
00231             }
00232 
00233             for(int c = 0; c < undistorted.channels(); ++c){
00234               undistorted.data[  i * undistorted.step + j * undistorted.elemSize() + c ]
00235                 = distorted.data[ int(new_x) * distorted.step + int(new_y) * distorted.elemSize() + c];
00236             }
00237           }
00238         }
00239       }
00240       pub_undistorted_image_.publish(
00241                                      cv_bridge::CvImage(
00242                                                         image_msg->header,
00243                                                         image_msg->encoding,
00244                                                         undistorted).toImageMsg());
00245     }
00246   }
00247 }
00248 
00249 
00250 #include <pluginlib/class_list_macros.h>
00251 PLUGINLIB_EXPORT_CLASS (jsk_perception::FisheyeToPanorama, nodelet::Nodelet);


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Sun Oct 8 2017 02:43:23