11#include <pcl/features/crh.h>
12#include <pcl/common/fft/kiss_fftr.h>
13#include <pcl/common/transforms.h>
29 template<
typename Po
intT,
int nbins_>
38 operator() (std::pair<float, int>
const& a, std::pair<float, int>
const& b)
40 return a.first > b.first;
47 PointTPtr target_view_;
49 PointTPtr input_view_;
51 Eigen::Vector3f centroid_target_;
53 Eigen::Vector3f centroid_input_;
55 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transforms_;
63 float accept_threshold_;
70 computeTransformToZAxes (Eigen::Vector3f & centroid, Eigen::Affine3f & transform)
72 Eigen::Vector3f plane_normal;
73 plane_normal[0] = -centroid[0];
74 plane_normal[1] = -centroid[1];
75 plane_normal[2] = -centroid[2];
76 Eigen::Vector3f
z_vector = Eigen::Vector3f::UnitZ ();
77 plane_normal.normalize ();
81 transform = Eigen::Affine3f (Eigen::AngleAxisf (
static_cast<float>(
rotation),
axis));
109 accept_threshold_ = 0.8f;
137 centroid_target_ = c2;
138 centroid_input_ = c1;
151 std::vector<float>
peaks;
159 computeRollTransform (centroid_input_, centroid_target_,
peak,
rollToRot);
173 transforms_.push_back(
resultHom.inverse());
185 std::vector<float> &
peaks)
190 for (
int i = 2; i < (nbins_); i += 2)
206 for (
int i = 1; i <
cutoff; i += 2, k++)
CRHAlignment uses two Camera Roll Histograms (CRH) to find the roll rotation that aligns both views.
void setInputAndTargetCentroids(Eigen::Vector3f &c1, Eigen::Vector3f &c2)
sets model and input centroids
void align(pcl::PointCloud< pcl::Histogram< nbins_ > > &input_ftt, pcl::PointCloud< pcl::Histogram< nbins_ > > &target_ftt)
Computes the transformation aligning model to input.
void computeRollAngle(pcl::PointCloud< pcl::Histogram< nbins_ > > &input_ftt, pcl::PointCloud< pcl::Histogram< nbins_ > > &target_ftt, std::vector< float > &peaks)
Computes the roll angle that aligns input to model.
CRHAlignment()
Constructor.
void setInputAndTargetView(PointTPtr &input_view, PointTPtr &target_view)
sets model and input views
void getTransforms(std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &transforms)
returns the computed transformations
Iterator class for point clouds with or without given indices.
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
Define standard C methods and C++ classes that are common to all methods.