83 lines
1.9 KiB
C
83 lines
1.9 KiB
C
|
#ifndef POINT_CLOUD_ODOMETRY_H
|
||
|
|
#define POINT_CLOUD_ODOMETRY_H
|
||
|
|
|
||
|
|
#include <pcl/point_cloud.h>
|
||
|
|
#include <pcl/point_types.h>
|
||
|
|
|
||
|
|
#include <pcl/registration/icp.h>
|
||
|
|
|
||
|
|
#include <geometry_utils/Transform3.h>
|
||
|
|
#include"common.h"
|
||
|
|
//#include <pcl_ros/point_cloud.h>
|
||
|
|
//#include <tf2_ros/transform_broadcaster.h>
|
||
|
|
|
||
|
|
class PointCloudOdometry {
|
||
|
|
public:
|
||
|
|
typedef pcl::PointCloud<pcl::POINT_TYPE> PointCloud;
|
||
|
|
|
||
|
|
PointCloudOdometry();
|
||
|
|
~PointCloudOdometry();
|
||
|
|
|
||
|
|
bool Initialize(/*const ros::NodeHandle& n*/);
|
||
|
|
|
||
|
|
bool UpdateEstimate(const PointCloud& points);
|
||
|
|
|
||
|
|
// Get pose estimates.
|
||
|
|
const geometry_utils::Transform3& GetIncrementalEstimate() const;
|
||
|
|
const geometry_utils::Transform3& GetIntegratedEstimate() const;
|
||
|
|
|
||
|
|
// not initialized.
|
||
|
|
bool GetLastPointCloud(PointCloud::Ptr& out) const;
|
||
|
|
|
||
|
|
private:
|
||
|
|
// Node initialization.
|
||
|
|
bool LoadParameters(/*const ros::NodeHandle& n*/);
|
||
|
|
|
||
|
|
bool UpdateICP();
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// The node's name.
|
||
|
|
std::string name_;
|
||
|
|
|
||
|
|
// Pose estimates.
|
||
|
|
geometry_utils::Transform3 integrated_estimate_;
|
||
|
|
geometry_utils::Transform3 incremental_estimate_;
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// Coordinate frames.
|
||
|
|
std::string fixed_frame_id_;
|
||
|
|
std::string odometry_frame_id_;
|
||
|
|
|
||
|
|
|
||
|
|
// For initialization.
|
||
|
|
bool initialized_;
|
||
|
|
|
||
|
|
// Point cloud containers.
|
||
|
|
PointCloud::Ptr query_;
|
||
|
|
PointCloud::Ptr reference_;
|
||
|
|
|
||
|
|
// Parameters for filtering, and ICP.
|
||
|
|
struct Parameters {
|
||
|
|
// Stop ICP if the transformation from the last iteration was this small.
|
||
|
|
double icp_tf_epsilon;
|
||
|
|
|
||
|
|
// During ICP, two points won't be considered a correspondence if they are
|
||
|
|
// at least this far from one another.
|
||
|
|
double icp_corr_dist;
|
||
|
|
|
||
|
|
// Iterate ICP this many times.
|
||
|
|
unsigned int icp_iterations;
|
||
|
|
} params_;
|
||
|
|
|
||
|
|
// Maximum acceptable translation and rotation tolerances. If
|
||
|
|
// transform_thresholding_ is set to false, neither of these thresholds are
|
||
|
|
// considered.
|
||
|
|
bool transform_thresholding_;
|
||
|
|
double max_translation_;
|
||
|
|
double max_rotation_;
|
||
|
|
};
|
||
|
|
|
||
|
|
#endif
|