add extract_image_positions

Author: miquelmassot

bag_tools/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(bag_tools)
 
-find_package(catkin REQUIRED COMPONENTS rospy rosbag sensor_msgs cv_bridge message_filters image_proc stereo_image_proc image_geometry camera_calibration_parsers)
+find_package(catkin REQUIRED COMPONENTS rospy rosbag sensor_msgs cv_bridge message_filters image_proc stereo_image_proc image_geometry camera_calibration_parsers auv_msgs)
 find_package(Boost REQUIRED COMPONENTS signals thread)
 find_package(OpenCV REQUIRED)
 find_package(console_bridge REQUIRED)
@@ -14,10 +14,12 @@ catkin_package(
 include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${console_bridge_INCLUDE_DIRS})
 
 add_executable(extract_images src/extract_images.cpp)
+add_executable(extract_image_positions src/extract_image_positions.cpp)
 add_executable(extract_stereo_images src/extract_stereo_images.cpp)
 add_executable(process_stereo src/process_stereo.cpp)
 
 target_link_libraries(extract_images ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES})
+target_link_libraries(extract_image_positions ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES})
 target_link_libraries(extract_stereo_images ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES})
 target_link_libraries(process_stereo ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES})
 

bag_tools/src/extract_image_positions.cpp

@@ -0,0 +1,109 @@
+#include <iostream>
+#include <fstream>
+
+#include <ros/ros.h>
+#include <cv_bridge/cv_bridge.h>
+#include <tf/transform_listener.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <image_transport/subscriber_filter.h>
+#include <sensor_msgs/image_encodings.h>
+#include <auv_msgs/NavSts.h>
+#include <image_transport/image_transport.h>
+#include <opencv2/opencv.hpp>
+
+ // Topic sync
+  typedef message_filters::sync_policies::ApproximateTime<auv_msgs::NavSts,
+                                                          sensor_msgs::Image,
+                                                          sensor_msgs::CameraInfo> SyncPolicy;
+  typedef message_filters::Synchronizer<SyncPolicy> Sync;
+
+class ImagePositions {
+  ros::NodeHandle nh_;
+  image_transport::ImageTransport it_;
+  image_transport::CameraSubscriber cam_sub_;
+  image_transport::CameraSubscriber sub_;
+  tf::TransformListener tf_listener_;
+  std::string parent_frame_id_;
+  std::string child_frame_id_;
+  std::string output_file_;
+  std::string output_folder_;
+  std::ofstream file_;
+  int count_;
+
+public:
+  ImagePositions(const std::vector<std::string>& frame_ids)
+    : it_(nh_), count_(0){
+    nh_.getParam("parent_frame_id", parent_frame_id_);
+    nh_.getParam("child_frame_id", child_frame_id_);
+    nh_.getParam("output_file", output_file_);
+    nh_.getParam("output_folder", output_folder_);
+    std::string fn(output_folder_ + "/" + output_file_);
+    file_.open(fn.c_str());
+    file_ << "name, lat, lon, depth, altitude, roll, pitch, yaw\n";
+    std::string camera_topic = nh_.resolveName("camera");
+    std::string nav_sts_topic = nh_.resolveName("nav_sts");
+
+    image_transport::SubscriberFilter image_sub;
+    message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub;
+    message_filters::Subscriber<auv_msgs::NavSts> nav_sts_sub;
+
+    // Message sync
+    boost::shared_ptr<Sync> sync;
+    nav_sts_sub.subscribe(nh_, nav_sts_topic, 20);
+    image_sub  .subscribe(it_, camera_topic + "/image_rect_color", 5);
+    info_sub   .subscribe(nh_, camera_topic + "/camera_info", 5);
+    sync.reset(new Sync(SyncPolicy(10), nav_sts_sub, image_sub, info_sub) );
+    sync->registerCallback(boost::bind(&ImagePositions::msgsCallback, this, _1, _2, _3));
+  }
+
+  ~ImagePositions() {
+    file_.close();
+  }
+
+  void msgsCallback(const auv_msgs::NavStsConstPtr& nav_sts_msg,
+                    const sensor_msgs::ImageConstPtr& image_msg,
+                    const sensor_msgs::CameraInfoConstPtr& info_msg) {
+    cv::Mat image;
+    cv_bridge::CvImagePtr input_bridge;
+    try {
+      input_bridge = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
+      image = input_bridge->image;
+    }
+    catch (cv_bridge::Exception& ex){
+      ROS_ERROR("[ImagePositions] Failed to convert image");
+      return;
+    }
+
+    double lat      = nav_sts_msg->position.north;
+    double lon      = nav_sts_msg->position.east;
+    double depth    = nav_sts_msg->position.depth;
+    double altitude = nav_sts_msg->altitude;
+    double roll     = nav_sts_msg->orientation.roll;
+    double pitch    = nav_sts_msg->orientation.pitch;
+    double yaw      = nav_sts_msg->orientation.yaw;
+
+    std::stringstream ss;
+    ss << std::setw(6) << std::setfill('0') << count_;
+    std::string name = ss.str() + ".png";
+
+    ROS_INFO_STREAM("[ImagePositions]: Saving image " << name);
+    cv::imwrite(output_folder_ + "/" + name, image);
+
+    file_ << name      << ", "
+          << lat       << ", "
+          << lon       << ", "
+          << depth     << ", "
+          << altitude  << ", "
+          << roll      << ", "
+          << pitch     << ", "
+          << yaw       << "\n";
+  }
+};
+
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "image_positions");
+  ImagePositions ip();
+  ros::spin();
+}