00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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;
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
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);