36 #include <sensor_msgs/Image.h> 40 #include <opencv2/core/core.hpp> 41 #include <opencv2/highgui/highgui.hpp> 75 std::string temp_filename;
77 "/normalization_image.png";
84 ROS_ERROR(
"Planning to write normalization image to: %s",
97 void image_cb(
const sensor_msgs::ImageConstPtr& msg)
99 if (image_count_ >= max_num_to_average_)
105 if (raw_count_++ % num_to_skip_ == 0)
110 max_num_to_average_);
113 cv::Mat image(im_ptr->image);
114 image_array_.push_back(image);
115 if (image_count_ >= max_num_to_average_)
126 if (!norm_im.empty())
130 cv::imwrite(filename_, norm_im);
132 catch (
const std::exception& e)
134 ROS_ERROR(
"Failed to save the normalization image: %s",
138 ROS_ERROR(
"Successfully wrote normalization image to: %s",
141 image_written_ =
true;
145 ROS_ERROR(
"Failed to generate a normalization image");
153 max_num_to_average_(100),
156 image_written_(false)
159 "/normalization_image.png";
168 if (!image_written_ && image_array_.size() > 25)
171 fprintf(stderr,
"\nNode killed before enough frames received to generate " 172 "normalized image, so a normalized image was generated with " 173 "available frames (%d vs. %d)\n",
175 max_num_to_average_);
177 else if (!image_written_)
179 fprintf(stderr,
"\nNode killed before enough frames received to generate " 180 "normalized image: Too few frames in the buffer to generate a " 181 "normalization image\n");
185 fprintf(stderr,
"\nExiting normally\n");
195 int main(
int argc,
char **argv)
197 ros::init(argc, argv,
"image_normalization_node");
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int32_t max_num_to_average_
ROSCPP_DECL const std::string & getName()
void generate_and_write_image()
ROSCPP_DECL void spin(Spinner &spinner)
cv::Mat generate_normalization_image(const std::vector< cv::Mat > &image_list)
Computes a best estimate of a normalization image from a vector of images.
NormalizationImageNode(const ros::NodeHandle &nh)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void subscribe_to_topics()
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
int main(int argc, char **argv)
std::vector< cv::Mat > image_array_
ROSLIB_DECL std::string getPath(const std::string &package_name)
ros::Subscriber image_sub_
void image_cb(const sensor_msgs::ImageConstPtr &msg)