void FloorAxisAlignment::alignCloudPrincipleAxis (const PointCloudConstPtr input_cloud_ptr,
const PointCloudPtr output_cloud_ptr,
Eigen::Matrix4f& transform_output)
{
// Visualization visualizer;
Eigen::Matrix4f inital_guess;
inital_guess.block<3,3>(0,0) = Eigen::AngleAxisf(angle_, axis_).toRotationMatrix();
// inital_guess = Eigen::Matrix4f::Zero(4,4);
// inital_guess(0,0) = 1.0;
// inital_guess(1,2) = 1.0;
// inital_guess(2,1) = -1.0;
// inital_guess(3,3) = 1.0;
PointCloudPtr rotated_cloud_ptr (new PointCloud);
transformPointCloud (*input_cloud_ptr, *rotated_cloud_ptr, inital_guess);
// std::vector<PointCloudConstPtr> vis_cloud_ptrs;
// vis_cloud_ptrs.push_back(rotated_cloud_ptr);
//transformPointCloud (*input_cloud_ptr, *output_cloud_ptr, inital_guess);
//assume the model is approx correct, i.e. z is height
// find the largest plane perpendicular to z axis and use this to align cloud
pcl17::ModelCoefficients::Ptr coefficients (new pcl17::ModelCoefficients);
pcl17::PointIndices::Ptr inliers (new pcl17::PointIndices);
pcl17::SACSegmentation<PointType> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl17::SACMODEL_PERPENDICULAR_PLANE);
seg.setMethodType (pcl17::SAC_RANSAC);
seg.setDistanceThreshold (ransac_threshold_);
seg.setMaxIterations(max_iterations_);
//seg.setProbability(0.1);
seg.setAxis (Eigen::Vector3f (0, 0, 1));
seg.setEpsAngle (30.0 * (M_PI / 180));//radians
seg.setInputCloud (rotated_cloud_ptr);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
PCL17_ERROR("Could not estimate a planar model for the given dataset.");
// I had some issues with pcl on a different computer to where I normally dev. This is a workaround to be removed
coefficients->values.push_back(-0.0370391);
coefficients->values.push_back(0.0777064);
coefficients->values.push_back(0.996288);
coefficients->values.push_back(2.63374);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1]
<< " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl;
// calculate angle and axis from dot product
double rotate = acos (coefficients->values[2]);
Eigen::Vector3f axis_to_rotate_about (-coefficients->values[1], coefficients->values[0], 0.0);
axis_to_rotate_about.normalize ();
//convert axis and angle to rotation matrix and apply to pointcloud
Eigen::AngleAxis<float> angle_axis (rotate, axis_to_rotate_about);
transform_output = Eigen::Matrix4f::Identity (4, 4);
transform_output.block<3, 3> (0, 0) = angle_axis.toRotationMatrix ().inverse ();
// apply translation to bring the floor to z = 0
transform_output(2, 3) = coefficients->values[3];
transformPointCloud (*rotated_cloud_ptr, *output_cloud_ptr, transform_output);
// pcl17::ExtractIndices<PointType> eifilter;
// eifilter.setInputCloud (rotated_cloud_ptr);
// eifilter.setIndices (inliers);
// eifilter.filter (*output_cloud_ptr);
// vis_cloud_ptrs.push_back(output_cloud_ptr);
// visualizer.visualizeCloud(vis_cloud_ptrs);
}
int main(int argc, char** argv)
{
if (argc < 5)
{
PCL17_INFO ("Usage %s -input_dir /dir/with/models -output_dir /output/dir [options]\n", argv[0]);
PCL17_INFO (" * where options are:\n"
" -height <X> : simulate scans with sensor mounted on height X\n"
" -noise_std <X> : std of gaussian noise added to pointcloud. Default value 0.0001.\n"
" -distance <X> : simulate scans with object located on a distance X. Can be used multiple times. Default value 4.\n"
" -tilt <X> : tilt sensor for X degrees. X == 0 - sensor looks strait. X < 0 Sensor looks down. X > 0 Sensor looks up . Can be used multiple times. Default value -15.\n"
" -shift <X> : shift object from the straight line. Can be used multiple times. Default value 0.\n"
" -num_views <X> : how many times rotate the object in for every distance, tilt and shift. Default value 6.\n"
"");
return -1;
}
std::string input_dir, output_dir;
double height = 1.5;
double num_views = 6;
double noise_std = 0.0001;
std::vector<double> distances;
std::vector<double> tilt;
std::vector<double> shift;
int full_model_n_points = 30000;
pcl17::console::parse_argument(argc, argv, "-input_dir", input_dir);
pcl17::console::parse_argument(argc, argv, "-output_dir", output_dir);
pcl17::console::parse_argument(argc, argv, "-num_views", num_views);
pcl17::console::parse_argument(argc, argv, "-height", height);
pcl17::console::parse_argument(argc, argv, "-noise_std", noise_std);
pcl17::console::parse_multiple_arguments(argc, argv, "-distance", distances);
pcl17::console::parse_multiple_arguments(argc, argv, "-tilt", tilt);
pcl17::console::parse_multiple_arguments(argc, argv, "-shift", shift);
PCL17_INFO("distances size: %d\n", distances.size());
for (size_t i = 0; i < distances.size(); i++)
{
PCL17_INFO("distance: %f\n", distances[i]);
}
// Set default values if user didn't provide any
if (distances.size() == 0)
distances.push_back(4);
if (tilt.size() == 0)
tilt.push_back(-15);
if (shift.size() == 0)
shift.push_back(0);
// Check if input path exists
boost::filesystem::path input_path(input_dir);
if (!boost::filesystem::exists(input_path))
{
PCL17_ERROR("Input directory doesnt exists.");
return -1;
}
// Check if input path is a directory
if (!boost::filesystem::is_directory(input_path))
{
PCL17_ERROR("%s is not directory.", input_path.c_str());
return -1;
}
// Check if output directory exists
boost::filesystem::path output_path(output_dir);
if (!boost::filesystem::exists(output_path) || !boost::filesystem::is_directory(output_path))
{
if (!boost::filesystem::create_directories(output_path))
{
PCL17_ERROR ("Error creating directory %s.\n", output_path.c_str ());
return -1;
}
}
// Find all .vtk files in the input directory
std::vector<std::string> files_to_process;
PCL17_INFO("Processing following files:\n");
boost::filesystem::directory_iterator end_iter;
for (boost::filesystem::directory_iterator iter(input_path); iter != end_iter; iter++)
{
boost::filesystem::path class_dir_path(*iter);
for (boost::filesystem::directory_iterator iter2(class_dir_path); iter2 != end_iter; iter2++)
{
boost::filesystem::path file(*iter2);
if (file.extension() == ".vtk")
{
files_to_process.push_back(file.c_str());
PCL17_INFO("\t%s\n", file.c_str());
}
}
}
// Check if there are any .vtk files to process
if (files_to_process.size() == 0)
{
PCL17_ERROR("Directory %s has no .vtk files.", input_path.c_str());
return -1;
}
// Iterate over all files
for (size_t i = 0; i < files_to_process.size(); i++)
{
vtkSmartPointer<vtkPolyData> model;
vtkSmartPointer<vtkPolyDataReader> reader = vtkPolyDataReader::New();
vtkSmartPointer<vtkTransform> transform = vtkTransform::New();
pcl17::PointCloud<pcl17::PointXYZ>::Ptr cloud(new pcl17::PointCloud<pcl17::PointXYZ>());
// Compute output directory for this model
std::vector<std::string> st;
boost::split(st, files_to_process.at(i), boost::is_any_of("/"), boost::token_compress_on);
std::string class_dirname = st.at(st.size() - 2);
std::string dirname = st.at(st.size() - 1);
dirname = dirname.substr(0, dirname.size() - 4);
dirname = output_dir + class_dirname + "/" + dirname;
// Check if output directory for this model exists. If not create
boost::filesystem::path dirpath(dirname);
if (!boost::filesystem::exists(dirpath))
{
if (!boost::filesystem::create_directories(dirpath))
{
PCL17_ERROR ("Error creating directory %s.\n", dirpath.c_str ());
return -1;
}
}
// Load model from file
reader->SetFileName(files_to_process.at(i).c_str());
reader->Update();
model = reader->GetOutput();
PCL17_INFO("Number of points %d\n",model->GetNumberOfPoints());
// Coumpute bounds and center of the model
double bounds[6];
model->GetBounds(bounds);
double min_z_value = bounds[4];
double center[3];
model->GetCenter(center);
createFullModelPointcloud(model, full_model_n_points, *cloud);
pcl17::io::savePCDFile(dirname + "/full.pcd", *cloud);
// Initialize PCLVisualizer. Add model to scene.
pcl17::visualization::PCLVisualizer viz;
viz.initCameraParameters();
viz.updateCamera();
viz.setCameraPosition(0, 0, 0, 1, 0, 0, 0, 0, 1);
viz.addModelFromPolyData(model, transform);
viz.setRepresentationToSurfaceForAllActors();
// Iterate over all shifts
for (size_t shift_index = 0; shift_index < shift.size(); shift_index++)
{
// Iterate over all tilts
for (size_t tilt_index = 0; tilt_index < tilt.size(); tilt_index++)
{
// Iterate over all distances
for (size_t distance_index = 0; distance_index < distances.size(); distance_index++)
{
// Iterate over all angles
double angle = 0;
double angle_step = 360.0 / num_views;
for (int angle_index = 0; angle_index < num_views; angle_index++)
{
// Set transformation with distance, shift, tilt and angle parameters.
transform->Identity();
transform->RotateY(tilt[tilt_index]);
transform->Translate(distances[distance_index], shift[shift_index], -(height + min_z_value));
transform->RotateZ(angle);
/*
// Render pointcloud
viz.renderView(640, 480, cloud);
//Add noise
addNoise(cloud, noise_std);
// Compute new coordinates of the model center
double new_center[3];
transform->TransformPoint(center, new_center);
// Shift origin of the poincloud to the model center and align with initial coordinate system.
pcl17::PointCloud<pcl17::PointXYZ>::Ptr cloud_transformed(new pcl17::PointCloud<pcl17::PointXYZ>());
moveToNewCenterAndAlign(cloud, cloud_transformed, new_center, tilt[tilt_index]);
// Compute file name for this pointcloud and save it
std::stringstream ss;
ss << dirname << "/rotation" << angle << "_distance" << distances[distance_index] << "_tilt"
<< tilt[tilt_index] << "_shift" << shift[shift_index] << ".pcd";
PCL17_INFO("Writing %d points to file %s\n", cloud->points.size(), ss.str().c_str());
pcl17::io::savePCDFile(ss.str(), *cloud_transformed);
*/
// increment angle by step
angle += angle_step;
}
}
}
}
}
return 0;
}
// --------------
// -----Main-----
// --------------
int main(int argc, char** argv)
{
// --------------------------------------
// -----Parse Command Line Arguments-----
// --------------------------------------
if (pcl17::console::find_argument(argc, argv, "-h") >= 0)
{
printUsage(argv[0]);
return 0;
}
bool simple(false), rgb(false), custom_c(false), normals(false), shapes(false), viewports(false),
interaction_customization(false);
if (pcl17::console::find_argument(argc, argv, "-s") >= 0)
{
simple = true;
std::cout << "Simple visualisation example\n";
}
else if (pcl17::console::find_argument(argc, argv, "-c") >= 0)
{
custom_c = true;
std::cout << "Custom colour visualisation example\n";
}
else if (pcl17::console::find_argument(argc, argv, "-r") >= 0)
{
rgb = true;
std::cout << "RGB colour visualisation example\n";
}
else if (pcl17::console::find_argument(argc, argv, "-n") >= 0)
{
normals = true;
std::cout << "Normals visualisation example\n";
}
else if (pcl17::console::find_argument(argc, argv, "-a") >= 0)
{
shapes = true;
std::cout << "Shapes visualisation example\n";
}
else if (pcl17::console::find_argument(argc, argv, "-v") >= 0)
{
viewports = true;
std::cout << "Viewports example\n";
}
else if (pcl17::console::find_argument(argc, argv, "-i") >= 0)
{
interaction_customization = true;
std::cout << "Interaction Customization example\n";
}
else
{
printUsage(argv[0]);
return 0;
}
// ------------------------------------
// -----Create example point cloud-----
// ------------------------------------
pcl17::PointCloud<pcl17::PointXYZRGB>::Ptr basic_cloud_ptr(new pcl17::PointCloud<pcl17::PointXYZRGB>);
pcl17::PointCloud<pcl17::PointXYZRGB>::Ptr point_cloud_ptr(new pcl17::PointCloud<pcl17::PointXYZRGB>);
pcl17::PointCloud<pcl17::PointXYZRGB>::Ptr cloud_f(new pcl17::PointCloud<pcl17::PointXYZRGB>);
pcl17::VoxelGrid<pcl17::PointXYZRGB > sor;
//pcl17::PLYReader reader;
//reader.read("box.ply",*point_cloud_ptr,0);
//pcl17::PointXYZRGB point;
if (pcl17::io::loadPCDFile<pcl17::PointXYZRGB>(argv[1], *point_cloud_ptr) == -1) //* load the file
{
PCL17_ERROR("Couldn't read file test_pcd.pcd \n");
return (-1);
}
basic_cloud_ptr = point_cloud_ptr;
/*
std::cout << "Loaded " << point_cloud_ptr->width * point_cloud_ptr->height << " data points" << std::endl;
cout << "gobbel" << endl;
sor.setInputCloud(point_cloud_ptr);
std::cerr << "PointCloud before filtering: " << point_cloud_ptr->width * point_cloud_ptr->height << " data points ("
<< pcl17::getFieldsList(*point_cloud_ptr) << ").";
std::cout << std::endl;
sor.setLeafSize(0.01, 0.01, 0.01);
sor.filter(*cloud_f);
pcl17::io::savePCDFileASCII("BoxFiltered.pcd", *cloud_f); */
/* for (size_t i = 0; i < point_cloud_ptr->points.size (); ++i)
std::cout << " " << point_cloud_ptr->points[i].x
<< " " << point_cloud_ptr->points[i].y
<< " " << point_cloud_ptr->points[i].z << std::endl;
cout << point_cloud_ptr->width << endl;
cout << point_cloud_ptr->height << endl;
for(int i=0;i<point_cloud_ptr->width * point_cloud_ptr->height;i++)
{
point=point_cloud_ptr->points.at(i);
point.r = 255;
point.g = 0;
point.b = 0;
basic_cloud_ptr->points.at(i)=point;
}
cout << basic_cloud_ptr->width << endl;
cout << basic_cloud_ptr->height << endl;
pcl17::PCDWriter writer;
writer.write<pcl17::PointXYZRGB> (argv[1], *basic_cloud_ptr, false);
point_cloud_ptr=basic_cloud_ptr; */
// ----------------------------------------------------------------
// -----Calculate surface normals with a search radius of 0.05-----
// ----------------------------------------------------------------
pcl17::NormalEstimation<pcl17::PointXYZRGB, pcl17::Normal> ne;
ne.setInputCloud(point_cloud_ptr);
pcl17::search::KdTree<pcl17::PointXYZRGB>::Ptr tree(new pcl17::search::KdTree<pcl17::PointXYZRGB>());
ne.setSearchMethod(tree);
pcl17::PointCloud<pcl17::Normal>::Ptr cloud_normals1(new pcl17::PointCloud<pcl17::Normal>);
ne.setRadiusSearch(0.05);
ne.compute(*cloud_normals1);
// ---------------------------------------------------------------
// -----Calculate surface normals with a search radius of 0.1-----
// ---------------------------------------------------------------
pcl17::PointCloud<pcl17::Normal>::Ptr cloud_normals2(new pcl17::PointCloud<pcl17::Normal>);
ne.setRadiusSearch(0.1);
ne.compute(*cloud_normals2);
boost::shared_ptr<pcl17::visualization::PCLVisualizer> viewer;
if (simple)
{
viewer = simpleVis(basic_cloud_ptr);
}
else if (rgb)
{
viewer = rgbVis(point_cloud_ptr);
}
else if (custom_c)
{
viewer = customColourVis(basic_cloud_ptr);
}
else if (normals)
{
viewer = normalsVis(point_cloud_ptr, cloud_normals2);
}
else if (shapes)
{
viewer = shapesVis(point_cloud_ptr);
}
else if (viewports)
{
viewer = viewportsVis(point_cloud_ptr, cloud_normals1, cloud_normals2);
}
else if (interaction_customization)
{
viewer = interactionCustomizationVis();
}
//--------------------
// -----Main loop-----
//--------------------
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
