publish ba separately

raphaelchang · Sep 21, 2019 · 253af41 · 253af41
1 parent 0b2193a
commit 253af41
Show file tree

Hide file tree

Showing 6 changed files with 34 additions and 6 deletions.
diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch
@@ -12,8 +12,10 @@
         <param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" />
         <param name="tracked_image_topic" value="/omni_slam/tracked" />
         <param name="odometry_estimate_topic" value="/omni_slam/odometry" />
+        <param name="odometry_optimized_topic" value="/omni_slam/odometry_optimized" />
         <param name="odometry_ground_truth_topic" value="/omni_slam/odometry_truth" />
         <param name="path_estimate_topic" value="/omni_slam/odometry_path" />
+        <param name="path_optimized_topic" value="/omni_slam/odometry_path_optimized" />
         <param name="path_ground_truth_topic" value="/omni_slam/odometry_path_truth" />
         <param name="output_frame" value="map" />
         <param name="rate" value="$(arg rate)" />

diff --git a/launch/slam_eval.launch b/launch/slam_eval.launch
@@ -13,8 +13,10 @@
         <param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" />
         <param name="tracked_image_topic" value="/omni_slam/tracked" />
         <param name="odometry_estimate_topic" value="/omni_slam/odometry" />
+        <param name="odometry_optimized_topic" value="/omni_slam/odometry_optimized" />
         <param name="odometry_ground_truth_topic" value="/omni_slam/odometry_truth" />
         <param name="path_estimate_topic" value="/omni_slam/odometry_path" />
+        <param name="path_optimized_topic" value="/omni_slam/odometry_path_optimized" />
         <param name="path_ground_truth_topic" value="/omni_slam/odometry_path_truth" />
         <param name="point_cloud_topic" value="/omni_slam/reconstructed" />
         <param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />

diff --git a/launch/slam_eval_kitti.launch b/launch/slam_eval_kitti.launch
@@ -13,8 +13,10 @@
         <param name="pose_topic" value="pose_imu" />
         <param name="tracked_image_topic" value="/omni_slam/tracked" />
         <param name="odometry_estimate_topic" value="/omni_slam/odometry" />
+        <param name="odometry_optimized_topic" value="/omni_slam/odometry_optimized" />
         <param name="odometry_ground_truth_topic" value="/omni_slam/odometry_truth" />
         <param name="path_estimate_topic" value="/omni_slam/odometry_path" />
+        <param name="path_optimized_topic" value="/omni_slam/odometry_path_optimized" />
         <param name="path_ground_truth_topic" value="/omni_slam/odometry_path_truth" />
         <param name="point_cloud_topic" value="/omni_slam/reconstructed" />
         <param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />

diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc
@@ -44,16 +44,22 @@ void OdometryEval<Stereo>::InitPublishers()
 {
     TrackingEval<Stereo>::InitPublishers();
     string outputTopic;
+    string outputOptTopic;
     string outputGndTopic;
     string outputPathTopic;
+    string outputPathOptTopic;
     string outputPathGndTopic;
     this->nhp_.param("odometry_estimate_topic", outputTopic, string("/omni_slam/odometry"));
+    this->nhp_.param("odometry_optimized_topic", outputOptTopic, string("/omni_slam/odometry_optimized"));
     this->nhp_.param("odometry_ground_truth_topic", outputGndTopic, string("/omni_slam/odometry_truth"));
     this->nhp_.param("path_estimate_topic", outputPathTopic, string("/omni_slam/odometry_path"));
+    this->nhp_.param("path_optimized_topic", outputOptTopic, string("/omni_slam/odometry_path_optimized"));
     this->nhp_.param("path_ground_truth_topic", outputPathGndTopic, string("/omni_slam/odometry_path_truth"));
     odometryPublisher_ = this->nh_.template advertise<geometry_msgs::PoseStamped>(outputTopic, 2);
+    odometryOptPublisher_ = this->nh_.template advertise<geometry_msgs::PoseStamped>(outputOptTopic, 2);
     odometryGndPublisher_ = this->nh_.template advertise<geometry_msgs::PoseStamped>(outputGndTopic, 2);
     pathPublisher_ = this->nh_.template advertise<nav_msgs::Path>(outputPathTopic, 2);
+    pathOptPublisher_ = this->nh_.template advertise<nav_msgs::Path>(outputPathOptTopic, 2);
     pathGndPublisher_ = this->nh_.template advertise<nav_msgs::Path>(outputPathGndTopic, 2);
 }
 
@@ -72,7 +78,7 @@ void OdometryEval<Stereo>::Finish()
 {
     ROS_INFO("Performing bundle adjustment...");
     odometryModule_->BundleAdjust(this->trackingModule_->GetLandmarks());
-    PublishOdometry();
+    PublishOdometry(true);
 }
 
 template <bool Stereo>
@@ -89,7 +95,7 @@ void OdometryEval<Stereo>::Visualize(cv_bridge::CvImagePtr &base_img)
 }
 
 template <bool Stereo>
-void OdometryEval<Stereo>::PublishOdometry()
+void OdometryEval<Stereo>::PublishOdometry(bool optimized)
 {
     geometry_msgs::PoseStamped poseMsg;
     poseMsg.header.frame_id = cameraFrame_;
@@ -105,7 +111,14 @@ void OdometryEval<Stereo>::PublishOdometry()
         poseMsg.pose.orientation.z = quat.normalized().z();
         poseMsg.pose.orientation.w = quat.normalized().w();
         poseMsg.header.stamp = ::ros::Time(this->trackingModule_->GetFrames().back()->GetTime());
-        odometryPublisher_.publish(poseMsg);
+        if (optimized)
+        {
+            odometryOptPublisher_.publish(poseMsg);
+        }
+        else
+        {
+            odometryPublisher_.publish(poseMsg);
+        }
     }
     const Matrix<double, 3, 4> &poseGnd = this->trackingModule_->GetFrames().back()->GetPose();
     poseMsg.header.frame_id = cameraFrame_;
@@ -163,7 +176,14 @@ void OdometryEval<Stereo>::PublishOdometry()
         }
         first = false;
     }
-    pathPublisher_.publish(path);
+    if (optimized)
+    {
+        pathOptPublisher_.publish(path);
+    }
+    else
+    {
+        pathPublisher_.publish(path);
+    }
     pathGndPublisher_.publish(pathGnd);
 }
 

diff --git a/src/ros/odometry_eval.h b/src/ros/odometry_eval.h
@@ -27,14 +27,16 @@ class OdometryEval : public virtual TrackingEval<Stereo>
     virtual void GetResultsData(std::map<std::string, std::vector<std::vector<double>>> &data);
     virtual void Visualize(cv_bridge::CvImagePtr &base_img);
 
-    void PublishOdometry();
+    void PublishOdometry(bool optimized = false);
 
     std::unique_ptr<module::OdometryModule> odometryModule_;
 
 private:
     ::ros::Publisher odometryPublisher_;
+    ::ros::Publisher odometryOptPublisher_;
     ::ros::Publisher odometryGndPublisher_;
     ::ros::Publisher pathPublisher_;
+    ::ros::Publisher pathOptPublisher_;
     ::ros::Publisher pathGndPublisher_;
 
     std::string cameraFrame_;

diff --git a/src/ros/slam_eval.cc b/src/ros/slam_eval.cc
@@ -55,7 +55,7 @@ void SLAMEval::ProcessFrame(unique_ptr<data::Frame> &&frame)
 void SLAMEval::Finish()
 {
     ReconstructionEval<true>::Finish();
-    PublishOdometry();
+    PublishOdometry(true);
 }
 
 void SLAMEval::GetResultsData(std::map<std::string, std::vector<std::vector<double>>> &data)