<commit_before>#include "proctor/frame_hough_proposer.h"

namespace pcl
{
  namespace proctor
  {
    void
    FrameHoughProposer::houghVote(Entry &query, Entry &target, bin_t& bins)
    {
      // Compute the reference point for the R-table
      Eigen::Vector4f centroid4;
      compute3DCentroid(*(target.cloud), centroid4);
      Eigen::Vector3f centroid(centroid4[0], centroid4[1], centroid4[2]);

      assert(query.keypoints->size() == query.features->size());
      assert(target.keypoints->size() == target.features->size());

      // Figure out bin dimension
      Eigen::Vector4f query_min4, query_max4;
      getMinMax3D(*query.cloud, query_min4, query_max4);
      Eigen::Vector3f query_min(query_min4[0], query_min4[1], query_min4[2]);
      Eigen::Vector3f query_max(query_max4[0], query_max4[1], query_max4[2]);

      PointCloud<PrincipalCurvatures>::Ptr target_curvatures = computeCurvatures(target);
      PointCloud<PrincipalCurvatures>::Ptr query_curvatures = computeCurvatures(query);

      for (unsigned int i = 0; i < query.keypoints->size(); i++)
      {
        std::vector<int> feature_indices;
        std::vector<float> sqr_distances;

        int num_correspondences = 2;
        if (!pcl_isfinite (query.features->points.row(i)(0)))
          continue;
        int num_found = target.tree->nearestKSearch(*query.features, i, num_correspondences, feature_indices, sqr_distances);

        for (int j = 0; j < num_found; j++)
        {
          // For each one of the feature correspondences
          int feature_index = feature_indices[j];

          // Get corresponding target keypoint, and calculate its r to its centroid in world coordinates
          PointNormal correspondence = target.keypoints->at(feature_index); // Since features correspond to the keypoints
          Eigen::Vector3f r = correspondence.getVector3fMap() - centroid;

          // Calculate a local frame using principal curvature
          Eigen::Vector3f target_normal = correspondence.getNormalVector3fMap();
          target_normal.normalize();
          Eigen::Vector3f query_normal = query.keypoints->at(i).getNormalVector3fMap();
          query_normal.normalize();

          Eigen::Vector3f query_pc = getVectorCurvatureMap(query_curvatures->at(i));
          Eigen::Vector3f query_perp = query_pc - (query_pc.dot(query_normal.normalized())) * query_normal.normalized();
          query_perp.normalize();

          Eigen::Vector3f target_pc = getVectorCurvatureMap(target_curvatures->at(feature_index));
          Eigen::Vector3f target_perp = target_pc - (target_pc.dot(target_normal.normalized())) * target_normal.normalized();
          target_perp.normalize();

          // In case the PrincipalCurvaturesEstimation outputs NaNs,
          // skip this iteration
          if (query_perp != query_perp || target_perp != target_perp)
          {
            continue;
          }

          Eigen::Vector3f world_x(1, 0, 0);
          Eigen::Vector3f world_y(0, 1, 0);
          Eigen::Vector3f world_z(0, 0, 1);
          Eigen::Matrix3f A = getTransformationBetweenFrames(world_x, world_y, target_normal, target_perp);
          Eigen::Matrix3f B = getTransformationBetweenFrames(world_x, world_y, query_normal, query_perp);
          Eigen::Matrix3f rot_transform = B.transpose() * A;
          //Eigen::Matrix3f other = getTransformationBetweenFrames(target_normal, target_perp, query_normal, query_perp);

          // Test that the target_normal goes to (1, 0, 0)
          assert((A * target_normal).isApprox(world_x, 0.01));

          // Test that the target_normal goes to (1, 0, 0) in A space,
          // then when moved to B space, and returned back to world space,
          // it stays the same
          assert((B.transpose() * other * A * target_normal).isApprox(target_normal, 0.01));

          // Test that the target_normal goes to (1, 0, 0) in A space,
          // then when rotated in A space by the difference between B and A,
          // then returned back to world space, is equal to the query_normal
          //Eigen::Vector3f test = (A.transpose() * other.transpose() * A * target_normal);
          assert((A.transpose() * other.transpose() * A * target_normal).isApprox(query_normal, 0.01));
<commit_msg>Fix compilation of proctor in debug mode.<commit_after>#include "proctor/frame_hough_proposer.h"

namespace pcl
{
  namespace proctor
  {
    void
    FrameHoughProposer::houghVote(Entry &query, Entry &target, bin_t& bins)
    {
      // Compute the reference point for the R-table
      Eigen::Vector4f centroid4;
      compute3DCentroid(*(target.cloud), centroid4);
      Eigen::Vector3f centroid(centroid4[0], centroid4[1], centroid4[2]);

      assert(query.keypoints->size() == query.features->size());
      assert(target.keypoints->size() == target.features->size());

      // Figure out bin dimension
      Eigen::Vector4f query_min4, query_max4;
      getMinMax3D(*query.cloud, query_min4, query_max4);
      Eigen::Vector3f query_min(query_min4[0], query_min4[1], query_min4[2]);
      Eigen::Vector3f query_max(query_max4[0], query_max4[1], query_max4[2]);

      PointCloud<PrincipalCurvatures>::Ptr target_curvatures = computeCurvatures(target);
      PointCloud<PrincipalCurvatures>::Ptr query_curvatures = computeCurvatures(query);

      for (unsigned int i = 0; i < query.keypoints->size(); i++)
      {
        std::vector<int> feature_indices;
        std::vector<float> sqr_distances;

        int num_correspondences = 2;
        if (!pcl_isfinite (query.features->points.row(i)(0)))
          continue;
        int num_found = target.tree->nearestKSearch(*query.features, i, num_correspondences, feature_indices, sqr_distances);

        for (int j = 0; j < num_found; j++)
        {
          // For each one of the feature correspondences
          int feature_index = feature_indices[j];

          // Get corresponding target keypoint, and calculate its r to its centroid in world coordinates
          PointNormal correspondence = target.keypoints->at(feature_index); // Since features correspond to the keypoints
          Eigen::Vector3f r = correspondence.getVector3fMap() - centroid;

          // Calculate a local frame using principal curvature
          Eigen::Vector3f target_normal = correspondence.getNormalVector3fMap();
          target_normal.normalize();
          Eigen::Vector3f query_normal = query.keypoints->at(i).getNormalVector3fMap();
          query_normal.normalize();

          Eigen::Vector3f query_pc = getVectorCurvatureMap(query_curvatures->at(i));
          Eigen::Vector3f query_perp = query_pc - (query_pc.dot(query_normal.normalized())) * query_normal.normalized();
          query_perp.normalize();

          Eigen::Vector3f target_pc = getVectorCurvatureMap(target_curvatures->at(feature_index));
          Eigen::Vector3f target_perp = target_pc - (target_pc.dot(target_normal.normalized())) * target_normal.normalized();
          target_perp.normalize();

          // In case the PrincipalCurvaturesEstimation outputs NaNs,
          // skip this iteration
          if (query_perp != query_perp || target_perp != target_perp)
          {
            continue;
          }

          Eigen::Vector3f world_x(1, 0, 0);
          Eigen::Vector3f world_y(0, 1, 0);
          Eigen::Vector3f world_z(0, 0, 1);
          Eigen::Matrix3f A = getTransformationBetweenFrames(world_x, world_y, target_normal, target_perp);
          Eigen::Matrix3f B = getTransformationBetweenFrames(world_x, world_y, query_normal, query_perp);
          Eigen::Matrix3f rot_transform = B.transpose() * A;
#ifndef NDEBUG
          Eigen::Matrix3f other = getTransformationBetweenFrames(target_normal, target_perp, query_normal, query_perp);
#endif

          // Test that the target_normal goes to (1, 0, 0)
          assert((A * target_normal).isApprox(world_x, 0.01));

          // Test that the target_normal goes to (1, 0, 0) in A space,
          // then when moved to B space, and returned back to world space,
          // it stays the same
          assert((B.transpose() * other * A * target_normal).isApprox(target_normal, 0.01));

          // Test that the target_normal goes to (1, 0, 0) in A space,
          // then when rotated in A space by the difference between B and A,
          // then returned back to world space, is equal to the query_normal
          //Eigen::Vector3f test = (A.transpose() * other.transpose() * A * target_normal);
          assert((A.transpose() * other.transpose() * A * target_normal).isApprox(query_normal, 0.01));

{'fraction_non_alphanumeric': 0.11213535589264878, 'fraction_numerical': 0.01866977829638273, 'mean_word_length': 3.0872675250357653, 'pattern_counts': {'":': 0, '<': 15, '<?xml version=': 0, '>': 33, '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': '2998733', 'n_tokens_mistral': 2673, 'n_tokens_neox': 2546, 'n_words': 609}