rspfGeoAnnotationPolyLineObject::rspfGeoAnnotationPolyLineObject(
const vector<rspfGpt>& groundPts,
rspf_uint8 r,
rspf_uint8 g,
rspf_uint8 b,
rspf_uint8 thickness)
:rspfGeoAnnotationObject(r, g, b, thickness),
theProjectedMultiLineObject(0)
{
thePolygon = groundPts;
// we will initialize the projected polygon's size
// so we don't have to reset it every time we do a
// projection
//
vector<rspfDpt> projectedPoints(thePolygon.size());
theProjectedMultiLineObject = new rspfAnnotationMultiLineObject(rspfPolyLine(projectedPoints),
r,
g,
b,
thickness);
}
int main(int argc, char** argv)
{
if (argc < 2)
{
std::cerr << "Input test PCD file(.pcd)...\n";
return 0;
}
// Objects for storing the point clouds.
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudNoPlane(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr planePoints(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr projectedPoints(new pcl::PointCloud<pcl::PointXYZ>);
// Read a PCD file from disk.
if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
{
return -1;
}
// Get the plane model, if present.
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::SACSegmentation<pcl::PointXYZ> segmentation;
segmentation.setInputCloud(cloud);
segmentation.setModelType(pcl::SACMODEL_PLANE);
segmentation.setMethodType(pcl::SAC_RANSAC);
segmentation.setDistanceThreshold(0.01);
segmentation.setOptimizeCoefficients(true);
pcl::PointIndices::Ptr inlierIndices(new pcl::PointIndices);
segmentation.segment(*inlierIndices, *coefficients);
if (inlierIndices->indices.size() == 0)
std::cerr << "Could not find a plane in the scene." << std::endl;
else
{
std::cerr << "Plane coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
// Create a second point cloud that does not have the plane points.
// Also, extract the plane points to visualize them later.
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud);
extract.setIndices(inlierIndices);
extract.filter(*planePoints);
extract.setNegative(true);
extract.filter(*cloudNoPlane);
// Object for projecting points onto a model.
pcl::ProjectInliers<pcl::PointXYZ> projection;
// We have to specify what model we used.
projection.setModelType(pcl::SACMODEL_PLANE);
projection.setInputCloud(cloudNoPlane);
// And we have to give the coefficients we got.
projection.setModelCoefficients(coefficients);
projection.filter(*projectedPoints);
// Visualize everything.
pcl::visualization::CloudViewer viewerPlane("Plane");
viewerPlane.showCloud(planePoints);
while (!viewerPlane.wasStopped())
{
// Do nothing but wait.
}
pcl::visualization::CloudViewer viewerProjection("Projected points");
viewerProjection.showCloud(projectedPoints);
while (!viewerProjection.wasStopped())
{
// Do nothing but wait.
}
}
}
