Added documentation for graph_bag scripts, renamed to localization_an…

…alysis (nasa#432)

Renamed graph_bag to localization_analysis and added better documentation for scripts and tools as well as updating the readme. Also added usage instructions for the imu_bias_tester package.

tools/calibration/scripts/get_bags_with_topic.py

@@ -35,7 +35,7 @@ def get_bags_with_topic(bag_names, topic):
     return bag_names_with_topic
 
 
-# TODO(rsoussan): combine this with same fcn in graph_bag/merge_bags.py
+# TODO(rsoussan): combine this with same fcn in localization_analysis/merge_bags.py
 def find_bags_in_directory(directory):
     # Find bagfiles with bag prefix in current directory, fail if none found
     # TODO(rsoussan): make function for getting filenames in a dir with extenstion, use for view_all_detections.py as well

tools/imu_bias_tester/readme.md

@@ -1,7 +1,17 @@
 \page imubiastester Imu Bias Tester 
 
 ## ImuBiasTester
-The imu bias tester provides a way to evaluate the accuracy of imu biases without having bias groundtruth.  It works by relying on some sort of localization groundtruth instead.  The bias tester integrates imu measurements but updates biases as localization estimates are provided.  Thus, if the biases are perfectly estimated by the localizer, the integrated imu measurements should perfectly match localization groundtruth. The nodelet subscribes to the imu and localization state and published a imu bias pose. 
+The ImuBiasTester integrates IMU measurements using the latest estimated IMU biases from graph_state messages.
+The integrated measurements are published as pose messages.
+These poses can then be plotted against localization groundtruth to measure the accuracy of the estimated IMU biases.
+
+# Usage
+Launch the nodelet online or while playing a recored bagfile using
+`roslaunch imu_bias_tester imu_bias_tester.launch`
+to generated IMU bias poses. Ensure that localization estimates and IMU messages are
+available as these are required for IMU bias pose estimation. 
+Record the results to a bagfile and plot using the `plot_results.py` script in 
+localization_analysis.  See the localization_analysis readme for more details on plotting results.
 
 # Inputs
 * `/hw/imu`

tools/graph_bag/CMakeLists.txt → tools/localization_analysis/CMakeLists.txt

@@ -16,7 +16,7 @@
 #under the License.
 
 cmake_minimum_required(VERSION 3.0)
-project(graph_bag)
+project(localization_analysis)
 
 if (USE_ROS)
 
@@ -73,6 +73,12 @@ if (USE_ROS)
   add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
   target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 
+  ## Declare a C++ executable: convert_depth_msg 
+  add_executable(convert_depth_msg tools/convert_depth_msg.cc)
+  add_dependencies(convert_depth_msg ${catkin_EXPORTED_TARGETS})
+  target_link_libraries(convert_depth_msg
+    ${PROJECT_NAME} gflags gtsam ${catkin_LIBRARIES})
+
   ## Declare a C++ executable: run_bag_imu_filterer
   add_executable(run_bag_imu_filterer tools/run_bag_imu_filterer.cc)
   add_dependencies(run_bag_imu_filterer ${catkin_EXPORTED_TARGETS})
@@ -121,6 +127,7 @@ install(TARGETS run_graph_bag DESTINATION bin)
 install(TARGETS run_imu_bias_tester_adder DESTINATION bin)
 install(TARGETS run_sparse_mapping_pose_adder DESTINATION bin)
 install(CODE "execute_process(
+  COMMAND ln -s ../../bin/convert_depth_msg share/${PROJECT_NAME}
   COMMAND ln -s ../../bin/run_bag_imu_filterer share/${PROJECT_NAME}
   COMMAND ln -s ../../bin/run_depth_odometry_adder share/${PROJECT_NAME}
   COMMAND ln -s ../../bin/run_graph_bag share/${PROJECT_NAME}

tools/graph_bag/include/graph_bag/bag_imu_filterer.h → tools/localization_analysis/include/localization_analysis/bag_imu_filterer.h

@@ -16,8 +16,8 @@
  * under the License.
  */
 
-#ifndef GRAPH_BAG_BAG_IMU_FILTERER_H_
-#define GRAPH_BAG_BAG_IMU_FILTERER_H_
+#ifndef LOCALIZATION_ANALYSIS_BAG_IMU_FILTERER_H_
+#define LOCALIZATION_ANALYSIS_BAG_IMU_FILTERER_H_
 
 #include <imu_integration/imu_filter.h>
 
@@ -26,7 +26,7 @@
 
 #include <string>
 
-namespace graph_bag {
+namespace localization_analysis {
 // Reads through a bag file and filters imu measurements, replacing the old imu measurements with new filtered ones.
 // Saves output to a new bagfile.
 class BagImuFilterer {
@@ -39,6 +39,6 @@ class BagImuFilterer {
   rosbag::Bag bag_;
   rosbag::Bag filtered_bag_;
 };
-}  // end namespace graph_bag
+}  // end namespace localization_analysis
 
-#endif  // GRAPH_BAG_BAG_IMU_FILTERER_H_
+#endif  // LOCALIZATION_ANALYSIS_BAG_IMU_FILTERER_H_

tools/graph_bag/include/graph_bag/depth_odometry_adder.h → tools/localization_analysis/include/localization_analysis/depth_odometry_adder.h

@@ -16,8 +16,8 @@
  * under the License.
  */
 
-#ifndef GRAPH_BAG_DEPTH_ODOMETRY_ADDER_H_
-#define GRAPH_BAG_DEPTH_ODOMETRY_ADDER_H_
+#ifndef LOCALIZATION_ANALYSIS_DEPTH_ODOMETRY_ADDER_H_
+#define LOCALIZATION_ANALYSIS_DEPTH_ODOMETRY_ADDER_H_
 
 #include <depth_odometry/depth_odometry_wrapper.h>
 
@@ -58,7 +58,7 @@
 
 #include <string>
 
-namespace graph_bag {
+namespace localization_analysis {
 class DepthOdometryAdder {
  public:
   DepthOdometryAdder(const std::string& input_bag_name, const std::string& output_bag_name, const bool save_all_topics,
@@ -71,6 +71,6 @@ class DepthOdometryAdder {
   rosbag::Bag output_bag_;
   bool save_all_topics_;
 };
-}  // namespace graph_bag
+}  // namespace localization_analysis
 
-#endif  // GRAPH_BAG_DEPTH_ODOMETRY_ADDER_H_
+#endif  // LOCALIZATION_ANALYSIS_DEPTH_ODOMETRY_ADDER_H_

tools/graph_bag/include/graph_bag/graph_bag.h → tools/localization_analysis/include/localization_analysis/graph_bag.h

@@ -16,14 +16,14 @@
  * under the License.
  */
 
-#ifndef GRAPH_BAG_GRAPH_BAG_H_
-#define GRAPH_BAG_GRAPH_BAG_H_
+#ifndef LOCALIZATION_ANALYSIS_GRAPH_BAG_H_
+#define LOCALIZATION_ANALYSIS_GRAPH_BAG_H_
 
 #include <camera/camera_params.h>
-#include <graph_bag/graph_localizer_simulator.h>
-#include <graph_bag/graph_bag_params.h>
+#include <localization_analysis/graph_localizer_simulator.h>
+#include <localization_analysis/graph_bag_params.h>
 #include <imu_bias_tester/imu_bias_tester_wrapper.h>
-#include <graph_bag/live_measurement_simulator.h>
+#include <localization_analysis/live_measurement_simulator.h>
 #include <imu_augmentor/imu_augmentor_wrapper.h>
 
 #include <rosbag/bag.h>
@@ -33,7 +33,7 @@
 #include <string>
 #include <vector>
 
-namespace graph_bag {
+namespace localization_analysis {
 // Reads through a bag file and passes relevant messages to graph localizer
 // wrapper.  Uses LiveMeasurementSimulator which contains its own instances of sensor parsers (lk_optical_flow,
 // localizer (for sparse map matching)) and passes output to graph localizer
@@ -58,6 +58,6 @@ class GraphBag {
   const std::string kFeatureTracksImageTopic_ = "feature_track_image";
   GraphBagParams params_;
 };
-}  // end namespace graph_bag
+}  // end namespace localization_analysis
 
-#endif  // GRAPH_BAG_GRAPH_BAG_H_
+#endif  // LOCALIZATION_ANALYSIS_GRAPH_BAG_H_

tools/graph_bag/include/graph_bag/graph_bag_params.h → tools/localization_analysis/include/localization_analysis/graph_bag_params.h

@@ -15,16 +15,16 @@
  * License for the specific language governing permissions and limitations
  * under the License.
  */
-#ifndef GRAPH_BAG_GRAPH_BAG_PARAMS_H_
-#define GRAPH_BAG_GRAPH_BAG_PARAMS_H_
+#ifndef LOCALIZATION_ANALYSIS_GRAPH_BAG_PARAMS_H_
+#define LOCALIZATION_ANALYSIS_GRAPH_BAG_PARAMS_H_
 
 #include <camera/camera_params.h>
 
 #include <gtsam/geometry/Pose3.h>
 
 #include <memory>
 
-namespace graph_bag {
+namespace localization_analysis {
 struct GraphBagParams {
   bool save_optical_flow_images;
   bool log_relative_time;
@@ -33,6 +33,6 @@ struct GraphBagParams {
   int ar_min_num_landmarks;
   int sparse_mapping_min_num_landmarks;
 };
-}  // namespace graph_bag
+}  // namespace localization_analysis
 
-#endif  // GRAPH_BAG_GRAPH_BAG_PARAMS_H_
+#endif  // LOCALIZATION_ANALYSIS_GRAPH_BAG_PARAMS_H_