int main(int argc, char** argv)
{
PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
PointCloud<PointXYZ>::Ptr final (new PointCloud<PointXYZ>);
cloud->width = 500;
cloud->height = 1;
cloud->is_dense = false;
cloud->points.resize(cloud->width * cloud->height);
for (size_t i=0; i<cloud->points.size(); ++i)
{
cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.00);
cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.00);
if (console::find_argument(argc, argv, "-s") >= 0 || console::find_argument(argc, argv, "-sf") >= 0)
{
if (i % 5 == 0)
cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.00);
else if (i % 2 == 0)
cloud->points[i].z = sqrt(1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
else
cloud->points[i].z = -sqrt(1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
}
else
{
if (i % 2 == 0)
cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.00);
else
cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
}
}
vector<int> inliers;
SampleConsensusModelSphere<PointXYZ>::Ptr
model_s(new SampleConsensusModelSphere<PointXYZ>(cloud));
SampleConsensusModelPlane<PointXYZ>::Ptr
model_p(new SampleConsensusModelPlane<PointXYZ>(cloud));
if (console::find_argument(argc, argv, "-f") >= 0)
{
RandomSampleConsensus<PointXYZ> ransac (model_p);
ransac.setDistanceThreshold(0.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
else if (console::find_argument(argc, argv, "-sf") >= 0)
{
RandomSampleConsensus<PointXYZ> ransac (model_s);
ransac.setDistanceThreshold(0.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
copyPointCloud<PointXYZ>(*cloud, inliers, *final);
boost::shared_ptr<visualization::PCLVisualizer> viewer;
if (console::find_argument(argc, argv, "-f") >= 0 || console::find_argument(argc, argv, "-sf") >= 0)
viewer = simpleVis(final);
else
TEST (ModelOutlierRemoval, Model_Outlier_Filter)
{
PointCloud<PointXYZ>::Ptr cloud_filter_out (new PointCloud<PointXYZ>);
std::vector<int> ransac_inliers;
float thresh = 0.01;
//run ransac
Eigen::VectorXf model_coefficients;
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud_in));
RandomSampleConsensus < pcl::PointXYZ > ransac (model_p);
ransac.setDistanceThreshold (thresh);
ransac.computeModel ();
ransac.getInliers (ransac_inliers);
ransac.getModelCoefficients (model_coefficients);
// test ransacs result
EXPECT_EQ (model_coefficients.size (), 4);
if (model_coefficients.size () != 4)
return;
//run filter
pcl::ModelCoefficients model_coeff;
model_coeff.values.resize (4);
for (int i = 0; i < 4; i++)
model_coeff.values[i] = model_coefficients[i];
pcl::ModelOutlierRemoval < pcl::PointXYZ > filter;
filter.setModelCoefficients (model_coeff);
filter.setThreshold (thresh);
filter.setModelType (pcl::SACMODEL_PLANE);
filter.setInputCloud (cloud_in);
filter.filter (*cloud_filter_out);
//compare results
EXPECT_EQ (cloud_filter_out->points.size (), ransac_inliers.size ());
//TODO: also compare content
}
void
callback (const sensor_msgs::Image::ConstPtr &depth,
const sensor_msgs::Image::ConstPtr &rgb,
const sensor_msgs::CameraInfo::ConstPtr &info)
{
//typedef pcl_cuda::SampleConsensusModel<pcl_cuda::Host>::Indices Indices;
//pcl_cuda::PointCloudAOS<pcl_cuda::Host>::Ptr data (new pcl_cuda::PointCloudAOS<pcl_cuda::Host>);
PointCloudAOS<Device>::Ptr data;
d2c.callback (depth, rgb, info, data);
SampleConsensusModelPlane<Device>::Ptr sac_model (new SampleConsensusModelPlane<Device> (data));
// SampleConsensusModelPlane<Host>::Ptr sac_model (new pcl_cuda::SampleConsensusModelPlane<pcl_cuda::Host> (data));
RandomSampleConsensus<Device> sac (sac_model);
sac.setDistanceThreshold (0.05);
if (!sac.computeModel (2))
std::cerr << "Failed to compute model" << std::endl;
// Convert data from Thrust/HOST to PCL
pcl::PointCloud<pcl::PointXYZRGB> output;
// pcl_cuda::toPCL (*data, output);
output.header.stamp = ros::Time::now ();
pub.publish (output);
}
TEST (RANSAC, Base)
{
// Create a shared plane model pointer directly
SampleConsensusModelPlanePtr model (new SampleConsensusModelPlane<PointXYZ> (cloud_));
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
// Basic tests
ASSERT_EQ (sac.getDistanceThreshold (), 0.03);
sac.setDistanceThreshold (0.03);
ASSERT_EQ (sac.getDistanceThreshold (), 0.03);
sac.setProbability (0.99);
ASSERT_EQ (sac.getProbability (), 0.99);
sac.setMaxIterations (10000);
ASSERT_EQ (sac.getMaxIterations (), 10000);
}