add CalibProc

CMakeLists.txt

@@ -22,6 +22,7 @@ find_package(OpenCV)
 generate_dynamic_reconfigure_options(
     cfg/FlirGigeDyn.cfg
     cfg/ThermalProcDyn.cfg
+    cfg/CalibProcDyn.cfg
     )
 
 catkin_package(
@@ -46,6 +47,7 @@ add_library(${PROJECT_NAME}
     src/flir_gige/flir_gige_nodelet.cpp
     src/thermal_proc/thermal_proc_node.cpp
     src/thermal_proc/thermal_proc_nodelet.cpp
+    src/calib_proc/calib_proc_node.cpp
     )
 target_link_libraries(${PROJECT_NAME}
     ${catkin_LIBRARIES}
@@ -66,6 +68,12 @@ add_executable(thermal_proc_node
 target_link_libraries(thermal_proc_node
     ${PROJECT_NAME}
     )
+add_executable(calib_proc_node
+    src/calib_proc/calib_proc_main.cpp
+    )
+target_link_libraries(calib_proc_node
+    ${PROJECT_NAME}
+    )
 
 ## Add cmake target dependencies of the executable/library
 add_dependencies(${PROJECT_NAME}

calib/draw_circles_grid.py

@@ -0,0 +1,17 @@
+__author__ = 'chao'
+
+import matplotlib.pyplot as plt
+
+fig = plt.gcf()
+n_cols = 5
+n_rows = 4
+r = 0.2
+for x in range(0, n_cols):
+    for y in range(0, n_rows):
+        circle = plt.Circle((x, y), r, color='k')
+        fig.gca().add_artist(circle)
+fig.gca().set_aspect('equal')
+fig.gca().axis([-1.5, n_cols + 0.5, -1.5, n_rows + 0.5])
+plt.show()
+
+fig.savefig('circles_grid.png')

cfg/CalibProcDyn.cfg

@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+PACKAGE = "flir_gige"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# name / type / level / description / default / min / max
+# Flir A35 has a maximum resolution of 320 x 256
+gen.add("thresh_window", int_t, 0,
+        "adaptive threshold window size", 11, 3, 100)
+size_enum = gen.enum([gen.const("mean", int_t, 0, "Adaptive thresh mean"),
+                      gen.const("gaussian", int_t, 1, "Adaptive thresh gaussian")],
+                     "Adaptive thresh type ")
+gen.add("thresh_type", int_t, 0,
+        "Adaptive thresh type", 0, 0, 1, edit_method=size_enum)
+
+exit(gen.generate(PACKAGE, "flir_gige", "CalibProcDyn"))

include/calib_proc/calib_proc_node.h

@@ -0,0 +1,40 @@
+#ifndef FLIR_GIGE_CALIB_PROC_NODE_H_
+#define FLIR_GIGE_CALIB_PROC_NODE_H_
+
+#include <mutex>
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <image_transport/image_transport.h>
+#include <dynamic_reconfigure/server.h>
+#include <flir_gige/CalibProcDynConfig.h>
+
+#include <opencv2/core/core.hpp>
+
+namespace flir_gige {
+
+class CalibProcNode {
+ public:
+  CalibProcNode(const ros::NodeHandle &nh, const ros::NodeHandle &pnh);
+
+ private:
+  void ImageCb(const sensor_msgs::ImageConstPtr &image_msg);
+  void ConnectCb();
+  void ConfigCb(CalibProcDynConfig &config, int level);
+
+  ros::NodeHandle nh_;
+  ros::NodeHandle pnh_;
+  image_transport::ImageTransport it_;
+  image_transport::Subscriber sub_image_;
+  image_transport::Publisher pub_calib_;
+  std::mutex connect_mutex_;
+  dynamic_reconfigure::Server<CalibProcDynConfig> cfg_server_;
+  CalibProcDynConfig config_;
+};
+
+void DetectAndDrawCriclesGrid(const cv::Mat &src, const cv::Size &size,
+                              cv::Mat &disp);
+
+}  // namespace flir_gige
+
+#endif  // FLIR_GIGE_CALIB_PROC_NODE_H_

include/thermal_proc/thermal_proc_node.h.autosave

@@ -0,0 +1,50 @@
+#ifndef FLIR_GIGE_THERMAL_PROC_NODE_H_
+#define FLIR_GIGE_THERMAL_PROC_NODE_H_
+
+#include <cstdint>
+#include <mutex>
+
+#include <ros/ros.h>
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <dynamic_reconfigure/server.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "flir_gige/planck.h"
+#include "flir_gige/ThermalProcDynConfig.h"
+
+namespace flir_gige {
+
+class ThermalProcNode {
+ public:
+  ThermalProcNode(const ros::NodeHandle &nh, const ros::NodeHandle &pnh);
+
+ private:
+  void CameraCb(const sensor_msgs::ImageConstPtr &image_msg,
+                const sensor_msgs::CameraInfoConstPtr &cinfo_msg);
+  void ConfigCb(ThermalProcDynConfig &config, int level);
+  void ConnectCb();
+
+  void RawToJet(const cv::Mat &raw, const Planck &planck, cv::Mat *color) const;
+
+  ros::NodeHandle nh_, pnh_;
+  image_transport::ImageTransport it_;
+  image_transport::CameraSubscriber sub_camera_;
+  image_transport::Publisher pub_proc_;
+  dynamic_reconfigure::Server<ThermalProcDynConfig> cfg_server_;
+  std::mutex connect_mutex_;
+  ThermalProcDynConfig config_;
+};
+
+Planck GetPlanck(const sensor_msgs::CameraInfo &cinfo_msg);
+
+void RawToHeat(const cv::Mat &raw, const Planck &planck, cv::Mat *heat);
+}  // namespace flir_gige
+
+#endif  // FLIR_GIGE_THERMAL_PROC_NODE_H_

launch/test_calib.launch

@@ -0,0 +1,28 @@
+<launch>
+    <arg name="output" default="screen"/>
+    <arg name="calib" default="false"/>
+    <arg name="camera" default="flir_gige"/>
+
+    <node pkg="rosbag" type="play" name="player" output="$(arg output)"
+        args="-l /home/chao/Workspace/bag/flir/flir_gige_2.bag">
+    </node>
+
+    <node pkg="flir_gige" type="calib_proc_node" name="calib_proc"
+        ns="$(arg camera)" output="screen">
+        <param name="thresh_window" type="int" value="17"/>
+        <param name="thresh_type" type="int" value="0"/>
+    </node>
+
+    <node pkg="image_view" type="image_view" name="viewr"
+        args="image:=/$(arg camera)/image_calib">
+    </node>
+
+    <node if="$(arg calib)" pkg="camera_calibration" type="cameracalibrator.py"
+        name="calibrator"
+        args="camera:=/$(arg camera) image:=/$(arg camera)/image_calib
+        --pattern circles --size 5x4 --square 0.028 --no-service-check">
+    </node>
+
+    <node pkg="rqt_reconfigure" type="rqt_reconfigure" name="reconfigure">
+    </node>
+</launch>

src/calib_proc/calib_proc_main.cpp

@@ -0,0 +1,15 @@
+#include "calib_proc/calib_proc_node.h"
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "calib_proc");
+  ros::NodeHandle nh;
+  ros::NodeHandle pnh("~");
+
+  try {
+    flir_gige::CalibProcNode calib_proc_node(nh, pnh);
+    ros::spin();
+  }
+  catch (const std::exception &e) {
+    ROS_ERROR("%s: %s", nh.getNamespace().c_str(), e.what());
+  }
+}

src/calib_proc/calib_proc_node.cpp

@@ -0,0 +1,96 @@
+#include "calib_proc/calib_proc_node.h"
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+
+namespace flir_gige {
+
+CalibProcNode::CalibProcNode(const ros::NodeHandle &nh,
+                             const ros::NodeHandle &pnh)
+    : nh_(nh), pnh_(pnh), it_(nh), cfg_server_(pnh) {
+  image_transport::SubscriberStatusCallback connect_cb =
+      boost::bind(&CalibProcNode::ConnectCb, this);
+  pub_calib_ = it_.advertise("image_calib", 1, connect_cb, connect_cb);
+  cfg_server_.setCallback(boost::bind(&CalibProcNode::ConfigCb, this, _1, _2));
+}
+
+void CalibProcNode::ConnectCb() {
+  std::lock_guard<std::mutex> lock(connect_mutex_);
+  if (!pub_calib_.getNumSubscribers()) {
+    sub_image_.shutdown();
+  } else if (!sub_image_) {
+    image_transport::TransportHints hints("raw", ros::TransportHints(), nh_);
+    sub_image_ =
+        it_.subscribe("image_raw", 2, &CalibProcNode::ImageCb, this, hints);
+  }
+}
+
+void CalibProcNode::ConfigCb(CalibProcDynConfig &config, int level) {
+  if (level < 0) {
+    ROS_INFO("%s: initialize dynamic reconfigure server",
+             pnh_.getNamespace().c_str());
+  }
+  if (!(config.thresh_window % 2)) {
+    config.thresh_window += 1;
+  }
+  config_ = config;
+}
+
+void CalibProcNode::ImageCb(const sensor_msgs::ImageConstPtr &image_msg) {
+  cv::Mat image = cv_bridge::toCvCopy(image_msg, image_msg->encoding)->image;
+  //  cv::imshow("image", image);
+
+  cv::Mat inverted;
+  cv::bitwise_not(image, inverted);
+  //  cv::imshow("inverted", inverted);
+
+  /*
+  cv::Mat thresh;
+  cv::adaptiveThreshold(image, thresh, 255, config_.thresh_type,
+                        cv::THRESH_BINARY, config_.thresh_window, 0);
+  cv::imshow("thresh", thresh);
+  */
+
+  /*
+   std::vector<std::vector<cv::Point>> contours;
+   cv::findContours(raw, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+
+   cv::Mat contour_image;
+   cv::cvtColor(raw, contour_image, CV_GRAY2BGR);
+   for (size_t i = 0; i < contours.size(); ++i) {
+     cv::drawContours(contour_image, contours, i, cv::Scalar(255, 0, 0), 2, 8);
+   }
+   cv::imshow("contour", contour_image);
+   */
+
+  // Detect circles grid
+  cv::Mat display;
+  DetectAndDrawCriclesGrid(inverted, cv::Size(5, 4), display);
+
+  cv::Mat calib(inverted);
+
+  // Display
+  cv::imshow("display", display);
+  cv::waitKey(1);
+
+  // Publish processed image
+  cv_bridge::CvImage cvimg_calib(image_msg->header, image_msg->encoding, calib);
+  pub_calib_.publish(cvimg_calib.toImageMsg());
+}
+
+void DetectAndDrawCriclesGrid(const cv::Mat &src, const cv::Size &size,
+                              cv::Mat &disp) {
+  std::vector<cv::Point2f> centers;
+  bool found = cv::findCirclesGrid(src, size, centers);
+  if (disp.empty()) {
+    disp = src.clone();
+  }
+  if (disp.channels() == 1) {
+    cv::cvtColor(disp, disp, CV_GRAY2BGR);
+  }
+  cv::drawChessboardCorners(disp, size, cv::Mat(centers), found);
+}
+}  // namespace flir_gige

src/thermal_proc/thermal_proc_node.cpp

@@ -9,7 +9,7 @@ namespace flir_gige {
 
 ThermalProcNode::ThermalProcNode(const ros::NodeHandle &nh,
                                  const ros::NodeHandle &pnh)
-    : nh_(nh), it_(nh), cfg_server_(pnh) {
+    : nh_(nh), pnh_(pnh), it_(nh), cfg_server_(pnh) {
   image_transport::SubscriberStatusCallback connect_cb =
       boost::bind(&ThermalProcNode::ConnectCb, this);
   pub_proc_ = it_.advertise("image_proc", 1, connect_cb, connect_cb);