<commit_before>#include <walrus_stair_detector/walrus_stair_detector.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <walrus_stair_detector/multi_timer.h>
#include <pcl/common/common.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/surface/convex_hull.h>
#include <walrus_stair_detector/ransac.h>
#include <walrus_stair_detector/histogram.h>
#include <walrus_stair_detector/parallel_plane_ransac_model.h>

namespace walrus_stair_detector {

WalrusStairDetector::WalrusStairDetector() : shutdown_(false),
					     min_rise_(0.1), max_rise_(0.35),
					     min_run_(0.2), max_run_(0.4),
					     max_skipped_risers_(2), min_stair_width_(0.5) {
#if VISUALIZE
  visualizer_update_ = false;
  visualizer_thread_.reset(new boost::thread(boost::bind(&WalrusStairDetector::visualize, this)));
  previous_plane_visual_count_ = 0;
  previous_risers_count_ = 0;
  previous_stair_count_ = 0;
#endif
}
WalrusStairDetector::~WalrusStairDetector() {
  shutdown_ = true;
#if VISUALIZE
  visualizer_thread_->join();
#endif
}
void WalrusStairDetector::detect(const PointCloud::ConstPtr& original_cloud, const Eigen::Vector3f& vertical_estimate, std::vector<StairModel>* stairs) {
  stairs->clear();
#if VISUALIZE
  boost::mutex::scoped_lock lock(visualizer_mutex_);
  visualizer_update_ = true;
  plane_visual_ = std::vector<DetectedPlane::Ptr>();
  model_ = StairModel();
  risers_visual_.clear();
#endif

  ROS_INFO("Cloud: width = %d, height = %d, size = %ld\n", original_cloud->width, original_cloud->height, original_cloud->points.size());

  MultiTimer timer;

  // Downsize the pointcloud
  PointCloud::Ptr downsized_cloud (new PointCloud);
  downsizePointCloud(original_cloud, downsized_cloud);
  timer.end("downsize");

  // Remove invalid points
  pcl::IndicesPtr filter_indices(new std::vector <int>);
  pcl::removeNaNFromPointCloud<PointCloud::PointType>(*downsized_cloud, *filter_indices);
  timer.end("filter");

  // Compute the normal of all points
  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
  computeNormals(downsized_cloud, filter_indices, normals);
#if VISUALIZE
  normals_visual_ = normals;
#endif
  timer.end("normals");

  // Grow points to form planes
  std::vector<pcl::PointIndices> clusters;
  regionGrow(downsized_cloud, filter_indices, normals, &clusters);
  timer.end("region grow");


  std::vector<DetectedPlane::Ptr> planes;
  int plane_id = 0;
  BOOST_FOREACH(const pcl::PointIndices& cluster, clusters) {
    DetectedPlane::Ptr plane(new DetectedPlane(plane_id++));

    bool plane_model_result = computePlaneModel(downsized_cloud, cluster, plane->coefficients, plane->inliers);
    if(plane_model_result) {
      plane->normal[0] = plane->coefficients->values[0];
      plane->normal[1] = plane->coefficients->values[1];
      plane->normal[2] = plane->coefficients->values[2];
      plane->normal.normalize();
      planes.push_back(plane);
    }
    timer.end("plane_model");
    if(!plane_model_result)
      continue;

    // Project all points into the computed plane
    projectPoints(downsized_cloud, plane->inliers, plane->coefficients, plane->cluster_projected);
    timer.end("project to planes");
  }

  Eigen::Vector3f vertical;
  computeVertical(planes, vertical_estimate.normalized(), &vertical);

  BOOST_FOREACH(DetectedPlane::Ptr& plane, planes) {
    // Compute the bounding points of the plane
    pcl::ConvexHull<pcl::PointXYZ> chull;
    chull.setInputCloud(plane->cluster_projected);
    chull.reconstruct(*plane->cluster_hull);
    timer.end("convex hull");

    computePlaneOrientation(plane, vertical);

    if(plane->orientation == DetectedPlane::Vertical) {
      Eigen::Vector3f plane_horizontal = vertical.cross(plane->normal);
      computePlaneSize(plane, plane_horizontal, vertical);
    }

    guessPlaneType(plane);

    timer.end("plane properties");


    timer.skip();
  }
#if VISUALIZE
  plane_visual_ = planes;
#endif

  StairModel model;
  model.vertical = vertical;

  Eigen::Vector3f riser_direction;
  bool stair_orientation_result = computeStairOrientation(planes, vertical, &riser_direction);<commit_msg>Fixed axis not being a normalized vector<commit_after>#include <walrus_stair_detector/walrus_stair_detector.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <walrus_stair_detector/multi_timer.h>
#include <pcl/common/common.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/surface/convex_hull.h>
#include <walrus_stair_detector/ransac.h>
#include <walrus_stair_detector/histogram.h>
#include <walrus_stair_detector/parallel_plane_ransac_model.h>

namespace walrus_stair_detector {

WalrusStairDetector::WalrusStairDetector() : shutdown_(false),
					     min_rise_(0.1), max_rise_(0.35),
					     min_run_(0.2), max_run_(0.4),
					     max_skipped_risers_(2), min_stair_width_(0.5) {
#if VISUALIZE
  visualizer_update_ = false;
  visualizer_thread_.reset(new boost::thread(boost::bind(&WalrusStairDetector::visualize, this)));
  previous_plane_visual_count_ = 0;
  previous_risers_count_ = 0;
  previous_stair_count_ = 0;
#endif
}
WalrusStairDetector::~WalrusStairDetector() {
  shutdown_ = true;
#if VISUALIZE
  visualizer_thread_->join();
#endif
}
void WalrusStairDetector::detect(const PointCloud::ConstPtr& original_cloud, const Eigen::Vector3f& vertical_estimate, std::vector<StairModel>* stairs) {
  stairs->clear();
#if VISUALIZE
  boost::mutex::scoped_lock lock(visualizer_mutex_);
  visualizer_update_ = true;
  plane_visual_ = std::vector<DetectedPlane::Ptr>();
  model_ = StairModel();
  risers_visual_.clear();
#endif

  ROS_INFO("Cloud: width = %d, height = %d, size = %ld\n", original_cloud->width, original_cloud->height, original_cloud->points.size());

  MultiTimer timer;

  // Downsize the pointcloud
  PointCloud::Ptr downsized_cloud (new PointCloud);
  downsizePointCloud(original_cloud, downsized_cloud);
  timer.end("downsize");

  // Remove invalid points
  pcl::IndicesPtr filter_indices(new std::vector <int>);
  pcl::removeNaNFromPointCloud<PointCloud::PointType>(*downsized_cloud, *filter_indices);
  timer.end("filter");

  // Compute the normal of all points
  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
  computeNormals(downsized_cloud, filter_indices, normals);
#if VISUALIZE
  normals_visual_ = normals;
#endif
  timer.end("normals");

  // Grow points to form planes
  std::vector<pcl::PointIndices> clusters;
  regionGrow(downsized_cloud, filter_indices, normals, &clusters);
  timer.end("region grow");


  std::vector<DetectedPlane::Ptr> planes;
  int plane_id = 0;
  BOOST_FOREACH(const pcl::PointIndices& cluster, clusters) {
    DetectedPlane::Ptr plane(new DetectedPlane(plane_id++));

    bool plane_model_result = computePlaneModel(downsized_cloud, cluster, plane->coefficients, plane->inliers);
    if(plane_model_result) {
      plane->normal[0] = plane->coefficients->values[0];
      plane->normal[1] = plane->coefficients->values[1];
      plane->normal[2] = plane->coefficients->values[2];
      plane->normal.normalize();
      planes.push_back(plane);
    }
    timer.end("plane_model");
    if(!plane_model_result)
      continue;

    // Project all points into the computed plane
    projectPoints(downsized_cloud, plane->inliers, plane->coefficients, plane->cluster_projected);
    timer.end("project to planes");
  }

  Eigen::Vector3f vertical;
  computeVertical(planes, vertical_estimate.normalized(), &vertical);

  BOOST_FOREACH(DetectedPlane::Ptr& plane, planes) {
    // Compute the bounding points of the plane
    pcl::ConvexHull<pcl::PointXYZ> chull;
    chull.setInputCloud(plane->cluster_projected);
    chull.reconstruct(*plane->cluster_hull);
    timer.end("convex hull");

    computePlaneOrientation(plane, vertical);

    if(plane->orientation == DetectedPlane::Vertical) {
      Eigen::Vector3f plane_horizontal = vertical.cross(plane->normal).normalized();
      computePlaneSize(plane, plane_horizontal, vertical);
    }

    guessPlaneType(plane);

    timer.end("plane properties");


    timer.skip();
  }
#if VISUALIZE
  plane_visual_ = planes;
#endif

  StairModel model;
  model.vertical = vertical;

  Eigen::Vector3f riser_direction;
  bool stair_orientation_result = computeStairOrientation(planes, vertical, &riser_direction);
{'fraction_non_alphanumeric': 0.13063888252805128, 'fraction_numerical': 0.006182734142431876, 'mean_word_length': 5.617424242424242, 'pattern_counts': {'":': 0, '<': 51, '<?xml version=': 0, '>': 99, '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': '4327472', 'n_tokens_mistral': 3102, 'n_tokens_neox': 2927, 'n_words': 491}