2015年3月11日水曜日

入力された点群の各座標の最大値と最小値を求める

 題意の通り、極めて簡単なものなので必要がないと言われればその通りかもしれないが、なにかの機会に使用することがあるかもしれない。

コード

#include "stdafx.h"

#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <stdlib.h>

class Size
{
public:
    double min_x;
    double min_y;
    double min_z;
    double max_x;
    double max_y;
    double max_z;
};

Size Min_Max(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    Size size;
    size.max_x = 0;
    size.max_y = 0;
    size.max_z = 0;
    size.min_x = 0;
    size.min_y = 0;
    size.min_z = 0;

    for(size_t i = 0; i < cloud->points.size(); i++){
        if(size.max_z > cloud->points[i].z)
            size.max_z = size.max_z;
        else
            size.max_z = cloud->points[i].z;

        if(size.max_y > cloud->points[i].y)
            size.max_y = size.max_y;
        else
            size.max_y = cloud->points[i].y;

        if(size.max_x > cloud->points[i].x)
            size.max_x = size.max_x;
        else
            size.max_x = cloud->points[i].x;

        if(size.min_z < cloud->points[i].z)
            size.min_z = size.min_z;
        else
            size.min_z = cloud->points[i].z;

        if(size.min_y < cloud->points[i].y)
            size.min_y = size.min_y;
        else
            size.min_y = cloud->points[i].y;

        if(size.min_x < cloud->points[i].x)
            size.min_x = size.min_x;
        else
            size.min_x = cloud->points[i].x;
    }
    return size;
}


int _tmain(int argc, _TCHAR* argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile(argv[1], *cloud);

    std::cout << Min_Max(cloud).max_x << std::endl;
    std::cout << Min_Max(cloud).max_y << std::endl;
    std::cout << Min_Max(cloud).max_z << std::endl;
    std::cout << Min_Max(cloud).min_x << std::endl;
    std::cout << Min_Max(cloud).min_y << std::endl;
    std::cout << Min_Max(cloud).min_z << std::endl;

    return (0); 

}

0 件のコメント:

コメントを投稿