[PCL] 平面の検出

    // cloud

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_original;

    int count = 0;  

    // 処理用のクラウドに初期値のクラウドを与える

    cloud = cloud_original;

    // 平面を繰り返し探索

    while( cloud->points.size() > ( int )( cloud_original->points.size() ) * 0.1 ) {

        // set input

        seg.setInputCloud( cloud );

        seg.segment( *inliers, *coefficients );

        if( inliers->indices.size() == 0 ) {

            std::cout << "No planes in the scene" << std::endl;

            break;

        }

        // 平面として選ばれた側の点に対する処理

        extract.setInputCloud( cloud );

        extract.setIndices( inliers );

        extract.setNegative( false );

        extract.filter( *cloud_p );

        std::cout << "Plane " << count << ": " << inliers->indices.size() << " points, " << color[count % 6] << std::endl;

        std::cout << " Coeffs: " << coefficients->values[0] << ", " << coefficients->values[1] << ", " << coefficients->values[2] << ", " << coefficients->values[3] << std::endl;

        // arrayに格納

        coeffsArray.push_back( *coefficients );

        std::stringstream   ss;

        ss << "cloud_" << count;

        viewer->addPointCloud( cloud_p, ss.str() );

        // 平面として選ばれなかった側の点に対する処理

        extract.setNegative( true );

        extract.filter( *cloud_f );

        cloud = cloud_f;

        // カウントアップ

        count++;

    }