working pnp odometry

raphaelchang · Sep 13, 2019 · 272ca95 · 272ca95
1 parent db3da67
commit 272ca95
Show file tree

Hide file tree

Showing 16 changed files with 487 additions and 48 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -59,6 +59,7 @@ add_library(omni_slam_eval_lib
   src/module/tracking_module.cc
   src/module/matching_module.cc
   src/module/reconstruction_module.cc
+  src/module/odometry_module.cc
   src/data/frame.cc
   src/data/feature.cc
   src/data/landmark.cc
@@ -90,6 +91,13 @@ add_executable(omni_slam_reconstruction_eval_node
     src/omni_slam_reconstruction_eval_node.cc
 )
 
+add_executable(omni_slam_odometry_eval_node
+    src/ros/odometry_eval.cc
+    src/ros/tracking_eval.cc
+    src/ros/eval_base.cc
+    src/omni_slam_odometry_eval_node.cc
+)
+
 target_link_libraries(omni_slam_tracking_eval_node PUBLIC omni_slam_eval_lib
   ${catkin_LIBRARIES}
   ${OpenCV_LIBS}
@@ -117,9 +125,19 @@ target_link_libraries(omni_slam_reconstruction_eval_node PUBLIC omni_slam_eval_l
   ${CERES_LIBRARIES}
 )
 
+target_link_libraries(omni_slam_odometry_eval_node PUBLIC omni_slam_eval_lib
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBS}
+  ${HDF5_CXX_LIBRARIES}
+  ${OpenMP_CXX_FLAGS}
+  ${PCL_LIBRARIES}
+  ${CERES_LIBRARIES}
+)
+
 target_compile_options(omni_slam_tracking_eval_node PUBLIC ${OpenMP_CXX_FLAGS})
 target_compile_options(omni_slam_matching_eval_node PUBLIC ${OpenMP_CXX_FLAGS})
 target_compile_options(omni_slam_reconstruction_eval_node PUBLIC ${OpenMP_CXX_FLAGS})
+target_compile_options(omni_slam_odometry_eval_node PUBLIC ${OpenMP_CXX_FLAGS})
 set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS})
 
 install(TARGETS omni_slam_tracking_eval_node
@@ -139,3 +157,9 @@ install(TARGETS omni_slam_reconstruction_eval_node
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
+
+install(TARGETS omni_slam_odometry_eval_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/launch/matching_eval.launch b/launch/matching_eval.launch
@@ -15,7 +15,7 @@
         <param name="image_topic" value="/unity_ros/Sphere/FisheyeCamera/image_raw" />
         <param name="depth_image_topic" value="/unity_ros/Sphere/FisheyeDepthCamera/image_raw" />
         <param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" />
-        <param name="matched_image_topic" value="/omni_slam_eval/matched" />
+        <param name="matched_image_topic" value="/omni_slam/matched" />
         <param name="rate" value="$(arg rate)" />
         <rosparam subst_value="true">
             camera_model: '$(arg camera_model)'

diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch
@@ -0,0 +1,34 @@
+<launch>
+    <arg name="bag_file" default="" />
+    <arg name="results_file" default="$(arg bag_file).odometry.hdf5" />
+    <arg name="camera_model" default="double_sphere" />
+    <arg name="camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
+    <arg name="rate" default="1" />
+    <node pkg="omni_slam_eval" type="omni_slam_odometry_eval_node" name="omni_slam_odometry_eval_node" required="true" output="screen">
+        <param name="bag_file" value="$(arg bag_file)" />
+        <param name="results_file" value="$(arg results_file)" />
+        <param name="image_topic" value="/unity_ros/Sphere/FisheyeCamera/image_raw" />
+        <param name="depth_image_topic" value="/unity_ros/Sphere/FisheyeDepthCamera/image_raw" />
+        <param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" />
+        <param name="tracked_image_topic" value="/omni_slam/tracked" />
+        <param name="odometry_topic" value="/omni_slam/odometry" />
+        <param name="odometry_ground_truth_topic" value="/omni_slam/odometry_truth" />
+        <param name="rate" value="$(arg rate)" />
+        <rosparam subst_value="true">
+            camera_model: '$(arg camera_model)'
+            camera_parameters: $(arg camera_params)
+            detector_type: 'GFTT'
+            detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
+            tracker_window_size: 128
+            tracker_num_scales: 4
+            tracker_delta_pixel_error_threshold: 3.0
+            tracker_error_threshold: 20.0
+            min_features_per_region: 10
+            pnp_inlier_threshold: 5.0
+            pnp_iterations: 1000
+            bundle_adjustment_max_iterations: 1000
+            bundle_adjustment_logging: true
+        </rosparam>
+    </node>
+</launch>
+
diff --git a/launch/reconstruction_eval.launch b/launch/reconstruction_eval.launch
@@ -10,7 +10,8 @@
         <param name="image_topic" value="/unity_ros/Sphere/FisheyeCamera/image_raw" />
         <param name="depth_image_topic" value="/unity_ros/Sphere/FisheyeDepthCamera/image_raw" />
         <param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" />
-        <param name="tracked_image_topic" value="/omni_slam_eval/tracked" />
+        <param name="tracked_image_topic" value="/omni_slam/tracked" />
+        <param name="point_cloud_topic" value="/omni_slam/reconstructed" />
         <param name="rate" value="$(arg rate)" />
         <rosparam subst_value="true">
             camera_model: '$(arg camera_model)'

diff --git a/launch/tracking_eval.launch b/launch/tracking_eval.launch
@@ -10,7 +10,7 @@
         <param name="image_topic" value="/unity_ros/Sphere/FisheyeCamera/image_raw" />
         <param name="depth_image_topic" value="/unity_ros/Sphere/FisheyeDepthCamera/image_raw" />
         <param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" />
-        <param name="tracked_image_topic" value="/omni_slam_eval/tracked" />
+        <param name="tracked_image_topic" value="/omni_slam/tracked" />
         <param name="rate" value="$(arg rate)" />
         <rosparam subst_value="true">
             camera_model: '$(arg camera_model)'

diff --git a/src/module/odometry_module.cc b/src/module/odometry_module.cc
@@ -0,0 +1,31 @@
+#include "odometry_module.h"
+
+using namespace std;
+
+namespace omni_slam
+{
+namespace module
+{
+
+OdometryModule::OdometryModule(std::unique_ptr<odometry::PNP> &pnp)
+    : pnp_(std::move(pnp))
+{
+}
+
+OdometryModule::OdometryModule(std::unique_ptr<odometry::PNP> &&pnp)
+    : OdometryModule(pnp)
+{
+}
+
+void OdometryModule::Update(std::vector<data::Landmark> &landmarks, data::Frame &frame)
+{
+    pnp_->Compute(landmarks, frame);
+}
+
+OdometryModule::Stats& OdometryModule::GetStats()
+{
+    return stats_;
+}
+
+}
+}
diff --git a/src/module/odometry_module.h b/src/module/odometry_module.h
@@ -0,0 +1,44 @@
+#ifndef _ODOMETRY_MODULE_H_
+#define _ODOMETRY_MODULE_H_
+
+#include <vector>
+#include <set>
+#include <memory>
+
+#include "odometry/pnp.h"
+#include "data/landmark.h"
+
+#include <pcl/common/projection_matrix.h>
+#include <pcl/point_types.h>
+
+using namespace Eigen;
+
+namespace omni_slam
+{
+namespace module
+{
+
+class OdometryModule
+{
+public:
+    struct Stats
+    {
+    };
+
+    OdometryModule(std::unique_ptr<odometry::PNP> &pnp);
+    OdometryModule(std::unique_ptr<odometry::PNP> &&pnp);
+
+    void Update(std::vector<data::Landmark> &landmarks, data::Frame &frame);
+
+    Stats& GetStats();
+
+private:
+    std::shared_ptr<odometry::PNP> pnp_;
+
+    Stats stats_;
+};
+
+}
+}
+
+#endif /* _ODOMETRY_MODULE_H_ */
diff --git a/src/module/tracking_module.cc b/src/module/tracking_module.cc
@@ -135,6 +135,11 @@ std::vector<data::Landmark>& TrackingModule::GetLandmarks()
     return landmarks_;
 }
 
+std::vector<std::unique_ptr<data::Frame>>& TrackingModule::GetFrames()
+{
+    return frames_;
+}
+
 TrackingModule::Stats& TrackingModule::GetStats()
 {
     return stats_;

diff --git a/src/module/tracking_module.h b/src/module/tracking_module.h
@@ -34,6 +34,7 @@ class TrackingModule
     void Redetect();
 
     std::vector<data::Landmark>& GetLandmarks();
+    std::vector<std::unique_ptr<data::Frame>>& GetFrames();
 
     Stats& GetStats();
     void Visualize(cv::Mat &base_img);