write raw data to file, read raw data from file

aicp_core/include/aicp_registration/app.hpp

@@ -39,6 +39,9 @@ struct CommandLineConfig
     int reference_update_frequency;
     float max_correction_magnitude;
     bool verbose;
+    bool write_input_clouds_to_file;
+    bool process_input_clouds_from_file;
+    string process_input_clouds_folder;
 };
 
 namespace aicp {
@@ -63,6 +66,10 @@ public:
     ClassificationParams class_params_;
     int online_results_line_;
 
+    void processFromFile(std::string file_path);
+
+    void processCloud(AlignedCloudPtr cloud);
+
     // thread function doing actual work
     void operator()();
 

aicp_core/include/aicp_utils/timing.hpp

@@ -11,7 +11,7 @@ class TimingUtils{
     ~TimingUtils(){}   
 
     static void tic();
-    static void toc();
+    static void toc(std::string message = "");
     static std::string currentDateTime();
 
     static void sleepSeconds(clock_t sec);

aicp_core/src/registration/app.cpp

@@ -241,25 +241,65 @@ void App::runAicpPipeline(pcl::PointCloud<pcl::PointXYZ>::Ptr& reference_prefilt
         computeRegistration(*reference_prefiltered, *reading_prefiltered, T);
 }
 
-void App::operator()() {
-    running_ = true;
-    while (running_) {
-        std::unique_lock<std::mutex> lock(worker_mutex_);
-        // Wait for notification from planarLidarHandler
-        worker_condition_.wait_for(lock, std::chrono::milliseconds(1000));
 
-        // Copy current workload from cloud queue to work queue
-        std::list<AlignedCloudPtr> work_queue;
-        {
-            std::unique_lock<std::mutex> lock(data_mutex_);
-            while (!cloud_queue_.empty()) {
-                work_queue.push_back(cloud_queue_.front());
-                cloud_queue_.pop_front();
+
+
+
+void App::processFromFile(std::string file_path){
+
+    // Reach the input poses file:
+    std::stringstream ss;
+    ss << file_path << "/input_poses.csv";
+    ifstream in( ss.str() );
+    vector<vector<double>> fields;
+    if (in) {
+        string line;
+        while (getline(in, line)) {
+            if (line.at(0) == '#'){
+                continue;
+            }
+
+            stringstream sep(line);
+            string field;
+            fields.push_back(vector<double>());
+            while (getline(sep, field, ',')) {
+                fields.back().push_back(stod(field));
             }
         }
+    }
+
+    // Read each point cloud and consecutively feed to AICP
+    for (auto row : fields) {
+        int counter = int(row[0]);
+        int sec = int(row[1]);
+        int nsec = int(row[2]);
+        int64_t utime = 0;
+
+        Eigen::Isometry3d world_to_body_ = Eigen::Isometry3d::Identity();
+        world_to_body_.translation() << row[3],row[4],row[5];
+        world_to_body_.rotate( Eigen::Quaterniond(row[9],row[6],row[7],row[8]) );
+
+        std::stringstream ss2;
+        ss2 << file_path << "/cloud_" << counter << "_" << sec << "_" << nsec << ".pcd";
+
+        std::cout << ss2.str() << "\n";
+        pcl::PointCloud<pcl::PointXYZ>::Ptr accumulated_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+
+        if (pcl::io::loadPCDFile<pcl::PointXYZ> (ss2.str(), *accumulated_cloud) == -1){
+           std::cout << "Couldn't read file " << ss2.str() << "\n";
+           return;
+        }
+
+        AlignedCloudPtr current_cloud (new AlignedCloud(utime,
+                                                        accumulated_cloud,
+                                                        world_to_body_));
+        processCloud(current_cloud);
+    }
+}
+
+
+void App::processCloud(AlignedCloudPtr cloud){
 
-        // Process workload
-        for (auto cloud : work_queue) {
 
             // First point cloud (becomes first reference)
             if(!cl_cfg_.localize_against_prior_map &&
@@ -345,7 +385,7 @@ void App::operator()() {
                          aligned_clouds_graph_->getNbClouds() != 0)
                     {
                         cout << "[Main] -----> WRONG ALIGNMENT: DROPPED POINT CLOUD" << endl;
-                        break;
+                        return;
                     }
 
                     pcl::transformPointCloud (*read_prefiltered, *output, correction);
@@ -493,6 +533,32 @@ void App::operator()() {
                 cout << "Updates: " << updates_counter_ << endl;
             }
             cout << "--------------------------------------------------------------------------------------" << endl;
+
+
+}
+
+
+
+void App::operator()() {
+    running_ = true;
+    while (running_) {
+        std::unique_lock<std::mutex> lock(worker_mutex_);
+        // Wait for notification from planarLidarHandler
+        worker_condition_.wait_for(lock, std::chrono::milliseconds(1000));
+
+        // Copy current workload from cloud queue to work queue
+        std::list<AlignedCloudPtr> work_queue;
+        {
+            std::unique_lock<std::mutex> lock(data_mutex_);
+            while (!cloud_queue_.empty()) {
+                work_queue.push_back(cloud_queue_.front());
+                cloud_queue_.pop_front();
+            }
+        }
+
+        // Process workload
+        for (auto cloud : work_queue) {
+            processCloud(cloud);
         }
     }
 }

aicp_core/src/utils/timing.cpp

@@ -7,10 +7,10 @@ void TimingUtils::tic() {
   tictoc_stack.push(clock());   
 }
 
-void TimingUtils::toc() {
+void TimingUtils::toc(std::string message) {
   std::cout << "Time elapsed: "
   << ((double)(clock() - tictoc_stack.top())) / CLOCKS_PER_SEC
-  << " sec"
+  << " sec " << message
   << std::endl;
   tictoc_stack.pop();
 }

aicp_ros/include/aicp_ros/app_ros.hpp

@@ -24,11 +24,17 @@ public:
            const ClassificationParams& class_params);
 
     inline ~AppROS() {
+        if (input_poses_file_.is_open()) {
+            input_poses_file_.close();
+        }
+
         delete accu_;
         delete vis_ros_;
         delete talk_ros_;
     }
 
+    void writeCloudToFile(AlignedCloudPtr cloud);
+
     // Subscriber callabacks
     void velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr& laser_msg_in);
     void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg_in);
@@ -45,6 +51,9 @@ public:
 private:
     ros::NodeHandle& nh_;
 
+    std::ofstream input_poses_file_;
+    int input_clouds_counter_;
+
     Eigen::Isometry3d world_to_body_;
     Eigen::Isometry3d world_to_body_previous_;
     tf::Pose temp_tf_pose_;

aicp_ros/include/aicp_ros/velodyne_accumulator.hpp

@@ -35,7 +35,7 @@ public:
 
     uint16_t getCounter() const;
     bool getFinished() const;
-    uint64_t getFinishedTime() const;
+    int64_t getFinishedTime() const;
 
     const PointCloud& getCloud();
     void clearCloud();
@@ -53,7 +53,7 @@ private:
     // implicitly discarding intensities from the clouds
     PointCloud point_cloud_;
     PointCloud accumulated_point_cloud_;
-    uint64_t utime_;
+    int64_t utime_;
 
     ros::NodeHandle& nh_;
     ros::Subscriber lidar_sub_;

aicp_ros/launch/aicp.launch

@@ -20,6 +20,11 @@
   <arg name="saveProbs"                        default="$(find aicp_core)/data/classification/probs_opencv3.txt"/>
   <arg name="modelLocation"                    default="$(find aicp_core)/data/models/svm_opencv3.xml" />
 
+
+  <arg name="write_input_clouds_to_file"       default="false"/>
+  <arg name="process_input_clouds_from_file"   default="false"/>
+  <arg name="process_input_clouds_folder"      default="/tmp/aicp_data/"/>
+
   <!-- Run AICP (command line configuration) -->
   <node pkg="aicp_ros" type="aicp_ros_node" name="aicp_ros_node" output="screen">
 
@@ -61,6 +66,13 @@
   	<!-- Visualize and store -->
     <param name="verbose"                       value="false" />
 
+    <!-- Write out incoming data to file -->
+    <param name="write_input_clouds_to_file"    value="$(arg write_input_clouds_to_file)" />
+    <!-- Read incoming data from file -->
+    <param name="process_input_clouds_from_file" value="$(arg process_input_clouds_from_file)" />
+    <!-- Read incoming data from file -->
+    <param name="process_input_clouds_folder"   value="$(arg process_input_clouds_folder)" />
+
   </node>
 
 </launch>

aicp_ros/launch/aicp_mapping.launch

@@ -8,10 +8,19 @@
   <arg name="localize_against_prior_map"   default="false"/> <!-- ! should always be false in this launch file ! -->
   <arg name="localize_against_built_map"   default="false"/> <!-- implicit loop closure when revisit known place -->
 
+  <!-- options for post processing -->
+  <arg name="write_input_clouds_to_file"       default="false"/>
+  <arg name="process_input_clouds_from_file"   default="false"/>
+  <arg name="process_input_clouds_folder"      default="/tmp/aicp_data"/>
+
   <include file="$(find aicp_ros)/launch/aicp.launch">
     <arg name="load_map_from_file"         value="$(arg load_map_from_file)"/>
     <arg name="localize_against_prior_map" value="$(arg localize_against_prior_map)"/>
     <arg name="localize_against_built_map" value="$(arg localize_against_built_map)"/>
+
+    <arg name="write_input_clouds_to_file"     value="$(arg write_input_clouds_to_file)"/>
+    <arg name="process_input_clouds_from_file" value="$(arg process_input_clouds_from_file)"/>
+    <arg name="process_input_clouds_folder"    value="$(arg process_input_clouds_folder)"/>
   </include>
   
   <group if="$(arg load_map_from_file)">

aicp_ros/src/aicp_ros_node.cpp

@@ -31,6 +31,9 @@ int main(int argc, char** argv){
     cl_cfg.pose_body_channel = "/state_estimator/pose_in_odom";
     cl_cfg.output_channel = "/aicp/pose_corrected"; // Create new channel...
     cl_cfg.verbose = false; // enable visualization for debug
+    cl_cfg.write_input_clouds_to_file = false; // write the raw incoming point clouds to a folder, for post processing
+    cl_cfg.process_input_clouds_from_file = false;  // process raw incoming point cloud from a folder
+    cl_cfg.process_input_clouds_folder = "/tmp/aicp_data";
 
     aicp::VelodyneAccumulatorConfig va_cfg;
     va_cfg.batch_size = 80; // 240 is about 1 sweep at 5RPM // 80 is about 1 sweep at 15RPM
@@ -57,6 +60,10 @@ int main(int argc, char** argv){
     nh.getParam("pose_body_channel", cl_cfg.pose_body_channel);
     nh.getParam("output_channel", cl_cfg.output_channel);
     nh.getParam("verbose", cl_cfg.verbose);
+    nh.getParam("write_input_clouds_to_file", cl_cfg.write_input_clouds_to_file);
+    nh.getParam("process_input_clouds_from_file", cl_cfg.process_input_clouds_from_file);
+    nh.getParam("process_input_clouds_folder", cl_cfg.process_input_clouds_folder);
+
 
     nh.getParam("batch_size", va_cfg.batch_size);
     nh.getParam("min_range", va_cfg.min_range);
@@ -100,19 +107,25 @@ int main(int argc, char** argv){
                                                        overlap_params,
                                                        classification_params));
 
-    // Subscribers
-    ros::Subscriber lidar_sub = nh.subscribe(va_cfg.lidar_topic, 100, &aicp::AppROS::velodyneCallBack, app.get());
-    ros::Subscriber pose_sub = nh.subscribe(cl_cfg.pose_body_channel, 100, &aicp::AppROS::robotPoseCallBack, app.get());
-    ros::Subscriber marker_sub = nh.subscribe("/interaction_marker/pose", 100, &aicp::AppROS::interactionMarkerCallBack, app.get());
+    if (!cl_cfg.process_input_clouds_from_file){
+
+        // Subscribers
+        ros::Subscriber lidar_sub = nh.subscribe(va_cfg.lidar_topic, 100, &aicp::AppROS::velodyneCallBack, app.get());
+        ros::Subscriber pose_sub = nh.subscribe(cl_cfg.pose_body_channel, 100, &aicp::AppROS::robotPoseCallBack, app.get());
+        ros::Subscriber marker_sub = nh.subscribe("/interaction_marker/pose", 100, &aicp::AppROS::interactionMarkerCallBack, app.get());
 
-    // Advertise services (using service published by anybotics icp_tools ui)
-    ros::ServiceServer load_map_server_ = nh.advertiseService("/icp_tools/load_map_from_file", &aicp::AppROS::loadMapFromFileCallBack, app.get());
-    ros::ServiceServer go_back_server_ = nh.advertiseService("/aicp/go_back_request", &aicp::AppROS::goBackRequestCallBack, app.get());
+        // Advertise services (using service published by anybotics icp_tools ui)
+        ros::ServiceServer load_map_server_ = nh.advertiseService("/icp_tools/load_map_from_file", &aicp::AppROS::loadMapFromFileCallBack, app.get());
+        ros::ServiceServer go_back_server_ = nh.advertiseService("/aicp/go_back_request", &aicp::AppROS::goBackRequestCallBack, app.get());
 
-    ROS_INFO_STREAM("[Aicp] Waiting for input messages...");
+        ROS_INFO_STREAM("[Aicp] Waiting for input messages...");
 
-    app->run();
-    ros::spin();
+        app->run();
+        ros::spin();
+
+    }else{
+        app->processFromFile(cl_cfg.process_input_clouds_folder);
+    }
 
     return 0;
 }

aicp_ros/src/app_ros.cpp

@@ -49,6 +49,19 @@ AppROS::AppROS(ros::NodeHandle &nh,
         alignability_pub_ = nh_.advertise<std_msgs::Float32>("/aicp/alignability",10);
         risk_pub_ = nh_.advertise<std_msgs::Float32>("/aicp/alignment_risk",10);
     }
+
+    // Write the incoming data to file. This should be false when running live
+    if (cl_cfg_.write_input_clouds_to_file)
+    {
+        std::stringstream input_poses_filename;
+        input_poses_filename << data_directory_path_.str() << "/input_poses.csv";
+
+        input_poses_file_.open (input_poses_filename.str().c_str() );
+        input_poses_file_ << "# counter, sec, nsec, x, y, z, qx, qy, qz, qw\n";
+        input_poses_file_.flush();
+        input_clouds_counter_ = 0;
+    }
+
 }
 
 void AppROS::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &pose_msg_in)
@@ -134,6 +147,33 @@ void AppROS::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedCon
     pose_initialized_ = true;
 }
 
+
+void AppROS::writeCloudToFile(AlignedCloudPtr cloud){
+    // Extract the data from AlignedCloud
+    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud = cloud->getCloud();
+    Eigen::Isometry3d pose = cloud->getPriorPose();
+    Eigen::Quaterniond quat(pose.rotation());
+    int64_t utime = cloud->getUtime();
+    int64_t sec = floor(utime * 1E-6);
+    int64_t nsec = utime - sec* 1E6;
+
+    // Write the base-to-odom estimate
+    std::stringstream ss;
+    ss << input_clouds_counter_ << ", " << sec << ", " << nsec << ", ";
+    ss << pose.translation().x() << ", " << pose.translation().y() << ", " << pose.translation().z() << ", ";
+    ss << quat.x() << ", " << quat.y() << ", " << quat.z() << ", " << quat.w();
+    input_poses_file_ << ss.str() << "\n";
+    input_poses_file_.flush();
+
+    // Write the point cloud
+    std::stringstream ss2;
+    ss2 << data_directory_path_.str() << "/cloud_" << input_clouds_counter_ << "_" << sec << "_" << nsec << ".pcd";
+    pcd_writer_.write<pcl::PointXYZ> (ss2.str (), *point_cloud, true);
+
+    input_clouds_counter_++;
+}
+
+
 void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_msg_in){
     if (!pose_initialized_){
         ROS_WARN_STREAM("[Aicp] Pose not initialized, waiting for pose prior...");
@@ -190,8 +230,12 @@ void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_ms
                                                                 world_to_body_));
                 world_to_body_previous_ = world_to_body_;
 
+                if (cl_cfg_.write_input_clouds_to_file)
+                    writeCloudToFile(current_cloud);
+
                 // Stack current cloud into queue
                 cloud_queue_.push_back(current_cloud);
+                //cout << "[App ROS] cloud_queue_ size: " << cloud_queue_.size() << endl;
 
                 if (cloud_queue_.size() > max_queue_size) {
                     cout << "|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||\n";

aicp_ros/src/velodyne_accumulator.cpp

@@ -91,7 +91,7 @@ bool VelodyneAccumulatorROS::getFinished() const {
     return finished_;
 }
 
-uint64_t VelodyneAccumulatorROS::getFinishedTime() const{
+int64_t VelodyneAccumulatorROS::getFinishedTime() const{
     return utime_;
 }