Merge pull request #24 from ori-drs/sn-fix-go-back-orientation

Fix orientation of poses in go back path

aicp_core/include/aicp_registration/aligned_cloud.hpp

@@ -18,6 +18,8 @@ public:
                  Eigen::Isometry3d prior_pose);
     ~AlignedCloud();
 
+    void removePitchRollCorrection();
+
     void updateCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                      bool is_reference);
     void updateCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
@@ -31,6 +33,7 @@ public:
     pcl::PointCloud<pcl::PointXYZ>::Ptr getCloud(){ return cloud_; }
     int getNbPoints(){ return cloud_->size(); }
 
+    Eigen::Isometry3d getOdomPose(){ return world_to_cloud_odom_; }
     Eigen::Isometry3d getPriorPose(){ return world_to_cloud_prior_; }
     Eigen::Isometry3d getCorrection(){ return cloud_to_reference_; }
     Eigen::Isometry3d getCorrectedPose(){ return world_to_cloud_corrected_; }
@@ -53,6 +56,8 @@ private:
 
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_; // Cloud (pre-filtered and global coordinates)
 
+    Eigen::Isometry3d world_to_cloud_odom_;          // odom to base:         world -> cloud (global coordinates). this is the unmodified input.
+
     Eigen::Isometry3d world_to_cloud_prior_;         // cloud pose prior:     world -> cloud (global coordinates)
     Eigen::Isometry3d cloud_to_reference_;           // relative transform:   cloud -> reference (global coordinates)
     Eigen::Isometry3d world_to_cloud_corrected_;     // cloud pose corrected: world -> cloud (global coordinates)

aicp_core/include/aicp_utils/common.hpp

@@ -22,6 +22,10 @@
 //convert formats in Eigen
 Eigen::Isometry3d fromMatrix4fToIsometry3d(Eigen::Matrix4f matrix);
 
+//compute angle between two vecotrs (returns degrees)
+double angleBetweenVectors2d(const Eigen::Vector2d& v1,
+                             const Eigen::Vector2d& v2);
+
 //swapping two values.
 template<typename T>
 inline bool swap_if_gt(T& a, T& b) {
@@ -45,3 +49,4 @@ Eigen::VectorXf get_random_gaussian_variable(float mean, float std_deviation, in
 
 void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& yaw);
 
+Eigen::Quaterniond euler_to_quat(double roll, double pitch, double yaw);

aicp_core/include/aicp_utils/visualizer.hpp

@@ -39,6 +39,20 @@ public:
                               std::string name,
                               int64_t utime = -1) = 0;
 
+    virtual void publishOdomPoses(Eigen::Isometry3d pose_,
+                              int param,
+                              std::string name,
+                              int64_t utime = -1) = 0;
+
+    virtual void publishPriorPoses(Eigen::Isometry3d pose_,
+                              int param,
+                              std::string name,
+                              int64_t utime = -1) = 0;
+
+    virtual void publishOdomToMapPose(Eigen::Isometry3d pose_,
+                              int64_t utime = -1) = 0;
+
+
     // Gets
     virtual const PathPoses& getPath() = 0;
 

aicp_core/src/registration/aligned_cloud.cpp

 #include "aicp_registration/aligned_cloud.hpp"
+#include "aicp_utils/common.hpp"
 
 namespace aicp {
 
@@ -9,6 +10,8 @@ AlignedCloud::AlignedCloud(int64_t utime,
     utime_ = utime;
     cloud_ = cloud;
 
+    world_to_cloud_odom_ = prior_pose;                          // prior pose. This is the original odom-to-base and is not updated elsewhere
+
     world_to_cloud_prior_ = prior_pose;                         // prior pose
     cloud_to_reference_ = Eigen::Isometry3d::Identity();        // correction
     world_to_cloud_corrected_ = world_to_cloud_prior_;          // corrected pose (set equal to prior pose when correction not available yet)
@@ -21,6 +24,35 @@ AlignedCloud::~AlignedCloud()
 { 
 }
 
+
+// Take the roll and pitch from the odom and use it to replace the roll and pitch estimated by ICP
+// this is to ensure gravity consistency
+// TODO: the cloud_to_reference_ is currently inconsistent with these two frames.
+void AlignedCloud::removePitchRollCorrection()
+{
+    Eigen::Quaterniond q_odom = Eigen::Quaterniond(world_to_cloud_odom_.rotation());
+    double r_odom, p_odom, y_odom;
+    quat_to_euler(q_odom, r_odom, p_odom, y_odom);
+
+    Eigen::Quaterniond q_corr = Eigen::Quaterniond(world_to_cloud_corrected_.rotation());
+    double r_corr, p_corr, y_corr;
+    quat_to_euler(q_corr, r_corr, p_corr, y_corr);
+
+    //std::cout << r_odom*180/M_PI << " " 
+    //          << p_odom*180/M_PI << " " 
+    //          << y_odom*180/M_PI << " odom\n";
+    //std::cout << r_corr*180/M_PI << " " 
+    //          << p_corr*180/M_PI << " " 
+    //          << y_corr*180/M_PI << " corr\n";
+
+    Eigen::Isometry3d world_to_cloud_corrected_fixed = Eigen::Isometry3d::Identity();
+    world_to_cloud_corrected_fixed.translation() = world_to_cloud_corrected_.translation();
+    world_to_cloud_corrected_fixed.rotate( euler_to_quat(r_odom, p_odom, y_corr) );
+    world_to_cloud_corrected_ = world_to_cloud_corrected_fixed;
+}
+
+
+
 void AlignedCloud::updateCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                                bool is_reference)
 {
@@ -34,6 +66,9 @@ void AlignedCloud::updateCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
     cloud_ = cloud;
     cloud_to_reference_ = correction;
     world_to_cloud_corrected_ = cloud_to_reference_ * world_to_cloud_prior_;
+
+    removePitchRollCorrection();
+
     is_reference_ = is_reference;
     its_reference_id_ = its_reference_id;
 }

aicp_core/src/registration/app.cpp

@@ -401,10 +401,31 @@ void App::operator()() {
                 Eigen::Isometry3d relative_motion = vis_->getPath().back().inverse() *
                                                     aligned_clouds_graph_->getLastCloud()->getCorrectedPose();
                 double dist = relative_motion.translation().norm();
-                if (dist > 1.0)
+                if (dist > 1.0)//(1==1)// use threshold to reduce number of nearby markers. this shouldnt be done here
                 {
                     vis_->publishPoses(aligned_clouds_graph_->getLastCloud()->getCorrectedPose(), 0, "",
                                        cloud->getUtime());
+                    vis_->publishPriorPoses(aligned_clouds_graph_->getLastCloud()->getPriorPose(), 0, "",
+                                       cloud->getUtime());
+                    vis_->publishOdomPoses(aligned_clouds_graph_->getLastCloud()->getOdomPose(), 0, "",
+                                       cloud->getUtime());
+
+
+                    std::cout << "odom_to_map publish <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n";
+                    Eigen::Isometry3d odom_to_base = aligned_clouds_graph_->getLastCloud()->getOdomPose();
+                    Eigen::Isometry3d map_to_base = aligned_clouds_graph_->getLastCloud()->getCorrectedPose();
+
+                    Eigen::Isometry3d odom_to_map = (map_to_base * odom_to_base.inverse()).inverse();
+                    vis_->publishOdomToMapPose(odom_to_map, cloud->getUtime());
+
+                    Eigen::Quaterniond q_corr = Eigen::Quaterniond(odom_to_map.rotation());
+                    double r_corr, p_corr, y_corr;
+                    quat_to_euler(q_corr, r_corr, p_corr, y_corr);
+                    std::cout << odom_to_map.translation().transpose() << " odom_to_map publish\n";
+                    std::cout << r_corr*180/M_PI << " " 
+                              << p_corr*180/M_PI << " " 
+                              << y_corr*180/M_PI << " rpy\n";
+
                 }
 
                 // Store aligned map and VISUALIZE

aicp_core/src/utils/common.cpp

@@ -22,6 +22,11 @@ Eigen::Isometry3d fromMatrix4fToIsometry3d(Eigen::Matrix4f matrix){
   return isometry;
 }
 
+double angleBetweenVectors2d(const Eigen::Vector2d &v1,
+                             const Eigen::Vector2d &v2){
+    return atan2(v1(0)*v2(1) - v1(1)*v2(0), v1(0)*v2(0) + v1(1)*v2(1)) * 180.0 / M_PI;
+}
+
 //extract integers from a string.
 std::string extract_ints(std::ctype_base::mask category, std::string str, std::ctype<char> const& facet)
 {
@@ -71,3 +76,30 @@ void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& ya
   pitch = asin(2*(q0*q2-q3*q1));
   yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
 }
+
+
+Eigen::Quaterniond euler_to_quat(double roll, double pitch, double yaw) {
+  
+  // This conversion function introduces a NaN in Eigen Rotations when:
+  // roll == pi , pitch,yaw =0    ... or other combinations.
+  // cos(pi) ~=0 but not exactly 0 
+  if ( ((roll==M_PI) && (pitch ==0)) && (yaw ==0)){
+    return  Eigen::Quaterniond(0,1,0,0);
+  }else if( ((pitch==M_PI) && (roll ==0)) && (yaw ==0)){
+    return  Eigen::Quaterniond(0,0,1,0);
+  }else if( ((yaw==M_PI) && (roll ==0)) && (pitch ==0)){
+    return  Eigen::Quaterniond(0,0,0,1);
+  }
+  
+  double sy = sin(yaw*0.5);
+  double cy = cos(yaw*0.5);
+  double sp = sin(pitch*0.5);
+  double cp = cos(pitch*0.5);
+  double sr = sin(roll*0.5);
+  double cr = cos(roll*0.5);
+  double w = cr*cp*cy + sr*sp*sy;
+  double x = sr*cp*cy - cr*sp*sy;
+  double y = cr*sp*cy + sr*cp*sy;
+  double z = cr*cp*sy - sr*sp*cy;
+  return Eigen::Quaterniond(w,x,y,z);
+}

aicp_ros/include/aicp_ros/visualizer_ros.hpp

@@ -5,6 +5,7 @@
 #include "aicp_utils/visualizer.hpp"
 
 #include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <nav_msgs/Path.h>
 
 #include <eigen_conversions/eigen_msg.h>
@@ -50,8 +51,24 @@ public:
     void publishPoses(PathPoses poses,
                       int param, std::string name, int64_t utime);
 
+    void publishOdomPoses(Eigen::Isometry3d pose,
+                      int param, std::string name, int64_t utime);
+    void publishOdomPoses(PathPoses poses,
+                      int param, std::string name, int64_t utime);
+
+    void publishPriorPoses(Eigen::Isometry3d pose,
+                      int param, std::string name, int64_t utime);
+    void publishPriorPoses(PathPoses poses,
+                      int param, std::string name, int64_t utime);
+
+    void publishOdomToMapPose(Eigen::Isometry3d pose, int64_t utime);
+
+
     // Publish tf from fixed_frame to odom
-    void publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_base_eigen, ros::Time msg_time);
+    void publishFixedFrameToOdomTF(const Eigen::Isometry3d& fixed_frame_to_base_eigen,
+                                   ros::Time msg_time);
+    void publishFixedFrameToOdomPose(const Eigen::Isometry3d& fixed_frame_to_base_eigen,
+                                     ros::Time msg_time);
 
     // Gets
     const PathPoses& getPath(){
@@ -64,17 +81,32 @@ private:
     ros::Publisher prior_map_pub_;
     ros::Publisher aligned_map_pub_;
     ros::Publisher pose_pub_;
+    ros::Publisher odom_pose_pub_;
+    ros::Publisher prior_pose_pub_;
+
+    ros::Publisher fixed_to_odom_pub_;
+    ros::Publisher odom_to_map_pub_;
+    
     // Duplicates the list in collections renderer. assumed to be 3xN colors
     std::vector<double> colors_;
     // Path (vector of poses)
     PathPoses path_;
+    PathPoses odom_path_;
+    PathPoses prior_path_;
 
     std::string fixed_frame_; // map or map_test
     std::string odom_frame_;
     std::string base_frame_;
+    std::string fixed_to_odom_prefix_ = "/localization_manager/";
 
     // TF listener and broadcaster
     tf::TransformListener tf_listener_;
     tf::TransformBroadcaster tf_broadcaster_;
+
+    geometry_msgs::PoseWithCovarianceStamped fixed_to_odom_msg_;
+    tf::Pose temp_tf_pose_;
+
+    void computeFixedFrameToOdom(const Eigen::Isometry3d &fixed_frame_to_base_eigen,
+                                 Eigen::Isometry3d& fixed_frame_to_odom_eigen);
 };
 }

aicp_ros/launch/aicp.launch

@@ -31,6 +31,10 @@
     <param name="working_mode"                  value="debug" />
     <!-- Fixed frame (default: map, debug: map_test -->
     <param name="fixed_frame"                   value="$(arg fixed_frame)" />
+    <!-- default is /aicp/pose_corrected -->
+    <param name="output_channel"                value="/aicp/pose_corrected" />
+    <param name="lidar_channel"                   value="/point_cloud_filter/velodyne/point_cloud_filtered" />
+
     <!-- Prior map info -->
     <param name="load_map_from_file"            value="$(arg load_map_from_file)" />
     <param name="map_from_file_path"            value="$(arg map_from_file_path)" />

aicp_ros/launch/view_recorded_rosbag_aicp_simple.launch

+<?xml version="1.0" encoding="utf-8"?>
+
+<launch>
+
+  <arg name="rviz"                                  default="true"/>
+  <arg name="rviz_minimal"                          default="true"/>
+  <arg name="quadruped_name"                        default="anymal"/>
+  <arg name="quadruped_setup" default="minimal_mb_inspection_v_1_2"/>
+  <arg name="description_name"                      default="quadruped_description"/>
+  <arg name="world"                                 default="empty"/>
+
+  <param name="use_sim_time" value="true" />
+
+  <!-- Load robot description -->
+  <include file="$(find anymal_boxy_description)/launch/load.launch">
+    <arg name="description_name" value="$(arg description_name)"/>
+    <arg name="simulation"       value="false"/>
+    <arg name="velodyne"         value="true"/>
+    <arg name="realsense"        value="true"/>
+  </include>
+
+  <arg name="rviz_config_path"        default="$(find aicp_ros)/config/rviz/config_aicp.rviz"/>
+
+  <!-- rviz -->
+  <group if="$(arg rviz)">
+    <include file="$(find anymal_rviz_visualization)/launch/rviz.launch">
+      <arg name="rviz_world_path"  value="$(arg world)"/>
+      <arg name="rviz_config_path"      value="$(arg rviz_config_path)"/>
+      <arg name="tf_minimal"       value="false"/>
+    </include>
+  </group>
+
+</launch>

aicp_ros/python/aicp_tf_bridge.py

+#!/usr/bin/env python
+# pass aicp's correction to tf
+# this can be used instead of localization manager.
+
+import rospy
+from geometry_msgs.msg import TransformStamped
+from geometry_msgs.msg import *
+from tf2_msgs.msg import TFMessage
+from visualization_msgs.msg import Marker
+
+import tf
+import math
+import numpy
+import time
+
+pub = rospy.Publisher("/tf", TFMessage, queue_size=10)
+vis_pub = rospy.Publisher("/visualization_marker", Marker, queue_size=10)
+rospy.loginfo("aicp_tf_bridge")
+
+
+def handle_odom_to_map(msg):
+    print "x"
+
+    #
+    tfm = TFMessage()
+    this_t = TransformStamped()
+    this_t.header = msg.header
+    this_t.header.frame_id = "odom"
+    this_t.child_frame_id = "map"
+    this_t.transform.translation = msg.pose.pose.position
+    this_t.transform.rotation = msg.pose.pose.orientation
+    tfm.transforms.append(this_t)
+
+    pub.publish(tfm)
+
+
+rospy.init_node('aicp_tf_bridge')
+rospy.Subscriber('/icp_tools/map_pose',
+                  PoseWithCovarianceStamped,
+                  handle_odom_to_map)
+rospy.spin()

aicp_ros/python/aicp_tf_filter.py

+#!/usr/bin/env python
+# when playing back a bag, filter out odom-to-map from tf
+# Note: need to replay the bag while remapping /tf to /tf_old
+# rosbag play filename.bag --pause --clock /tf:=/tf_old
+
+import rospy
+from tf2_msgs.msg import TFMessage
+
+import tf
+import math
+import numpy
+import time
+
+
+
+tf_new_pub = rospy.Publisher("/tf", TFMessage, queue_size=10)
+
+
+def handle_odom_to_map(msg):
+
+    for t in msg.transforms:
+        if (t.child_frame_id == "map"):
+            print "y"
+            return
+
+    tf_new_pub.publish(msg)
+
+
+
+
+
+
+rospy.init_node('aicp_tf_filter')
+rospy.Subscriber('/tf_old',
+                  TFMessage,
+                  handle_odom_to_map)
+rospy.spin()

aicp_ros/src/app_ros.cpp

@@ -90,7 +90,8 @@ void AppROS::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedCon
 
     // Publish fixed_frame to odom tf
     ros::Time msg_time(pose_msg_in->header.stamp.sec, pose_msg_in->header.stamp.nsec);
-    vis_ros_->publishFixedFrameToOdomTF(corrected_pose_, msg_time);
+    // vis_ros_->publishFixedFrameToOdomTF(corrected_pose_, msg_time);
+    // vis_ros_->publishFixedFrameToOdomPose(corrected_pose_, msg_time);
 
     // Publish /aicp/pose_corrected
     tf::poseEigenToTF(corrected_pose_, temp_tf_pose_);
@@ -179,7 +180,7 @@ void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_ms
 //            vis_->publishCloud(accumulated_cloud, 10, "/aicp/accumulated_cloud", accu_->getFinishedTime());
 
             // Push this cloud onto the work queue (mutex safe)
-            const int max_queue_size = 100;
+            const int max_queue_size = 3;
             {
                 std::unique_lock<std::mutex> lock(data_mutex_);
 
@@ -193,6 +194,8 @@ void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_ms
                 cloud_queue_.push_back(current_cloud);
 
                 if (cloud_queue_.size() > max_queue_size) {
+                    cout << "|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||\n";
+                    cout << "|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||\n";
                     cout << "[App ROS] WARNING: dropping " <<
                             (cloud_queue_.size()-max_queue_size) << " clouds." << endl;
                 }

aicp_ros/src/talker_ros.cpp

 #include "aicp_ros/talker_ros.hpp"
-
-//#include <eigen_conversions/eigen_msg.h>
+#include "aicp_utils/common.hpp"
 
 namespace aicp {
 
@@ -46,11 +45,27 @@ void ROSTalker::reversePath(PathPoses& path){
     std::reverse(path.begin(),path.end());
 
     for (size_t i = 0; i < path.size(); ++i){
-        // Turn 90 degrees about z-axis
+        double angle = 180.0;
+
+        // Compute orientation along trajectory line
+        if (i != path.size()-1)
+        {
+            // angle between x-axis of current frame
+            Eigen::Vector2d v1;
+            v1 = path[i].matrix().block<2,1>(0,0);
+            // and trajectory line
+            Eigen::Vector2d v2;
+            v2 << path[i+1].translation()(0, 0) - path[i].translation()(0, 0),
+                  path[i+1].translation()(1, 0) - path[i].translation()(1, 0);
+
+            angle = angleBetweenVectors2d(v1, v2);
+        }
+
+        // Turn about z-axis
         Eigen::Matrix3d rot;
         rot = Eigen::AngleAxisd(0*M_PI/180, Eigen::Vector3d::UnitX())
             * Eigen::AngleAxisd(0*M_PI/180, Eigen::Vector3d::UnitY())
-            * Eigen::AngleAxisd(180*M_PI/180, Eigen::Vector3d::UnitZ());
+            * Eigen::AngleAxisd(angle*M_PI/180, Eigen::Vector3d::UnitZ());
 
         path[i].linear() = rot * path[i].rotation();
     }

aicp_ros/src/visualizer_ros.cpp

@@ -18,6 +18,13 @@ ROSVisualizer::ROSVisualizer(ros::NodeHandle& nh, string fixed_frame) : nh_(nh),
     prior_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/aicp/prior_map", 10);
     aligned_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/aicp/aligned_map", 10);
     pose_pub_ = nh_.advertise<nav_msgs::Path>("/aicp/poses",100);
+    odom_pose_pub_ = nh_.advertise<nav_msgs::Path>("/aicp/odom_poses",100);
+    prior_pose_pub_ = nh_.advertise<nav_msgs::Path>("/aicp/prior_poses",100);
+
+    fixed_to_odom_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(fixed_to_odom_prefix_ + fixed_frame_ + "_to_odom", 10);
+
+    odom_to_map_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/icp_tools/map_pose", 10);
+
     colors_ = {
          51/255.0, 160/255.0, 44/255.0,  //0
          166/255.0, 206/255.0, 227/255.0,
@@ -152,7 +159,87 @@ void ROSVisualizer::publishPoses(PathPoses poses, int param, string name, int64_
     pose_pub_.publish(path_msg);
 }
 
-void ROSVisualizer::publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_base_eigen, ros::Time msg_time)
+
+void ROSVisualizer::publishOdomPoses(Eigen::Isometry3d pose, int param, std::string name, int64_t utime)
+{
+    odom_path_.push_back(pose);
+    publishOdomPoses(odom_path_, param, name, utime);
+}
+
+void ROSVisualizer::publishOdomPoses(PathPoses poses, int param, string name, int64_t utime){
+
+    nav_msgs::Path path_msg;
+    int secs = utime*1E-6;
+    int nsecs = (utime - (secs*1E6))*1E3;
+    path_msg.header.stamp = ros::Time(secs, nsecs);
+    path_msg.header.frame_id = fixed_frame_;
+
+    for (size_t i = 0; i < poses.size(); ++i){
+        geometry_msgs::PoseStamped m;
+        m.header.stamp = ros::Time(secs, nsecs);
+        m.header.frame_id = fixed_frame_;
+        tf::poseEigenToMsg(poses[i], m.pose);
+        path_msg.poses.push_back(m);
+    }
+
+    odom_pose_pub_.publish(path_msg);
+}
+
+
+void ROSVisualizer::publishPriorPoses(Eigen::Isometry3d pose, int param, std::string name, int64_t utime)
+{
+    prior_path_.push_back(pose);
+    publishPriorPoses(odom_path_, param, name, utime);
+}
+
+void ROSVisualizer::publishPriorPoses(PathPoses poses, int param, string name, int64_t utime){
+
+    nav_msgs::Path path_msg;
+    int secs = utime*1E-6;
+    int nsecs = (utime - (secs*1E6))*1E3;
+    path_msg.header.stamp = ros::Time(secs, nsecs);
+    path_msg.header.frame_id = fixed_frame_;
+
+    for (size_t i = 0; i < poses.size(); ++i){
+        geometry_msgs::PoseStamped m;
+        m.header.stamp = ros::Time(secs, nsecs);
+        m.header.frame_id = fixed_frame_;
+        tf::poseEigenToMsg(poses[i], m.pose);
+        path_msg.poses.push_back(m);
+    }
+
+    prior_pose_pub_.publish(path_msg);
+}
+
+
+void ROSVisualizer::publishOdomToMapPose(Eigen::Isometry3d pose, int64_t utime)
+{
+    int secs = utime*1E-6;
+    int nsecs = (utime - (secs*1E6))*1E3;
+
+    geometry_msgs::PoseWithCovarianceStamped msg;
+    msg.header.stamp = ros::Time(secs, nsecs);
+    msg.header.frame_id = "odom";
+    tf::poseEigenToMsg(pose, msg.pose.pose);
+    odom_to_map_pub_.publish(msg);
+}
+
+
+void ROSVisualizer::publishFixedFrameToOdomPose(const Eigen::Isometry3d &fixed_frame_to_base_eigen,
+                                                ros::Time msg_time)
+{
+    Eigen::Isometry3d fixed_frame_to_odom_eigen;
+    this->computeFixedFrameToOdom(fixed_frame_to_base_eigen, fixed_frame_to_odom_eigen);
+
+    tf::poseEigenToTF(fixed_frame_to_odom_eigen, temp_tf_pose_);
+    tf::poseTFToMsg(temp_tf_pose_, fixed_to_odom_msg_.pose.pose);
+    fixed_to_odom_msg_.header.stamp = msg_time;
+    fixed_to_odom_msg_.header.frame_id = odom_frame_;
+    fixed_to_odom_pub_.publish(fixed_to_odom_msg_);
+}
+
+void ROSVisualizer::computeFixedFrameToOdom(const Eigen::Isometry3d &fixed_frame_to_base_eigen,
+                                            Eigen::Isometry3d& fixed_frame_to_odom_eigen)
 {
     // TF listener
     tf::StampedTransform base_to_odom_tf;
@@ -171,8 +258,14 @@ void ROSVisualizer::publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_
     tf::transformTFToEigen(base_to_odom_tf, base_to_odom_eigen);
 
     // Multiply
-    Eigen::Isometry3d fixed_frame_to_odom_eigen;
     fixed_frame_to_odom_eigen = fixed_frame_to_base_eigen * base_to_odom_eigen.inverse();
+}
+
+void ROSVisualizer::publishFixedFrameToOdomTF(const Eigen::Isometry3d& fixed_frame_to_base_eigen,
+                                              ros::Time msg_time)
+{
+    Eigen::Isometry3d fixed_frame_to_odom_eigen;
+    this->computeFixedFrameToOdom(fixed_frame_to_base_eigen, fixed_frame_to_odom_eigen);
 
     // Convert to TF
     tf::StampedTransform fixed_frame_to_odom_tf;