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