39 #include <sensor_msgs/Image.h> 43 #include <boost/assign.hpp> 51 DiagnosticNodelet::onInit();
59 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
60 dynamic_reconfigure::Server<Config>::CallbackType
f =
62 srv_->setCallback (f);
95 int l = distorted.rows / 2;
98 float min_degree = 30;
99 float min_radian = min_degree * 3.14159265 /180.0;
100 float tan_min_radian =
tan(min_radian);
101 const float K = 341.656050955;
103 cv::Mat undistorted(l, l*4, CV_8UC3);
104 cv::Mat undistorted_bilinear(l, l*4, CV_8UC3);
105 for(
int i = 0; i < undistorted.rows; ++i){
106 for(
int j = 0; j < undistorted.cols; ++j){
109 double theta = 2.0 *
PI * (double)(-j) / (double)(4.0 * l);
110 double fTrueX = radius *
cos(theta);
111 double fTrueY = radius *
sin(theta);
113 int x = (
int)round(fTrueX) + l;
114 int y = l - (
int)round(fTrueY);
115 if (x >= 0 && x < (2 * l) && y >= 0 && y < (2 * l))
117 for(
int c = 0;
c < undistorted.channels(); ++
c)
118 undistorted.data[ i * undistorted.step + j * undistorted.elemSize() +
c ]
119 = distorted.data[x * distorted.step + y * distorted.elemSize() +
c];
122 fTrueX = fTrueX + (double)l;
123 fTrueY = (double)l - fTrueY;
125 int iFloorX = (
int)floor(fTrueX);
126 int iFloorY = (
int)floor(fTrueY);
127 int iCeilingX = (
int)ceil(fTrueX);
128 int iCeilingY = (
int)ceil(fTrueY);
130 if (iFloorX < 0 || iCeilingX < 0 ||
131 iFloorX >= (2 * l) || iCeilingX >= (2 * l) ||
132 iFloorY < 0 || iCeilingY < 0 ||
133 iFloorY >= (2 * l) || iCeilingY >= (2 * l))
continue;
135 double fDeltaX = fTrueX - (double)iFloorX;
136 double fDeltaY = fTrueY - (double)iFloorY;
138 cv::Mat clrTopLeft(1,1, CV_8UC3), clrTopRight(1,1, CV_8UC3), clrBottomLeft(1,1, CV_8UC3), clrBottomRight(1,1, CV_8UC3);
139 for(
int c = 0;
c < undistorted.channels(); ++
c){
140 clrTopLeft.data[
c ] = distorted.data[iFloorX * distorted.step + iFloorY * distorted.elemSize() +
c];
141 clrTopRight.data[
c ] = distorted.data[iCeilingX * distorted.step + iFloorY * distorted.elemSize() +
c];
142 clrBottomLeft.data[
c ] = distorted.data[iFloorX * distorted.step + iCeilingY * distorted.elemSize() +
c];
143 clrBottomRight.data[
c ] = distorted.data[iCeilingX * distorted.step + iCeilingY * distorted.elemSize() +
c];
146 double fTop0 =
interpolate(fDeltaX, clrTopLeft.data[0], clrTopRight.data[0]);
147 double fTop1 =
interpolate(fDeltaX, clrTopLeft.data[1], clrTopRight.data[1]);
148 double fTop2 =
interpolate(fDeltaX, clrTopLeft.data[2], clrTopRight.data[2]);
149 double fBottom0 =
interpolate(fDeltaX, clrBottomLeft.data[0], clrBottomRight.data[0]);
150 double fBottom1 =
interpolate(fDeltaX, clrBottomLeft.data[1], clrBottomRight.data[1]);
151 double fBottom2 =
interpolate(fDeltaX, clrBottomLeft.data[2], clrBottomRight.data[2]);
157 i0 = std::min(255, std::max(i0, 0));
158 i1 = std::min(255, std::max(i1, 0));
159 i2 = std::min(255, std::max(i2, 0));
161 undistorted_bilinear.data[ i * undistorted_bilinear.step + j * undistorted_bilinear.elemSize() + 0] = i0;
162 undistorted_bilinear.data[ i * undistorted_bilinear.step + j * undistorted_bilinear.elemSize() + 1] = i1;
163 undistorted_bilinear.data[ i * undistorted_bilinear.step + j * undistorted_bilinear.elemSize() + 2] = i2;
177 cv::Mat undistorted(
int(l * 1.0 / tan_min_radian*
scale_),
int(l * 2.0 *
PI * scale_), CV_8UC3);
178 int center_x = distorted.rows/2, center_y = distorted.cols/2;
181 for(
int i = 0; i < undistorted.rows; ++i){
182 for(
int j = 0; j < undistorted.cols; ++j){
185 phi =
atan(l * 1.0 /i*scale_) + 0.5;
186 float theta = (j-
int(undistorted.cols/2))/scale_ * 1.0/l;
187 int x = K * phi *
cos(theta) + center_x;
188 int y = K * phi *
sin(theta) + center_y;
189 for(
int c = 0;
c < undistorted.channels(); ++
c){
190 int index = undistorted.rows - 1 - i;
193 int jndex = j + offset_jndex;
194 if(jndex > undistorted.cols - 1)
195 jndex -= undistorted.cols - 1;
196 undistorted.data[ index * undistorted.step + jndex * undistorted.elemSize() +
c ]
197 = distorted.data[ x * distorted.step + y * distorted.elemSize() +
c];
209 float max_radian = max_degree * 3.14159265 /180.0;
210 float tan_max_radian =
tan(max_radian);
211 const float K = 341.656050955;
212 const float absolute_max_degree = 85;
213 const float absolute_max_radian = absolute_max_degree * 3.14159265 /180.0;
214 float max_radius = max_radian * K;
215 cv::Mat undistorted(
int(l * tan_max_radian * 2 *
scale_),
int(l * tan_max_radian * 2 * scale_), CV_8UC3);
216 int center_x = distorted.rows/2, center_y = distorted.cols/2;
217 int un_center_x = undistorted.rows/(2 *
scale_), un_center_y = undistorted.cols/(2*scale_);
219 for(
int i = 0; i < undistorted.rows; ++i){
220 for(
int j = 0; j < undistorted.cols; ++j){
221 int diff_x = i/scale_-un_center_x, diff_y = j/scale_-un_center_y;
223 float radian =
atan(radius/l);
224 if( radian < absolute_max_radian ){
225 float multi = 0, new_x = center_x, new_y = center_y;
228 multi = radian * K / radius;
229 new_x += diff_x * multi;
230 new_y += diff_y * multi;
233 for(
int c = 0;
c < undistorted.channels(); ++
c){
234 undistorted.data[ i * undistorted.step + j * undistorted.elemSize() +
c ]
235 = distorted.data[
int(new_x) * distorted.step +
int(new_y) * distorted.elemSize() +
c];
void publish(const boost::shared_ptr< M > &message) const
jsk_perception::FisheyeConfig Config
virtual void rectify(const sensor_msgs::Image::ConstPtr &image_msg)
ros::Publisher pub_undistorted_bilinear_image_
std::vector< std::string > V_string
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
ros::Publisher pub_undistorted_image_
PLUGINLIB_EXPORT_CLASS(jsk_perception::FisheyeToPanorama, nodelet::Nodelet)
ros::Subscriber sub_image_
double interpolate(double rate, double first, double second)
virtual void unsubscribe()
void configCallback(Config &new_config, uint32_t level)
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
sensor_msgs::ImagePtr toImageMsg() const