keypoint.hpp

// Copyright 2021 Yu-Kai Lin. All rights reserved.
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.
 
#pragma once
 
#include "kcp/common.hpp"
 
namespace kcp {
 
namespace keypoint {
 
class RangeImage {
 protected:
  Eigen::MatrixX3d cloud;
 
  std::vector<int> channel;
 
  int n_channels;
 
  float min_vfov_deg;
 
  float max_vfov_deg;
 
  int hfov_resolution;
 
  Eigen::MatrixXi image_indices;
 
  std::vector<float> image_depth_sequence;
 
  std::vector<int> image_point_indices_sequence;
 
  std::vector<int> image_col_indices_sequence;
 
  std::vector<int> channel_start_indices;
 
  std::vector<int> channel_end_indices;
 
  void calculate_point_cloud_properties();
 
  void calculate_range_image();
 
 public:
  RangeImage(Eigen::MatrixX3d cloud,
             int n_channels      = 32,
             float min_vfov_deg  = -30.0,
             float max_vfov_deg  = 10.0,
             int hfov_resolution = 1800);
 
  const Eigen::MatrixX3d &get_cloud() const { return this->cloud; }
 
  int get_n_channels() const { return this->n_channels; }
 
  size_t get_image_sequence_size() const { return this->image_depth_sequence.size(); }
 
  const Eigen::MatrixXi &get_image_indices() const { return this->image_indices; }
 
  const std::vector<float> &get_image_depth_sequence() const { return this->image_depth_sequence; }
 
  const std::vector<int> &get_image_point_indices_sequence() const { return this->image_point_indices_sequence; }
 
  const std::vector<int> &get_image_col_indices_sequence() const { return this->image_col_indices_sequence; }
 
  const std::vector<int> &get_channel_start_indices() const { return this->channel_start_indices; }
 
  const std::vector<int> &get_channel_end_indices() const { return this->channel_end_indices; }
};
 
class MultiScaleCurvature {
 public:
  enum class Label {
    UNDEFINED,
    NORMAL,
    OCCLUDED,
    PARALLEL,
    CORNER,
    PLANE,
    AMBIGUOUS
  };
 
 protected:
  RangeImage range_image;
 
  std::vector<std::pair<float, int>> curvature;
 
  std::vector<Label> label;
 
  float corner_threshold;
 
  float plane_threshold;
 
  Eigen::MatrixX3d corner_points;
 
  Eigen::MatrixX3d plane_points;
 
  std::vector<int> corner_point_indices;
 
  std::vector<int> plane_point_indices;
 
  void calculate_multi_scale_curvature();
 
 public:
  MultiScaleCurvature(RangeImage range_image, float corner_threshold = 30.0, float plane_threshold = 0.1);
 
  MultiScaleCurvature(Eigen::MatrixX3d cloud,
                      int n_channels         = 32,
                      float min_vfov_deg     = -30.0,
                      float max_vfov_deg     = 10.0,
                      int hfov_resolution    = 1800,
                      float corner_threshold = 30.0,
                      float plane_threshold  = 0.1);
 
  const RangeImage &get_range_image() const { return this->range_image; }
 
  const Eigen::MatrixX3d &get_corner_points() const { return this->corner_points; }
 
  const Eigen::MatrixX3d &get_plane_points() const { return this->plane_points; }
 
  const std::vector<int> &get_corner_point_indices() const { return this->corner_point_indices; }
 
  const std::vector<int> &get_plane_point_indices() const { return this->plane_point_indices; }
 
  const std::vector<std::pair<float, int>> &get_curvature() const { return this->curvature; }
};
 
};  // namespace keypoint
 
};  // namespace kcp