crh_alignment.h

Go to the documentation of this file.

/*
 * crh_recognition.h
 *
 *  Created on: Mar 30, 2012
 *      Author: aitor
 */

#ifndef CRH_ALIGNMENT_H_
#define CRH_ALIGNMENT_H_

#include <pcl/common/common.h>
#include <pcl/features/crh.h>
#include <pcl/common/fft/kiss_fftr.h>
#include <pcl/common/transforms.h>

namespace pcl
{

  template<typename PointT, int nbins_>
    class PCL_EXPORTS CRHAlignment
    {
    private:

      typedef struct
      {
        bool
        operator() (std::pair<float, int> const& a, std::pair<float, int> const& b)
        {
          return a.first > b.first;
        }
      } peaks_ordering;

      typedef typename pcl::PointCloud<PointT>::Ptr PointTPtr;

      PointTPtr target_view_;
      PointTPtr input_view_;
      Eigen::Vector3f centroid_target_;
      Eigen::Vector3f centroid_input_;
      std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transforms_;
      int max_peaks_;
      float quantile_;
      float accept_threshold_;

      void
      computeTransformToZAxes (Eigen::Vector3f & centroid, Eigen::Affine3f & transform)
      {
        Eigen::Vector3f plane_normal;
        plane_normal[0] = -centroid[0];
        plane_normal[1] = -centroid[1];
        plane_normal[2] = -centroid[2];
        Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
        plane_normal.normalize ();
        Eigen::Vector3f axis = plane_normal.cross (z_vector);
        double rotation = -asin (axis.norm ());
        axis.normalize ();
        transform = Eigen::Affine3f (Eigen::AngleAxisf (static_cast<float>(rotation), axis));
      }

      void
      computeRollTransform (Eigen::Vector3f & centroidInput, Eigen::Vector3f & centroidResult, double roll_angle, Eigen::Affine3f & final_trans)
      {
        Eigen::Affine3f transformInputToZ;
        computeTransformToZAxes (centroidInput, transformInputToZ);

        transformInputToZ = transformInputToZ.inverse ();
        Eigen::Affine3f transformRoll (Eigen::AngleAxisf (-static_cast<float>(roll_angle * M_PI / 180), Eigen::Vector3f::UnitZ ()));
        Eigen::Affine3f transformDBResultToZ;
        computeTransformToZAxes (centroidResult, transformDBResultToZ);

        final_trans = transformInputToZ * transformRoll * transformDBResultToZ;
      }
    public:

      CRHAlignment() {
        max_peaks_ = 5;
        quantile_ = 0.2f;
        accept_threshold_ = 0.8f;
      }

      void getTransforms(std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transforms) {
        transforms = transforms_;
      }

      void
      setInputAndTargetView (PointTPtr & input_view, PointTPtr & target_view)
      {
        target_view_ = target_view;
        input_view_ = input_view;
      }

      void
      setInputAndTargetCentroids (Eigen::Vector3f & c1, Eigen::Vector3f & c2)
      {
        centroid_target_ = c2;
        centroid_input_ = c1;
      }

      void
      align (pcl::PointCloud<pcl::Histogram<nbins_> > & input_ftt, pcl::PointCloud<pcl::Histogram<nbins_> > & target_ftt)
      {

        transforms_.clear(); //clear from last round...

        std::vector<float> peaks;
        computeRollAngle (input_ftt, target_ftt, peaks);

        //if the number of peaks is too big, we should try to reduce using siluette matching

        for (size_t i = 0; i < peaks.size(); i++)
        {
          Eigen::Affine3f rollToRot;
          computeRollTransform (centroid_input_, centroid_target_, peaks[i], rollToRot);

          Eigen::Matrix4f rollHomMatrix = Eigen::Matrix4f ();
          rollHomMatrix.setIdentity (4, 4);
          rollHomMatrix = rollToRot.matrix ();

          Eigen::Matrix4f translation2;
          translation2.setIdentity (4, 4);
          Eigen::Vector3f centr = rollToRot * centroid_target_;
          translation2 (0, 3) = centroid_input_[0] - centr[0];
          translation2 (1, 3) = centroid_input_[1] - centr[1];
          translation2 (2, 3) = centroid_input_[2] - centr[2];

          Eigen::Matrix4f resultHom (translation2 * rollHomMatrix);
          transforms_.push_back(resultHom.inverse());
        }

      }

      void
      computeRollAngle (pcl::PointCloud<pcl::Histogram<nbins_> > & input_ftt, pcl::PointCloud<pcl::Histogram<nbins_> > & target_ftt,
                        std::vector<float> & peaks)
      {

        pcl::PointCloud<pcl::Histogram<nbins_> > input_ftt_negate (input_ftt);

        for (int i = 2; i < (nbins_); i += 2)
          input_ftt_negate.points[0].histogram[i] = -input_ftt_negate.points[0].histogram[i];

        int nr_bins_after_padding = 180;
        int peak_distance = 5;
        int cutoff = nbins_ - 1;

        kiss_fft_cpx * multAB = new kiss_fft_cpx[nr_bins_after_padding];
        for (int i = 0; i < nr_bins_after_padding; i++)
          multAB[i].r = multAB[i].i = 0.f;

        int k = 0;
        multAB[k].r = input_ftt_negate.points[0].histogram[0] * target_ftt.points[0].histogram[0];
        k++;

        float a, b, c, d;
        for (int i = 1; i < cutoff; i += 2, k++)
        {
          a = input_ftt_negate.points[0].histogram[i];
          b = input_ftt_negate.points[0].histogram[i + 1];
          c = target_ftt.points[0].histogram[i];
          d = target_ftt.points[0].histogram[i + 1];
          multAB[k].r = a * c - b * d;
          multAB[k].i = b * c + a * d;

          float tmp = sqrtf (multAB[k].r * multAB[k].r + multAB[k].i * multAB[k].i);

          multAB[k].r /= tmp;
          multAB[k].i /= tmp;
        }

        multAB[nbins_ - 1].r = input_ftt_negate.points[0].histogram[nbins_ - 1] * target_ftt.points[0].histogram[nbins_ - 1];

        kiss_fft_cfg mycfg = kiss_fft_alloc (nr_bins_after_padding, 1, NULL, NULL);
        kiss_fft_cpx * invAB = new kiss_fft_cpx[nr_bins_after_padding];
        kiss_fft (mycfg, multAB, invAB);

        std::vector < std::pair<float, int> > scored_peaks (nr_bins_after_padding);
        for (int i = 0; i < nr_bins_after_padding; i++)
          scored_peaks[i] = std::make_pair<float, int> (invAB[i].r, i);

        std::sort (scored_peaks.begin (), scored_peaks.end (), peaks_ordering ());

        std::vector<int> peaks_indices;
        std::vector<float> peaks_values;

        // we look at the upper quantile_
        float quantile = quantile_;
        int max_inserted= max_peaks_;

        int inserted=0;
        bool stop=false;
        for (int i = 0; (i < static_cast<int> (quantile * static_cast<float> (nr_bins_after_padding))) && !stop; i++)
        {
          if (scored_peaks[i].first >= scored_peaks[0].first * accept_threshold_)
          {
            bool insert = true;

            for (size_t j = 0; j < peaks_indices.size (); j++)
            { //check inserted peaks, first pick always inserted
              if (std::abs (peaks_indices[j] - scored_peaks[i].second) <= peak_distance || std::abs (
                                                                                             peaks_indices[j] - (scored_peaks[i].second
                                                                                                 - nr_bins_after_padding)) <= peak_distance)
              {
                insert = false;
                break;
              }
            }

            if (insert)
            {
              peaks_indices.push_back (scored_peaks[i].second);
              peaks_values.push_back (scored_peaks[i].first);
              peaks.push_back (static_cast<float> (scored_peaks[i].second * (360 / nr_bins_after_padding)));
              inserted++;
              if(inserted >= max_inserted)
                stop = true;
            }
          }
        }
      }
    };
}

#endif /* CRH_ALIGNMENT_H_ */