Changed system structure so everything is in the same node

Did that because things started getting messy with map manager
Correspondence Estimation is working
Next step: Correspondence Rejection and figuring out how to give that info to state_estimator

CMakeLists.txt

@@ -65,7 +65,7 @@ catkin_package(
 # include_directories(include)
 include_directories(
   include
-  ${catkin_INCLUDE_DIRS} 
+  ${catkin_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
 )
@@ -87,7 +87,10 @@ include_directories(
 #   ${catkin_LIBRARIES}
 # )
 
-add_executable(lara_rgbd src/lara_rgbd/lara_rgbd_node.cpp src/lara_rgbd/sensor_processor/sensor_processor.cpp)
+add_executable(lara_rgbd src/lara_rgbd/lara_rgbd_node.cpp
+                         src/lara_rgbd/sensor_processor/sensor_processor.cpp
+                         src/lara_rgbd/state_estimator/state_estimator.cpp
+                         src/lara_rgbd/map_manager/map_manager.cpp)
 target_link_libraries(lara_rgbd ${catkin_LIBRARIES} ${PCL_LIBRARIES})
 
 #add_executable(sensor_processor src/sensor_processor/sensor_processor_node.cpp src/sensor_processor/sensor_processor.cpp)

include/lara_rgbd/lara_rgbd_node.h

@@ -23,11 +23,20 @@
 #include <pcl_conversions/pcl_conversions.h>
 
 #include <lara_rgbd/sensor_processor/sensor_processor.h>
+#include <lara_rgbd/state_estimator/state_estimator.h>
+#include <lara_rgbd/map_manager/map_manager.h>
 
 // ROS Subscriber Callbacks
 void cloud_cb(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg);
 
-ros::Publisher pub_feature_cloud;
+// ROS Publisher Functions
+void pub_fake_pose_estimate(const nav_msgs::Odometry::ConstPtr& odom_msg);
+void pub_pose_estimate();
+void pub_keyframes_estimate();
+void pub_feature_cloud_estimate();
 
+ros::Publisher pub_feature_cloud;
+ros::Publisher pub_pose;
+ros::Publisher pub_keyframes;
 
 #endif  // LARA_RGBD_SENSOR_PROCER_NODE_H_

include/lara_rgbd/map_manager/map_manager.h

@@ -0,0 +1,74 @@
+/**
+ * @file   state_estimator.h
+ * @author George Andrew Brindeiro ([email protected])
+ * @date   Apr 24, 2014
+ *
+ * @attention Copyright (C) 2014
+ * @attention Laboratório de Automação e Robótica (LARA)
+ * @attention Universidade de Brasília (UnB)
+ */
+
+#ifndef LARA_RGBD_MAP_MANAGER_H_
+#define LARA_RGBD_MAP_MANAGER_H_
+
+#include <Eigen/Dense>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/ros/conversions.h>
+
+// PCL Registration headers
+#include <pcl/filters/extract_indices.h>
+#include <pcl/sample_consensus/ransac.h>
+#include <pcl/sample_consensus/sac_model_registration.h>
+
+// kdtree search
+#include <pcl/search/kdtree.h>
+#include <pcl/registration/correspondence_estimation.h>
+
+#include <lara_rgbd_msgs/PoseWithCovarianceStampedArray.h>
+
+class MapManager
+{
+    public:
+        MapManager()
+        {
+            num_keyframes_ = 0;
+        }
+
+        ~MapManager() {}
+
+        bool match(Eigen::VectorXf& pose_vector, Eigen::MatrixXf& pose_covariance, pcl::PointCloud<pcl::PointWithScale> & keypoint_cloud, pcl::PointCloud<pcl::PFHRGBSignature250> & feature_cloud, Eigen::VectorXf& measurement_vector);
+        void update(Eigen::VectorXf state_vector);
+
+       /*!
+        * @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:
+        int num_keyframes_;                                     /**< Number of keyframes added to history */
+        std::vector< Eigen::VectorXf > keyframes_pose_;         /**< State vector for keyframes */
+        std::vector< Eigen::MatrixXf > keyframes_cov_;          /**< Covariance matrix for keyframes */
+        std::vector< pcl::PointCloud<pcl::PointWithScale> > keyframes_keypoints_;    /**< Keypoints cloud for keyframes */
+        std::vector< pcl::PointCloud<pcl::PFHRGBSignature250> > keyframes_features_;    /**< Feature cloud for keyframes */
+        std::vector< pcl::search::KdTree<pcl::PFHRGBSignature250> > keyframes_kdtree_;   /**< Feature kdtree for keyframes */
+
+};
+
+/*MapManager::match(pose_estimate, feature_cloud)
+{
+    // for every cloud in db, estimate and reject correspondences
+        // add surviving correspondences to measurement difference vector: current(transformed)-past
+        // covariance for each match: comes from ransac rejection?
+}
+
+MapManager::update(state_estimate, <cloud,feature_cloud>)
+{
+    // for every pose in state_estimate, copy to nearest-neighbor kdtree
+    // if state_estimate size changed, add pose+cloud+feature_cloud to kdtree
+}*/
+
+#endif  // LARA_RGBD_MAP_MANAGER_H_

include/lara_rgbd/sensor_processor/sensor_processor.h

@@ -11,7 +11,6 @@
 
 #include <iostream>
 #include <queue>
-#include <utility>
 
 class SensorProcessor
 {
@@ -22,7 +21,8 @@ class SensorProcessor
         }
 
         bool enqueue(const pcl::PointCloud<pcl::PointXYZRGB>& cloud);
-        bool process(std::pair< pcl::PointCloud<pcl::PointXYZRGB>, pcl::PointCloud<pcl::PointWithScale> >& processed_clouds);
+        bool process(pcl::PointCloud<pcl::PointWithScale>& keypoint_cloud,
+                     pcl::PointCloud<pcl::PFHRGBSignature250>& feature_cloud);
 
     private:
         void init();

include/state_estimator/state_estimator.h → include/lara_rgbd/state_estimator/state_estimator.h

@@ -26,34 +26,11 @@
 
 #include <Eigen/Dense>
 
-#include <lara_rgbd_msgs/PoseWithCovarianceStampedArray.h>
-
-// PCL Registration headers
-//#include <pcl/filters/extract_indices.h>
-//#include <pcl/sample_consensus/ransac.h>
-//#include <pcl/sample_consensus/sac_model_registration.h>
-
-// kdtree search
-//#include <pcl/kdtree/kdtree_flann.h>
-
-// PCL features
-#include <pcl/features/normal_3d.h>
-
-// PCL visualizer
-#include <boost/thread/thread.hpp>
-#include <pcl/common/common_headers.h>
-#include <pcl/features/normal_3d.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/console/parse.h>
-
 class StateEstimator
 {
     public:
         StateEstimator()
         {
-            num_keyframes_ = 0;
-
             init_state_estimate();
         }
 
@@ -67,6 +44,11 @@ class StateEstimator
         */
         void init_state_estimate();
 
+        inline void predict()
+        {
+            quasiconstant_motion_model();
+        }
+
         /*!
         * @brief Updates the state estimate according to the Quasiconstant Motion Model
         *
@@ -95,50 +77,30 @@ class StateEstimator
         */
         void cloud_measurement_model(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg);
 
-        // From feature matching example
-        /**
-         * @brief find corresponding features based on some metric
-         * @param source source feature descriptors
-         * @param target target feature descriptors
-         * @param correspondences indices out of the target descriptors that correspond (nearest neighbor) to the source descriptors
-         */
-        //void findCorrespondences (typename pcl::PointCloud<FeatureType>::Ptr source, typename pcl::PointCloud<FeatureType>::Ptr target, std::vector<int>& correspondences) const;
-
-        /**
-         * @brief  remove non-consistent correspondences
-         */
-        //void filterCorrespondences ();
-
         /*!
         * @brief Repackages estimated pose into a ROS message for publishing
         *
         * Repackages current pose estimate into a geometry_msgs::PoseWithCovarianceStamped ROS message for publishing
         */
         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);
+        inline void pose_and_covariance(Eigen::VectorXf& pose_vector, Eigen::MatrixXf& pose_covariance)
+        {
+            pose_vector = state_estimate_.head(7);
+            pose_covariance = cov_state_estimate_.block(0,0,7,7);
+        }
+
+        void augment_state_vector();
 
     private:
         /*!
         * @brief Performs state vector augmentation using current pose
         *
         * Performs state vector augmentation using current pose, as described in LARA RGB-D SLAM
         */
-        void augment_state_vector_();
 
         Eigen::VectorXf state_estimate_;       /**< State vector for LARA RGB-D SLAM  */
         Eigen::MatrixXf cov_state_estimate_;   /**< State vector covariance matrix for LARA RGB-D SLAM  */
-
-        int num_keyframes_;                                     /**< Number of keyframes added to history */
-        std::vector< Eigen::VectorXf > keyframes_pose_;         /**< State vector for keyframes */
-        std::vector< Eigen::MatrixXf > keyframes_cov_;          /**< Covariance matrix for keyframes */
-        //std::vector< pcl::PointCloud<pcl::PointXYZRGB> > keyframes_cloud_;    /**< Feature cloud for keyframes */
-        //std::vector< pcl::KdTreeFLANN<pcl::PointXYZRGB> > keyframes_kdtree_;   /**< Feature kdtree for keyframes */
 };
 
 #endif  // LARA_RGBD_STATE_ESTIMATOR_H_