8 #ifndef CRH_ALIGNMENT_H_
9 #define CRH_ALIGNMENT_H_
11 #include <pcl/common/common.h>
12 #include <pcl/features/crh.h>
13 #include <pcl/common/fft/kiss_fftr.h>
14 #include <pcl/common/transforms.h>
30 template<
typename Po
intT,
int nbins_>
39 operator() (std::pair<float, int>
const& a, std::pair<float, int>
const& b)
41 return a.first > b.first;
48 PointTPtr target_view_;
50 PointTPtr input_view_;
52 Eigen::Vector3f centroid_target_;
54 Eigen::Vector3f centroid_input_;
56 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transforms_;
64 float accept_threshold_;
71 computeTransformToZAxes (Eigen::Vector3f & centroid, Eigen::Affine3f & transform)
73 Eigen::Vector3f plane_normal;
74 plane_normal[0] = -centroid[0];
75 plane_normal[1] = -centroid[1];
76 plane_normal[2] = -centroid[2];
77 Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
78 plane_normal.normalize ();
79 Eigen::Vector3f axis = plane_normal.cross (z_vector);
80 double rotation = -asin (axis.norm ());
82 transform = Eigen::Affine3f (Eigen::AngleAxisf (static_cast<float>(rotation), axis));
92 computeRollTransform (Eigen::Vector3f & centroidInput, Eigen::Vector3f & centroidResult,
double roll_angle, Eigen::Affine3f & final_trans)
94 Eigen::Affine3f transformInputToZ;
95 computeTransformToZAxes (centroidInput, transformInputToZ);
97 transformInputToZ = transformInputToZ.inverse ();
98 Eigen::Affine3f transformRoll (Eigen::AngleAxisf (-static_cast<float>(roll_angle * M_PI / 180), Eigen::Vector3f::UnitZ ()));
99 Eigen::Affine3f transformDBResultToZ;
100 computeTransformToZAxes (centroidResult, transformDBResultToZ);
102 final_trans = transformInputToZ * transformRoll * transformDBResultToZ;
110 accept_threshold_ = 0.8f;
116 void getTransforms(std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transforms) {
117 transforms = transforms_;
127 target_view_ = target_view;
128 input_view_ = input_view;
138 centroid_target_ = c2;
139 centroid_input_ = c1;
152 std::vector<float> peaks;
153 computeRollAngle (input_ftt, target_ftt, peaks);
157 for (
size_t i = 0; i < peaks.size(); i++)
159 Eigen::Affine3f rollToRot;
160 computeRollTransform (centroid_input_, centroid_target_, peaks[i], rollToRot);
162 Eigen::Matrix4f rollHomMatrix = Eigen::Matrix4f ();
163 rollHomMatrix.setIdentity (4, 4);
164 rollHomMatrix = rollToRot.matrix ();
166 Eigen::Matrix4f translation2;
167 translation2.setIdentity (4, 4);
168 Eigen::Vector3f centr = rollToRot * centroid_target_;
169 translation2 (0, 3) = centroid_input_[0] - centr[0];
170 translation2 (1, 3) = centroid_input_[1] - centr[1];
171 translation2 (2, 3) = centroid_input_[2] - centr[2];
173 Eigen::Matrix4f resultHom (translation2 * rollHomMatrix);
174 transforms_.push_back(resultHom.inverse());
186 std::vector<float> & peaks)
191 for (
int i = 2; i < (nbins_); i += 2)
192 input_ftt_negate.
points[0].histogram[i] = -input_ftt_negate.
points[0].histogram[i];
194 int nr_bins_after_padding = 180;
195 int peak_distance = 5;
196 int cutoff = nbins_ - 1;
199 for (
int i = 0; i < nr_bins_after_padding; i++)
200 multAB[i].r = multAB[i].i = 0.f;
203 multAB[k].
r = input_ftt_negate.
points[0].histogram[0] * target_ftt.points[0].histogram[0];
207 for (
int i = 1; i < cutoff; i += 2, k++)
209 a = input_ftt_negate.
points[0].histogram[i];
210 b = input_ftt_negate.
points[0].histogram[i + 1];
211 c = target_ftt.points[0].histogram[i];
212 d = target_ftt.points[0].histogram[i + 1];
213 multAB[k].r = a * c - b * d;
214 multAB[k].i = b * c + a * d;
216 float tmp = sqrtf (multAB[k].r * multAB[k].r + multAB[k].i * multAB[k].i);
222 multAB[nbins_ - 1].r = input_ftt_negate.
points[0].histogram[nbins_ - 1] * target_ftt.points[0].histogram[nbins_ - 1];
224 kiss_fft_cfg mycfg = kiss_fft_alloc (nr_bins_after_padding, 1, NULL, NULL);
226 kiss_fft (mycfg, multAB, invAB);
228 std::vector < std::pair<float, int> > scored_peaks (nr_bins_after_padding);
229 for (
int i = 0; i < nr_bins_after_padding; i++)
230 scored_peaks[i] = std::make_pair (invAB[i].r, i);
232 std::sort (scored_peaks.begin (), scored_peaks.end (), peaks_ordering ());
234 std::vector<int> peaks_indices;
235 std::vector<float> peaks_values;
238 float quantile = quantile_;
239 int max_inserted= max_peaks_;
243 for (
int i = 0; (i < static_cast<int> (quantile *
static_cast<float> (nr_bins_after_padding))) && !stop; i++)
245 if (scored_peaks[i].first >= scored_peaks[0].first * accept_threshold_)
249 for (
size_t j = 0; j < peaks_indices.size (); j++)
251 if (std::abs (peaks_indices[j] - scored_peaks[i].second) <= peak_distance || std::abs (
252 peaks_indices[j] - (scored_peaks[i].second
253 - nr_bins_after_padding)) <= peak_distance)
262 peaks_indices.push_back (scored_peaks[i].second);
263 peaks_values.push_back (scored_peaks[i].first);
264 peaks.push_back (static_cast<float> (scored_peaks[i].second * (360 / nr_bins_after_padding)));
266 if(inserted >= max_inserted)
void getTransforms(std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &transforms)
returns the computed transformations
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 modle.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setInputAndTargetView(PointTPtr &input_view, PointTPtr &target_view)
sets model and input views
void align(pcl::PointCloud< pcl::Histogram< nbins_ > > &input_ftt, pcl::PointCloud< pcl::Histogram< nbins_ > > &target_ftt)
Computes the transformation aligning model to input.
A point structure representing an N-D histogram.
CRHAlignment()
Constructor.
void setInputAndTargetCentroids(Eigen::Vector3f &c1, Eigen::Vector3f &c2)
sets model and input centroids
CRHAlignment uses two Camera Roll Histograms (CRH) to find the roll rotation that aligns both views...
boost::shared_ptr< PointCloud< PointT > > Ptr
PointCloud represents the base class in PCL for storing collections of 3D points. ...