odom_estimation.hpp

Go to the documentation of this file.

// Original Author of FLOAM: Wang Han 
// Email wh200720041@gmail.com
// Homepage https://wanghan.pro

#ifndef FLOAM__ODOM_ESTIMATION_HPP_
#define FLOAM__ODOM_ESTIMATION_HPP_

//std lib
#include <string>
#include <math.h>
#include <vector>

//PCL
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/crop_box.h>

//ceres
#include <ceres/ceres.h>
#include <ceres/rotation.h>

//eigen
#include <Eigen/Dense>
#include <Eigen/Geometry>

//LOCAL LIB
#include "floam/lidar_optimization.hpp"
#include "floam/lidar_utils.hpp"

namespace floam
{
namespace odom
{

class OdomEstimation
{
public:
  OdomEstimation();
  ~OdomEstimation() {};

  void init(const double & mapResolution);      
  void initMapWithPoints(const pcl::PointCloud<pcl::PointXYZ>::Ptr & edges, const pcl::PointCloud<pcl::PointXYZ>::Ptr & surfaces);
  void updatePointsToMap(const pcl::PointCloud<pcl::PointXYZ>::Ptr & edges, const pcl::PointCloud<pcl::PointXYZ>::Ptr & surfaces);
  void getMap(pcl::PointCloud<pcl::PointXYZ>::Ptr & lidarCloudMap);

  Eigen::Isometry3d m_odom, m_lastOdom;

  double m_parameters[7] = {0, 0, 0, 1, 0, 0, 0};

  Eigen::Map<Eigen::Quaterniond> m_currentRotation = Eigen::Map<Eigen::Quaterniond>(m_parameters);
  Eigen::Map<Eigen::Vector3d> m_currentTranslation = Eigen::Map<Eigen::Vector3d>(m_parameters + 4);

  pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr m_kdTreeEdgeMap;
  pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr m_kdTreeSurfMap;

  pcl::PointCloud<pcl::PointXYZ>::Ptr m_lidarCloudCornerMap;
  pcl::PointCloud<pcl::PointXYZ>::Ptr m_lidarCloudSurfMap;

  pcl::VoxelGrid<pcl::PointXYZ> m_downSizeFilterEdge;
  pcl::VoxelGrid<pcl::PointXYZ> m_downSizeFilterSurf;

  pcl::CropBox<pcl::PointXYZ> m_cropBoxFilter;

  int m_optimizationCount;

  floam::lidar::Total m_totals;

  void addEdgeCostFactor(
          const pcl::PointCloud<pcl::PointXYZ>::Ptr & points,
          const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
          ceres::Problem & problem,
          ceres::LossFunction * lossFunction);

  void addSurfCostFactor(
          const pcl::PointCloud<pcl::PointXYZ>::Ptr & points,
          const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
          ceres::Problem & problem,
          ceres::LossFunction * lossFunction);

  // TODO(flynneva): are these even used? I think these were moved to the mapping class
  void addPointsToMap(
          const pcl::PointCloud<pcl::PointXYZ>::Ptr & downsampledEdgeCloud,
          const pcl::PointCloud<pcl::PointXYZ>::Ptr & downsampledSurfCloud);

  // TODO(flynneva): are these even used? I think these were moved to the mapping class
  void pointAssociateToMap(
          const std::shared_ptr<pcl::PointXYZ> & pointsIn,
          std::shared_ptr<pcl::PointXYZ> & pointsOut);

  // TODO(flynneva): are these even used? I think these were moved to the mapping class
  void downSamplingToMap(
          const pcl::PointCloud<pcl::PointXYZ>::Ptr & edgesIn,
          pcl::PointCloud<pcl::PointXYZ>::Ptr & edgesOut,
          const pcl::PointCloud<pcl::PointXYZ>::Ptr & surfacesIn,
          pcl::PointCloud<pcl::PointXYZ>::Ptr & surfacesOut);
};
}  // namespace odom
}  // namespace floam

#endif  // FLOAM__ODOM_ESTIMATION_HPP_