<commit_before>#include <pcl/features/fpfh.h>
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/registration/icp.h>
#include <pcl/common/time.h>

#include "proctor/detector.h"
#include "proctor/ia_ransac_sub.h"
#include "proctor/proctor.h"


namespace pcl {
  namespace proctor {
    /** create the viewports to be used by top candidates */
    class create_viewports {
    public:
      create_viewports(PointCloud<PointNormal>::ConstPtr sentinel) : sentinel(sentinel) {}
      void operator()(visualization::PCLVisualizer &v) {
        for (int ci = 0; ci < Detector::num_registration; ci++) {
          int vp;
          v.createViewPort(double(ci) / Detector::num_registration, 0, double(ci + 1) / Detector::num_registration, 1, vp);
          {
            stringstream ss;
            ss << "candidate_" << ci;
            v.addPointCloud(sentinel, visualization::PointCloudColorHandlerCustom<PointNormal>(sentinel, 0xc0, 0x00, 0x40), ss.str(), vp);
          }
          {
            stringstream ss;
            ss << "aligned_" << ci;
            v.addPointCloud(sentinel, visualization::PointCloudColorHandlerCustom<PointNormal>(sentinel, 0xc0, 0x00, 0x40), ss.str(), vp);
          }
        }
      }
    private:
      PointCloud<PointNormal>::ConstPtr sentinel;
    };

    /** show a candidate model as the specified candidate */
    class show_candidate {
    public:
      show_candidate(int ci, PointCloud<PointNormal>::ConstPtr candidate) : ci(ci), candidate(candidate) {}
      void operator()(visualization::PCLVisualizer &v) {
        {
          stringstream ss;
          ss << "candidate_" << ci;
          v.updatePointCloud(candidate, visualization::PointCloudColorHandlerCustom<PointNormal>(candidate, 0xc0, 0x00, 0x40), ss.str());
        }
        {
          stringstream ss;
          ss << "aligned_" << ci;
          v.updatePointCloud(candidate, visualization::PointCloudColorHandlerCustom<PointNormal>(candidate, 0xc0, 0x00, 0x40), ss.str());
        }
      }
    private:
      int ci;
      PointCloud<PointNormal>::ConstPtr candidate;
    };

    /** show an aligned point cloud in the specified viewport */
    class show_aligned {
    public:
      show_aligned(int ci, PointCloud<PointNormal>::ConstPtr aligned) : ci(ci), aligned(aligned) {}
      void operator()(visualization::PCLVisualizer &v) {
        stringstream ss;
        ss << "aligned_" << ci;
        v.updatePointCloud(aligned, visualization::PointCloudColorHandlerCustom<PointNormal>(aligned, 0xff, 0xff, 0xff), ss.str());
      }
    private:
      int ci;
      PointCloud<PointNormal>::ConstPtr aligned;
    };

    class visualization_callback {
    public:
      visualization_callback(int ci, visualization::CloudViewer *vis) : ci(ci), vis(vis) {}
      void operator()(const PointCloud<PointNormal> &cloud_src,
                      const vector<int> &indices_src,
                      const PointCloud<PointNormal> &cloud_tgt,
                      const vector<int> &indices_tgt) {
        // make a copy of the cloud. expensive.
        PointCloud<PointNormal>::ConstPtr aligned (new PointCloud<PointNormal>(cloud_src));
        vis->runOnVisualizationThreadOnce(show_aligned(ci, aligned));
      }
    private:
      int ci;
      visualization::CloudViewer *vis;
    };

    /** Detector */
    const double Detector::keypoint_separation = 0.04;
    const int Detector::max_votes = 20;
    const int Detector::num_registration = 2;

    /* Trains a single model */
    void Detector::train(Scene &scene) {
      srand(time(NULL));
      IndicesPtr keypoints = computeKeypoints(scene.cloud);
      PointCloud<Detector::Signature>::Ptr features = obtainFeatures(scene, keypoints);
      //PointCloud<Signature>::Ptr features (new PointCloud<Signature>);
      //for (int mi = 0; mi < Config::num_models; mi++) {
        //cout << "begin model " << mi << endl;
      Entry &e = database[scene.id];
      e.cloud = scene.cloud;
      e.indices = keypoints;
      //
      e.features = features;
      e.tree = KdTree<Signature>::Ptr(new KdTreeFLANN<Signature>());
      e.tree->setInputCloud(e.features);
      //timer.stop(OBTAIN_FEATURES_TRAINING);
      //*features += *e.features;
      //cout << "finished model " << mi << endl;
      //}
      //timer.start();
      //tree = KdTree<Signature>::Ptr(new KdTreeFLANN<Signature>());
      //tree->setInputCloud(features);
      //timer.stop(BUILD_TREE);
    }

    typedef struct {
      int mi;
      float votes;
    } Candidate;

    bool operator<(const Candidate &a, const Candidate &b) {
      return a.votes > b.votes; // sort descending
    }

    double Detector::get_votes(Entry &query, Entry &match) {
      double votes = 0;
      for (unsigned int pi = 0; pi < query.indices->size(); pi++) {
        for (int mi = 0; mi < Config::num_models; mi++) {
          vector<int> indices;
          vector<float> distances;

          clock_t start = clock();
          StopWatch s;
          match.tree->nearestKSearch(*query.features, pi, max_votes, indices, distances);

          for (int ri = 0; ri < max_votes; ri++) {
            votes += 1. / (distances[ri] + numeric_limits<float>::epsilon());
          }
        }
      }

      return votes;
    }

    int Detector::query(PointCloud<PointNormal>::Ptr scene, float *classifier, double *registration) {
      Entry e;
      e.cloud = scene;
      timer.start();<commit_msg>Fixed overcounting in the voting algorithm<commit_after>#include <pcl/features/fpfh.h>
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/registration/icp.h>
#include <pcl/common/time.h>

#include "proctor/detector.h"
#include "proctor/ia_ransac_sub.h"
#include "proctor/proctor.h"


namespace pcl {
  namespace proctor {
    /** create the viewports to be used by top candidates */
    class create_viewports {
    public:
      create_viewports(PointCloud<PointNormal>::ConstPtr sentinel) : sentinel(sentinel) {}
      void operator()(visualization::PCLVisualizer &v) {
        for (int ci = 0; ci < Detector::num_registration; ci++) {
          int vp;
          v.createViewPort(double(ci) / Detector::num_registration, 0, double(ci + 1) / Detector::num_registration, 1, vp);
          {
            stringstream ss;
            ss << "candidate_" << ci;
            v.addPointCloud(sentinel, visualization::PointCloudColorHandlerCustom<PointNormal>(sentinel, 0xc0, 0x00, 0x40), ss.str(), vp);
          }
          {
            stringstream ss;
            ss << "aligned_" << ci;
            v.addPointCloud(sentinel, visualization::PointCloudColorHandlerCustom<PointNormal>(sentinel, 0xc0, 0x00, 0x40), ss.str(), vp);
          }
        }
      }
    private:
      PointCloud<PointNormal>::ConstPtr sentinel;
    };

    /** show a candidate model as the specified candidate */
    class show_candidate {
    public:
      show_candidate(int ci, PointCloud<PointNormal>::ConstPtr candidate) : ci(ci), candidate(candidate) {}
      void operator()(visualization::PCLVisualizer &v) {
        {
          stringstream ss;
          ss << "candidate_" << ci;
          v.updatePointCloud(candidate, visualization::PointCloudColorHandlerCustom<PointNormal>(candidate, 0xc0, 0x00, 0x40), ss.str());
        }
        {
          stringstream ss;
          ss << "aligned_" << ci;
          v.updatePointCloud(candidate, visualization::PointCloudColorHandlerCustom<PointNormal>(candidate, 0xc0, 0x00, 0x40), ss.str());
        }
      }
    private:
      int ci;
      PointCloud<PointNormal>::ConstPtr candidate;
    };

    /** show an aligned point cloud in the specified viewport */
    class show_aligned {
    public:
      show_aligned(int ci, PointCloud<PointNormal>::ConstPtr aligned) : ci(ci), aligned(aligned) {}
      void operator()(visualization::PCLVisualizer &v) {
        stringstream ss;
        ss << "aligned_" << ci;
        v.updatePointCloud(aligned, visualization::PointCloudColorHandlerCustom<PointNormal>(aligned, 0xff, 0xff, 0xff), ss.str());
      }
    private:
      int ci;
      PointCloud<PointNormal>::ConstPtr aligned;
    };

    class visualization_callback {
    public:
      visualization_callback(int ci, visualization::CloudViewer *vis) : ci(ci), vis(vis) {}
      void operator()(const PointCloud<PointNormal> &cloud_src,
                      const vector<int> &indices_src,
                      const PointCloud<PointNormal> &cloud_tgt,
                      const vector<int> &indices_tgt) {
        // make a copy of the cloud. expensive.
        PointCloud<PointNormal>::ConstPtr aligned (new PointCloud<PointNormal>(cloud_src));
        vis->runOnVisualizationThreadOnce(show_aligned(ci, aligned));
      }
    private:
      int ci;
      visualization::CloudViewer *vis;
    };

    /** Detector */
    const double Detector::keypoint_separation = 0.04;
    const int Detector::max_votes = 20;
    const int Detector::num_registration = 2;

    /* Trains a single model */
    void Detector::train(Scene &scene) {
      srand(time(NULL));
      IndicesPtr keypoints = computeKeypoints(scene.cloud);
      PointCloud<Detector::Signature>::Ptr features = obtainFeatures(scene, keypoints);
      //PointCloud<Signature>::Ptr features (new PointCloud<Signature>);
      //for (int mi = 0; mi < Config::num_models; mi++) {
        //cout << "begin model " << mi << endl;
      Entry &e = database[scene.id];
      e.cloud = scene.cloud;
      e.indices = keypoints;
      //
      e.features = features;
      e.tree = KdTree<Signature>::Ptr(new KdTreeFLANN<Signature>());
      e.tree->setInputCloud(e.features);
      //timer.stop(OBTAIN_FEATURES_TRAINING);
      //*features += *e.features;
      //cout << "finished model " << mi << endl;
      //}
      //timer.start();
      //tree = KdTree<Signature>::Ptr(new KdTreeFLANN<Signature>());
      //tree->setInputCloud(features);
      //timer.stop(BUILD_TREE);
    }

    typedef struct {
      int mi;
      float votes;
    } Candidate;

    bool operator<(const Candidate &a, const Candidate &b) {
      return a.votes > b.votes; // sort descending
    }

    double Detector::get_votes(Entry &query, Entry &match) {
      double votes = 0;
      for (unsigned int pi = 0; pi < query.indices->size(); pi++) {
        vector<int> indices;
        vector<float> distances;

        clock_t start = clock();
        StopWatch s;
        match.tree->nearestKSearch(*query.features, pi, max_votes, indices, distances);

        for (int ri = 0; ri < max_votes; ri++) {
          //votes += 1. / (distances[ri] + numeric_limits<float>::epsilon());
          votes -= distances[ri];
        }
      }

      return votes;
    }

    int Detector::query(PointCloud<PointNormal>::Ptr scene, float *classifier, double *registration) {
      Entry e;
      e.cloud = scene;
      timer.start();
{'fraction_non_alphanumeric': 0.1314506485143961, 'fraction_numerical': 0.00929077361788244, 'mean_word_length': 2.704258943781942, 'pattern_counts': {'":': 0, '<': 144, '<?xml version=': 0, '>': 81, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2066111', 'n_tokens_mistral': 3438, 'n_tokens_neox': 3246, 'n_words': 715}