Dissertation PointCloud stats

gistfile1.cpp

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <boost/lexical_cast.hpp>


typedef pcl::PointXYZ Point;
typedef pcl::PointCloud<Point> PointCloud;

void println(std::string output)
{
  std::cout << output << std::endl;
}

std::string pointToString(Point point)
{
  return "( " + 
            floatToString(point.x) + 
            floatToString(point.y) + 
            floatToString(point.z) + 
          ")";
}

std::string floatToString(float number)
{
  return boost::lexical_cast<std::string>(number);
}

int main (int argc, char** argv)
{
  PointCloud::Ptr cloud (new PointCloud);

  if (pcl::io::loadPCDFile<Point> ("test_pcd.pcd", *cloud) == -1)
  {
    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
    return (-1);
  }

  //Total Points A
  println("total points A : " + (cloud->width * cloud->height));

  std::vector<Point, Eigen::aligned_allocator<Point>> points = cloud->points;
  Point centerPoint;
  
  //Total Points B
  println("total points B : " +points.size());

  for(int i=0; i < points.size(); i++)
  {
    centerPoint.x += points[i].x;
    centerPoint.y += points[i].y;
    centerPoint.z += points[i].z;
  }

  centerPoint.x /= points.size();
  centerPoint.y /= points.size();
  centerPoint.z /= points.size();
  
  //Center Point
  println("center point : " + pointToString(centerPoint));

  float averageDistance;
  float minDistance;
  float maxDistance;

  int pairCount;

  for(int i=0; i < points.size(); i++)
  {
    Point pointa = points[i];

    for(int j=0; j < i; j++)
    {
      Point pointb = points[i];
      
      pairCount++;

      float distance =  std::sqrt(
            std::pow(pointa.x - pointb.x, 2) +
            std::pow(pointa.y - pointb.y, 2) +
            std::pow(pointa.z - pointb.z, 2)
          );

      averageDistance += distance;
      minDistance = std::min(minDistance, distance);
      maxDistance = std::max(maxDistance, distance);
    }
  }

  averageDistance /= pairCount;

  //Average distance between points
  println("average pair distance : " + floatToString(averageDistance));

  //Min distance between points
  println("min pair distance : " + floatToString(minDistance));

  //Max distance between points
  println("max pair distance : " + floatToString(maxDistance));

	return 0;
}