Added basic keyframe visualization functionality

Still need to check for memory leaks...

include/state_estimator/state_estimator.h

@@ -26,6 +26,8 @@
 
 #include <Eigen/Dense>
 
+#include <lara_rgbd_msgs/PoseWithCovarianceStampedArray.h>
+
 class StateEstimator
 {
     public:
@@ -81,6 +83,13 @@ class StateEstimator
         */
         void estimated_pose(geometry_msgs::PoseWithCovarianceStamped& current_pose);
 
+        /*!
+        * @brief Repackages keyframes into a ROS message for publishing
+        *
+        * Repackages keyframes into a geometry_msgs::PoseWithCovarianceStampedArray ROS message for publishing
+        */
+        void keyframes(lara_rgbd_msgs::PoseWithCovarianceStampedArray& keyframes);
+
     private:
         /*!
         * @brief Performs state vector augmentation using current pose

include/state_estimator/state_estimator_node.h

@@ -22,8 +22,10 @@ void odom_cb(const nav_msgs::Odometry::ConstPtr& odom_msg);
 // ROS Publisher Functions
 void pub_fake_pose_estimate(const nav_msgs::Odometry::ConstPtr& odom_msg);
 void pub_pose_estimate();
+void pub_keyframes_estimate();
 
 ros::Publisher pub_pose;
+ros::Publisher pub_keyframes;
 
 StateEstimator* state_estimator;
 

launch/settings.rviz

@@ -170,13 +170,21 @@ Visualization Manager:
       Use rainbow: true
       Value: true
     - Alpha: 0.5
-      Class: rviz_plugin_covariance/PoseWithCovariance
+      Class: rviz_lara_rgbd/PoseWithCovariance
       Color: 204; 51; 204
       Enabled: true
       Name: PoseWithCovariance
       Scale: 1
       Topic: /lara_rgbd/estimated_pose
       Value: true
+    - Alpha: 0.5
+      Class: rviz_lara_rgbd/PoseWithCovarianceArray
+      Color: 255; 255; 0
+      Enabled: true
+      Name: PoseWithCovarianceArray
+      Scale: 1
+      Topic: /lara_rgbd/keyframes
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

src/state_estimator/state_estimator.cpp

@@ -79,7 +79,7 @@ void StateEstimator::cloud_measurement_model(const sensor_msgs::PointCloud2::Con
         // Separate good matches and go through cloud matching model equations
     }
 
-    if((num_keyframes_ == 0) || !matches_found)
+    if((num_keyframes_ == 0)/* || !matches_found*/)
     {
         // Augment state vector with initial pose
         augment_state_vector_();
@@ -115,6 +115,46 @@ void StateEstimator::estimated_pose(geometry_msgs::PoseWithCovarianceStamped& cu
     current_pose.pose.covariance[30+5] = 0.01;
 }
 
+void StateEstimator::keyframes(lara_rgbd_msgs::PoseWithCovarianceStampedArray& keyframes)
+{
+    char frame_id[20];
+
+    for(int i = 0; i < num_keyframes_; i++)
+    {
+        geometry_msgs::PoseWithCovarianceStamped keyframe;
+
+        sprintf(frame_id, "/base_link_%d", i);
+
+        keyframe.header.frame_id = frame_id;
+        keyframe.header.stamp = keyframes.header.stamp;
+
+        keyframe.pose.pose.position.x = keyframes_pose_[i](0);
+        keyframe.pose.pose.position.y = keyframes_pose_[i](1);
+        keyframe.pose.pose.position.z = keyframes_pose_[i](2);
+        keyframe.pose.pose.orientation.w = keyframes_pose_[i](3);
+        keyframe.pose.pose.orientation.x = keyframes_pose_[i](4);
+        keyframe.pose.pose.orientation.y = keyframes_pose_[i](5);
+        keyframe.pose.pose.orientation.z = keyframes_pose_[i](6);
+
+        ROS_DEBUG("keyframes not fully implemented. returning fixed covariance for orientation!");
+        for(int ii = 0; ii < 3; ii++)
+        {
+            for(int jj = 0; jj < 3; jj++)
+            {
+                keyframe.pose.covariance[6*ii+jj] = keyframes_cov_[i](ii,jj);
+            }
+        }
+
+        keyframe.pose.covariance[18+3] = 0.01;
+        keyframe.pose.covariance[24+4] = 0.01;
+        keyframe.pose.covariance[30+5] = 0.01;
+
+        //ROS_INFO_STREAM_THROTTLE(1, "keyframe #" << i << ":\n\n" << keyframe << "\n");
+
+        keyframes.poses.push_back(keyframe);
+    }
+}
+
 void StateEstimator::augment_state_vector_()
 {
     int n = state_estimate_.size();

src/state_estimator/state_estimator_node.cpp

@@ -28,6 +28,9 @@ int main (int argc, char** argv)
     // Create a ROS publisher for estimated pose
     pub_pose = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("estimated_pose", 1);
 
+    // Create a ROS publisher for keyframes
+    pub_keyframes = nh.advertise<lara_rgbd_msgs::PoseWithCovarianceStampedArray>("keyframes", 1);
+
     // Create SensorProcessor
     state_estimator = new StateEstimator();
 
@@ -39,6 +42,7 @@ int main (int argc, char** argv)
         state_estimator->quasiconstant_motion_model();
 
         pub_pose_estimate();
+        pub_keyframes_estimate();
 
         ros::spinOnce();
         loop_rate.sleep();
@@ -54,18 +58,19 @@ void odom_cb(const nav_msgs::Odometry::ConstPtr& odom_msg)
 {
     state_estimator->odom_motion_model(odom_msg);
 
-    //pub_fake_pose_estimate(odom_msg);
+    pub_fake_pose_estimate(odom_msg);
 }
 
 void pub_fake_pose_estimate(const nav_msgs::Odometry::ConstPtr& odom_msg)
 {
     // Fake pose estimate from odometry
     geometry_msgs::PoseWithCovarianceStamped::Ptr estimated_pose_msg(new geometry_msgs::PoseWithCovarianceStamped);
     estimated_pose_msg->header = odom_msg->header;
+    estimated_pose_msg->header.frame_id = "world";
     estimated_pose_msg->pose = odom_msg->pose;
 
-    estimated_pose_msg->pose.covariance[0] = 10;
-    estimated_pose_msg->pose.covariance[6+1] = 10;
+    estimated_pose_msg->pose.covariance[0] = 1;
+    estimated_pose_msg->pose.covariance[6+1] = 1;
     estimated_pose_msg->pose.covariance[12+2] = 0.1;
     estimated_pose_msg->pose.covariance[18+3] = 0.01;
     estimated_pose_msg->pose.covariance[24+4] = 0.01;
@@ -85,3 +90,14 @@ void pub_pose_estimate()
 
     pub_pose.publish(current_pose);
 }
+
+void pub_keyframes_estimate()
+{
+    lara_rgbd_msgs::PoseWithCovarianceStampedArray keyframes;
+
+    keyframes.header.stamp = ros::Time::now();
+    keyframes.header.frame_id = "world";
+    state_estimator->keyframes(keyframes);
+
+    pub_keyframes.publish(keyframes);
+}