Minnor changes

Author: plnegre

bag_tools/launch/extract_images.launch

@@ -1,5 +1,5 @@
 <!-- launch file for image extraction from a bagfile
-     
+
      this launch does a playback of a bagfile,
      runs image processing to get coloured rectified images
      from raw images, and saves the images to harddrive.
@@ -15,14 +15,25 @@
 -->
 
 <launch>
-  <arg name="bagfile" />
-  <arg name="camera" default="camera_flex_wide/left" />
+  <param name="/use_sim_time" value="true"/>
+  <arg name="camera" default="/stereo_down/left" />
   <arg name="image" default="image_rect_color" />
   <arg name="format" default="frame%06i.png" />
-  <arg name="rate" default="1.0" />
+  <arg name="rate" default="50.0" />
+
+  <arg name="bagfile" default="/home/plnegre/workspace/ros/data/bagfiles/2015_Valldemossa/06/2015-06-08-20-28-51_0.bag
+                               /home/plnegre/workspace/ros/data/bagfiles/2015_Valldemossa/06/2015-06-08-20-30-18_1.bag
+                               /home/plnegre/workspace/ros/data/bagfiles/2015_Valldemossa/06/2015-06-08-20-31-46_2.bag
+                               /home/plnegre/workspace/ros/data/bagfiles/2015_Valldemossa/06/2015-06-08-20-33-14_3.bag
+                               /home/plnegre/workspace/ros/data/bagfiles/2015_Valldemossa/06/2015-06-08-20-34-41_4.bag
+                               /home/plnegre/workspace/ros/data/bagfiles/2015_Valldemossa/06/2015-06-08-20-36-09_5.bag
+                               /home/plnegre/workspace/ros/data/bagfiles/2015_Valldemossa/06/2015-06-08-20-37-36_6.bag
+                               /home/plnegre/workspace/ros/data/bagfiles/2015_Valldemossa/06/2015-06-08-20-39-04_7.bag
+                               /home/plnegre/workspace/ros/data/bagfiles/2015_Valldemossa/06/2015-06-08-20-40-31_8.bag
+                               /home/plnegre/workspace/ros/data/bagfiles/2015_Valldemossa/06/2015-06-08-20-41-59_9.bag"/>
 
   <!-- playback the data -->
-  <node pkg="rosbag" type="play" name="player" args="-d 1 -r $(arg rate) $(arg bagfile)" output="screen" required="true" />
+  <node pkg="rosbag" type="play" name="player" args="-d 1 -r $(arg rate) $(arg bagfile)" required="true" />
 
   <!-- run the image processing (de-bayering, undistortion, rectification) -->
   <node pkg="image_proc" type="image_proc" name="image_proc" ns="$(arg camera)" />

bag_tools/scripts/change_camera_info.py

@@ -75,10 +75,12 @@ def replacement(replace_string):
   cam_info.R = calib_data['rectification_matrix']['data']
   cam_info.P = calib_data['projection_matrix']['data']
   cam_info.distortion_model = calib_data['distortion_model']
-  cam_info.binning_x = 1;
-  cam_info.binning_y = 1;
-  cam_info.roi.width = calib_data['image_width']
-  cam_info.roi.height = calib_data['image_height']
+  cam_info.binning_x = calib_data['binning_x']
+  cam_info.binning_y = calib_data['binning_y']
+  cam_info.roi.width = calib_data['roi_image_width']
+  cam_info.roi.height = calib_data['roi_image_height']
+  cam_info.roi.x_offset = calib_data['roi_x_offset']
+  cam_info.roi.y_offset = calib_data['roi_y_offset']
   return pair[0], cam_info
 
 if __name__ == "__main__":

bag_tools/scripts/check_delay.py

@@ -12,9 +12,9 @@
     * Redistributions in binary form must reproduce the above copyright
       notice, this list of conditions and the following disclaimer in the
       documentation and/or other materials provided with the distribution.
-    * Neither the name of Systems, Robotics and Vision Group, University of 
-      the Balearican Islands nor the names of its contributors may be used to 
-      endorse or promote products derived from this software without specific 
+    * Neither the name of Systems, Robotics and Vision Group, University of
+      the Balearican Islands nor the names of its contributors may be used to
+      endorse or promote products derived from this software without specific
       prior written permission.
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND

bag_tools/scripts/stereo_sequence_publisher.py

@@ -15,9 +15,9 @@
     * Redistributions in binary form must reproduce the above copyright
       notice, this list of conditions and the following disclaimer in the
       documentation and/or other materials provided with the distribution.
-    * Neither the name of Systems, Robotics and Vision Group, University of 
-      the Balearican Islands nor the names of its contributors may be used to 
-      endorse or promote products derived from this software without specific 
+    * Neither the name of Systems, Robotics and Vision Group, University of
+      the Balearican Islands nor the names of its contributors may be used to
+      endorse or promote products derived from this software without specific
       prior written permission.
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
@@ -45,13 +45,13 @@
 import numpy as np
 import re
 
-def natural_sort(l): 
-    convert = lambda text: int(text) if text.isdigit() else text.lower() 
-    alphanum_key = lambda key: [ convert(c) for c in re.split('([0-9]+)', key) ] 
+def natural_sort(l):
+    convert = lambda text: int(text) if text.isdigit() else text.lower()
+    alphanum_key = lambda key: [ convert(c) for c in re.split('([0-9]+)', key) ]
     return sorted(l, key = alphanum_key)
 
 def collect_image_files(image_dir,file_pattern):
-  images = glob.glob(image_dir + '/' + file_pattern)
+  images = glob.glob(image_dir + '/' + str(file_pattern))
   images = natural_sort(images)
   return images
 

bag_tools/shell_scripts/change_calib.sh

@@ -0,0 +1,24 @@
+#!/bin/bash
+
+# Set your camera calibration files
+L_CALIB="/data/HD3/bagfiles/turbot/2016_06_13_sant_feliu/l_calibration_water_bags.yaml"
+R_CALIB="/data/HD3/bagfiles/turbot/2016_06_13_sant_feliu/r_calibration_water_bags.yaml"
+
+# Set the directories where the bagfiles are
+DIRS="13 14 15 16 17"
+
+
+
+for d in $DIRS
+do
+  echo "---- DIRECTORY $d ----"
+
+  mkdir corrected
+  FILES=*.bag
+  for f in $FILES
+  do
+    echo "Changing bag: $f"
+    rosrun bag_tools change_camera_info.py $f corrected/$f /stereo_down/left/camera_info=$L_CALIB /stereo_down/right/camera_info=$R_CALIB
+  done
+
+done

pointcloud_tools/src/pointcloud_viewer.cpp

@@ -48,6 +48,7 @@
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/features/feature.h>
 #include <pcl/io/pcd_io.h>
+#include <pcl/filters/statistical_outlier_removal.h>
 
 using pcl::visualization::PointCloudColorHandlerGenericField;
 
@@ -185,7 +186,15 @@ void saveCallback(const ros::WallTimerEvent&)
 
   if (cloud_xyz_rgb_.size() > 0)
   {
-    if (pcl::io::savePCDFile(pcd_filename_, cloud_xyz_rgb_) == 0)
+    // Remove outliers
+    pcl::PointCloud<PointRGB>::Ptr cloud_filtered (new pcl::PointCloud<PointRGB>);
+    pcl::StatisticalOutlierRemoval<PointRGB> sor;
+    sor.setInputCloud (cloud_xyz_rgb_.makeShared());
+    sor.setMeanK (50);
+    sor.setStddevMulThresh (1.0);
+    sor.filter (*cloud_filtered);
+
+    if (pcl::io::savePCDFile(pcd_filename_, *cloud_filtered) == 0)
       ROS_INFO_STREAM("[PointCloudViewer:] Pointcloud saved into: " << pcd_filename_);
     else
       ROS_ERROR_STREAM("[PointCloudViewer:] Problem saving " << pcd_filename_.c_str());
@@ -194,7 +203,15 @@ void saveCallback(const ros::WallTimerEvent&)
 
   if (cloud_xyz_.size() > 0)
   {
-    if (pcl::io::savePCDFile(pcd_filename_, cloud_xyz_) == 0)
+    // Remove outliers
+    pcl::PointCloud<Point>::Ptr cloud_filtered (new pcl::PointCloud<Point>);
+    pcl::StatisticalOutlierRemoval<Point> sor;
+    sor.setInputCloud (cloud_xyz_.makeShared());
+    sor.setMeanK (50);
+    sor.setStddevMulThresh (1.0);
+    sor.filter (*cloud_filtered);
+
+    if (pcl::io::savePCDFile(pcd_filename_, *cloud_filtered) == 0)
       ROS_INFO_STREAM("[PointCloudViewer:] Pointcloud saved into: " << pcd_filename_);
     else
       ROS_ERROR_STREAM("[PointCloudViewer:] Problem saving " << pcd_filename_.c_str());

tf_tools/src/apply_tf_to_odom_msg.cpp

@@ -25,7 +25,7 @@ class ApplyTF2Odom
     nh_private_.param("qx", qx, 0.0);
     nh_private_.param("qy", qy, 0.0);
     nh_private_.param("qz", qz, 0.0);
-    nh_private_.param("qw", qw, 0.0);
+    nh_private_.param("qw", qw, 1.0);
 
     // Build tf
     tf::Vector3 t(x, y, z);
@@ -52,11 +52,11 @@ class ApplyTF2Odom
 
     // Build the odometry pose
     tf::Vector3 t(x, y, z);
-    tf::Quaternion q(qx, qy, qz, qw);
+    tf::Quaternion q(qx, qy, qz, 1.0);
     tf::Transform pose(q, t);
 
     // Transform
-    tf::Transform new_pose = pose * trans_;
+    tf::Transform new_pose = trans_ * pose;
 
     // Publish
     nav_msgs::Odometry new_odom_msg = *odom_msg;