77 lines
2.0 KiB
C
77 lines
2.0 KiB
C
|
#ifndef POINT_CLOUD_LOCALIZATION_H
|
||
|
|
#define POINT_CLOUD_LOCALIZATION_H
|
||
|
|
#include <pcl/point_cloud.h>
|
||
|
|
#include <pcl/point_types.h>
|
||
|
|
//#include <ros/ros.h>
|
||
|
|
#include <geometry_utils/Transform3.h>
|
||
|
|
|
||
|
|
//#include <pcl_ros/point_cloud.h>
|
||
|
|
//#include <tf2_ros/transform_broadcaster.h>
|
||
|
|
#include"common.h"
|
||
|
|
|
||
|
|
class PointCloudLocalization {
|
||
|
|
public:
|
||
|
|
typedef pcl::PointCloud<pcl::POINT_TYPE> PointCloud;
|
||
|
|
|
||
|
|
PointCloudLocalization();
|
||
|
|
~PointCloudLocalization();
|
||
|
|
|
||
|
|
bool Initialize();
|
||
|
|
|
||
|
|
bool TransformPointsToFixedFrame(const PointCloud& points,
|
||
|
|
PointCloud* points_transformed) const;
|
||
|
|
|
||
|
|
bool TransformPointsToSensorFrame(const PointCloud& points,
|
||
|
|
PointCloud* points_transformed) const;
|
||
|
|
|
||
|
|
bool MotionUpdate(const geometry_utils::Transform3& incremental_odom);
|
||
|
|
|
||
|
|
bool MeasurementUpdate(const PointCloud::Ptr& query,
|
||
|
|
const PointCloud::Ptr& reference,
|
||
|
|
PointCloud* aligned_query);
|
||
|
|
|
||
|
|
const geometry_utils::Transform3& GetIncrementalEstimate() const;
|
||
|
|
const geometry_utils::Transform3& GetIntegratedEstimate() const;
|
||
|
|
|
||
|
|
void SetIntegratedEstimate(
|
||
|
|
const geometry_utils::Transform3& integrated_estimate);
|
||
|
|
|
||
|
|
private:
|
||
|
|
// Node initialization.
|
||
|
|
bool LoadParameters();
|
||
|
|
|
||
|
|
|
||
|
|
// The node's name.
|
||
|
|
std::string name_;
|
||
|
|
|
||
|
|
// Pose estimate.
|
||
|
|
geometry_utils::Transform3 incremental_estimate_;
|
||
|
|
geometry_utils::Transform3 integrated_estimate_;
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// Coordinate frames.
|
||
|
|
std::string fixed_frame_id_;
|
||
|
|
std::string base_frame_id_;
|
||
|
|
|
||
|
|
// Parameters for filtering and ICP.
|
||
|
|
struct Parameters {
|
||
|
|
// Stop ICP if the transformation from the last iteration was this small.
|
||
|
|
double tf_epsilon;
|
||
|
|
|
||
|
|
// During ICP, two points won't be considered a correspondence if they are
|
||
|
|
// at least this far from one another.
|
||
|
|
double corr_dist;
|
||
|
|
|
||
|
|
// Iterate ICP this many times.
|
||
|
|
unsigned int iterations;
|
||
|
|
} params_;
|
||
|
|
|
||
|
|
|
||
|
|
bool transform_thresholding_;
|
||
|
|
double max_translation_;
|
||
|
|
double max_rotation_;
|
||
|
|
};
|
||
|
|
|
||
|
|
#endif
|