38 #include <pcl/common/centroid.h> 46 float max_degree = 90;
47 float max_radian = max_degree * 3.14159265 /180.0;
48 float tan_max_radian =
tan(max_radian);
50 const float absolute_max_degree = 85;
51 const float absolute_max_radian = absolute_max_degree * 3.14159265 /180.0;
52 float max_radius = max_radian * K;
55 int center_x = shrink_distorted.rows/2, center_y = shrink_distorted.cols/2;
57 pcl::PointCloud<pcl::PointXYZRGB> pointcloud;
59 for(
int i = 0; i < shrink_distorted.rows; ++i){
60 for(
int j = 0; j < shrink_distorted.cols; ++j){
61 float radius =
sqrt((i-center_x)*(i-center_x) + (j-center_y)*(j-center_y));
62 float radian = radius/K;
63 if( radian < max_radian ){
65 float rate = l/radius;
68 p.x = -(i - center_x) * rate;
69 p.y = (j - center_y) * rate;
71 p.r = shrink_distorted.data[ int(i) * shrink_distorted.step + int(j) * shrink_distorted.elemSize() + 2];
72 p.g = shrink_distorted.data[ int(i) * shrink_distorted.step + int(j) * shrink_distorted.elemSize() + 1];
73 p.b = shrink_distorted.data[ int(i) * shrink_distorted.step + int(j) * shrink_distorted.elemSize() + 0];
74 pointcloud.push_back(p);
79 sensor_msgs::PointCloud2 pc2;
81 pc2.header.frame_id =
"fisheye";
105 DiagnosticNodelet::onInit();
111 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
112 dynamic_reconfigure::Server<Config>::CallbackType
f =
114 srv_->setCallback (f);
void publish(const boost::shared_ptr< M > &message) const
FisheyeSphereConfig Config
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::FisheyeSpherePublisher, nodelet::Nodelet)
virtual void configCallback(Config &config, uint32_t level)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
virtual void extract(const sensor_msgs::ImageConstPtr &input)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
ros::Subscriber sub_image_
ros::Publisher pub_sphere_
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
virtual void unsubscribe()