int main (int argc, char* argv[])
{
boost::program_options::options_description desc("Options");
desc.add_options()
("help", "Print help message")
("pcd_filename", boost::program_options::value<std::string>()->required(), "pcl pcd filename");
boost::program_options::variables_map vm;
try {
boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
if( vm.count("help") ){
std::cout << "Resize Point cloud using voxel grid filter" << std::endl;
std::cerr << desc << std::endl;
return 0;
}
boost::program_options::notify(vm);
} catch (boost::program_options::error& e) {
std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
std::cerr << desc << std::endl;
return -1;
}
std::string pcd_fname = vm["pcd_filename"].as<std::string>();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile (pcd_fname, *source_cloud);
// Rotate pointcloud 0.6[rad] around z axis
Eigen::Matrix4f rotate_around_zaxis = Eigen::Matrix4f::Identity();
float theta = 0.6;
rotate_around_zaxis(0,0) = cos(theta);
rotate_around_zaxis(0,1) = -sin(theta);
rotate_around_zaxis(1,0) = sin(theta);
rotate_around_zaxis(1,1) = cos(theta);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rotated_cloud(new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::transformPointCloud(*source_cloud, *rotated_cloud, rotate_around_zaxis);
// Transform point cloud using homogeneous transformation matrix
// which is got by icp mathcing between 3D map reference point and manual map reference point
Eigen::Matrix4f icp_transform_matrix = Eigen::Matrix4f::Identity();
icp_transform_matrix(0, 0) = 0.950235;
icp_transform_matrix(0, 1) = -0.307961;
icp_transform_matrix(0, 2) = 0.0470266;
icp_transform_matrix(0, 3) = 0.147511;
icp_transform_matrix(1, 0) = 0.30832;
icp_transform_matrix(1, 1) = 0.951281;
icp_transform_matrix(1, 2) = -0.000411891;
icp_transform_matrix(1, 3) = -0.0305933;
icp_transform_matrix(2, 0) = -0.0446088;
icp_transform_matrix(2, 1) = 0.0148907;
icp_transform_matrix(2, 2) = 0.998893;
icp_transform_matrix(2, 3) = -0.0312964;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::transformPointCloud(*rotated_cloud, *transformed_cloud, icp_transform_matrix);
pcl::io::savePCDFileASCII("transformed_cloud.pcd", *transformed_cloud);
std::cout << "Write to transformed_cloud.pcd" << std::endl;
return 0;
}
void FloorFilter::CloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud) {
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(
new pcl::PointCloud<pcl::PointXYZ>);
if (!WaitForTransformToReferenceFrame(cloud->header.frame_id, cloud->header.stamp)) {
ROS_WARN("Cannot transform pointcloud to reference frame (%s -> %s).",
cloud->header.frame_id.c_str(), reference_frame_.c_str());
return;
}
try {
pcl_ros::transformPointCloud(reference_frame_, *cloud, *transformed_cloud, tf_listener_);
} catch (tf::TransformException e) {
// Transformation fails in particular at start up because tilting
// laser transforms might not be coming in yet. This is logged by
// TF already, so we don't add another logging here.
return;
}
if(!transformed_cloud->points.size()) {
ROS_WARN("The input cloud is empty. No obstacles in range?");
return;
}
std::vector<int> floor_candidate_indices;
FilterFloorCandidates(floor_z_distance_, tan(max_floor_y_rotation_), *transformed_cloud,
&floor_candidate_indices);
Eigen::ParametrizedLine<float, 3> floor_line;
std::vector<int> line_inlier_indices;
std::vector<int> indices_without_floor;
pcl::PointCloud<pcl::PointXYZ>::Ptr cliff_cloud(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> cliff_indices;
if (GetFloorLine(transformed_cloud, floor_candidate_indices, &floor_line, &line_inlier_indices)) {
GetIndicesDifference(transformed_cloud->size(), line_inlier_indices, &indices_without_floor);
GenerateCliffCloud(floor_line, *transformed_cloud, indices_without_floor,
cliff_cloud.get(), &cliff_indices);
}
else {
cliff_cloud->header = transformed_cloud->header;
}
// Always publish clouds even if they are empty to signal that
// perception is still alive.
PublishCloudFromIndices(*transformed_cloud, line_inlier_indices, floor_cloud_publisher_);
PublishCloudFromIndices(*transformed_cloud, indices_without_floor, filtered_cloud_publisher_);
PublishCloudFromIndices(*transformed_cloud, cliff_indices, cliff_generating_cloud_publisher_);
cliff_cloud_publisher_.publish(cliff_cloud);
ros::Duration message_age = ros::Time::now() - cloud->header.stamp;
// This is just a hint. Throw a warning to make the user know
// about something being fishy with the current configuration
// because input data is pretty old.
if (message_age > ros::Duration(1.0)) {
ROS_WARN("Filtered cloud already %lf seconds old.", message_age.toSec());
}
}
void addToGlobalCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in) {
/*
// Initialize global cloud if not already done so
if (!globalCloudPtr){
globalCloudPtr = cloud_in;
return;
}
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
transformCam2Robot (*cloud_in, *transformed_cloud);
// Initialize global cloud if not already done so
if (!globalCloudPtr) {
globalCloudPtr = transformed_cloud;
return;
}
/*
// Preform ICP
pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
icp.setInputCloud(transformed_cloud);
icp.setInputTarget(globalCloudPtr);
pcl::PointCloud<pcl::PointXYZRGB> Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
*globalCloudPtr += Final;
*/
//globalCloudPtr = transformed_cloud;
//return;
*globalCloudPtr += *transformed_cloud;
// Perform voxelgrid filtering
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::VoxelGrid<pcl::PointXYZRGB> sor;
sor.setInputCloud (globalCloudPtr);
sor.setLeafSize (res, res, res);
sor.filter (*cloud_filtered);
globalCloudPtr = cloud_filtered;
}
// generate colorized point cloud from point cloud and image data
void LaserCamHyperopt::update()
{
ROS_DEBUG("Fusing image and point cloud!!");
// set up transformation and use it to transform the point cloud
Eigen::Affine3d transform_plus_deltas;
Eigen::Vector3d translation(m_translate_laser_to_cam.x() + m_delta_x,
m_translate_laser_to_cam.y() + m_delta_y,
m_translate_laser_to_cam.z() + m_delta_z);
Eigen::Matrix3d rotation(m_rotate_laser_to_cam
* Eigen::AngleAxisd(m_delta_yaw, Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(m_delta_pitch, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(m_delta_roll, Eigen::Vector3d::UnitX()));
transform_plus_deltas = Eigen::Translation3d(translation) * rotation;
PointCloudIntensity::Ptr transformed_cloud(new PointCloudIntensity);
ROS_DEBUG_STREAM("applying transformation to cloud");
pcl::transformPointCloud(*m_cloud, *transformed_cloud, transform_plus_deltas);
// save points that are visible for the camera
PointCloudIntensity::Ptr visible_cloud(new PointCloudIntensity);
PointCloudRGB::Ptr colorized_cloud(new PointCloudRGB);
ROS_DEBUG_STREAM("colorizing cloud");
if(m_camera_model == "pinhole")
{
colorizeCloudPinhole(transformed_cloud, colorized_cloud, visible_cloud);
}
else if(m_camera_model == "omni")
{
colorizeCloudOmni(transformed_cloud, colorized_cloud, visible_cloud);
}
ROS_DEBUG_STREAM("computing mutual information");
//float mutual_information = mutual_information::getMutualInformation(visible_cloud, colorized_cloud);
m_mutual_information += mutual_information::getExtendedMutualInformation(visible_cloud, colorized_cloud);
}
void VoxelGridDownsampleManager::pointCB(const sensor_msgs::PointCloud2ConstPtr &input)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (grid_.size() == 0) {
ROS_INFO("the number of registered grids is 0, skipping");
return;
}
fromROSMsg(*input, *cloud);
for (size_t i = 0; i < grid_.size(); i++)
{
visualization_msgs::Marker::ConstPtr target_grid = grid_[i];
// not yet tf is supported
int id = target_grid->id;
// solve tf with ros::Time 0.0
// transform pointcloud to the frame_id of target_grid
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl_ros::transformPointCloud(target_grid->header.frame_id,
*cloud,
*transformed_cloud,
tf_listener);
double center_x = target_grid->pose.position.x;
double center_y = target_grid->pose.position.y;
double center_z = target_grid->pose.position.z;
double range_x = target_grid->scale.x * 1.0; // for debug
double range_y = target_grid->scale.y * 1.0;
double range_z = target_grid->scale.z * 1.0;
double min_x = center_x - range_x / 2.0;
double max_x = center_x + range_x / 2.0;
double min_y = center_y - range_y / 2.0;
double max_y = center_y + range_y / 2.0;
double min_z = center_z - range_z / 2.0;
double max_z = center_z + range_z / 2.0;
double resolution = target_grid->color.r;
// filter order: x -> y -> z -> downsample
pcl::PassThrough<pcl::PointXYZ> pass_x;
pass_x.setFilterFieldName("x");
pass_x.setFilterLimits(min_x, max_x);
pcl::PassThrough<pcl::PointXYZ> pass_y;
pass_y.setFilterFieldName("y");
pass_y.setFilterLimits(min_y, max_y);
pcl::PassThrough<pcl::PointXYZ> pass_z;
pass_z.setFilterFieldName("z");
pass_z.setFilterLimits(min_z, max_z);
ROS_INFO_STREAM(id << " filter x: " << min_x << " - " << max_x);
ROS_INFO_STREAM(id << " filter y: " << min_y << " - " << max_y);
ROS_INFO_STREAM(id << " filter z: " << min_z << " - " << max_z);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_x (new pcl::PointCloud<pcl::PointXYZ>);
pass_x.setInputCloud (transformed_cloud);
pass_x.filter(*cloud_after_x);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_y (new pcl::PointCloud<pcl::PointXYZ>);
pass_y.setInputCloud (cloud_after_x);
pass_y.filter(*cloud_after_y);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_z (new pcl::PointCloud<pcl::PointXYZ>);
pass_z.setInputCloud (cloud_after_y);
pass_z.filter(*cloud_after_z);
// downsample
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (cloud_after_z);
sor.setLeafSize (resolution, resolution, resolution);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
sor.filter (*cloud_filtered);
// reverse transform
pcl::PointCloud<pcl::PointXYZ>::Ptr reverse_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl_ros::transformPointCloud(input->header.frame_id,
*cloud_filtered,
*reverse_transformed_cloud,
tf_listener);
// adding the output into *output_cloud
// tmp <- cloud_filtered + output_cloud
// output_cloud <- tmp
//pcl::PointCloud<pcl::PointXYZ>::Ptr tmp (new pcl::PointCloud<pcl::PointXYZ>);
//pcl::concatenatePointCloud (*cloud_filtered, *output_cloud, tmp);
//output_cloud = tmp;
ROS_INFO_STREAM(id << " includes " << reverse_transformed_cloud->points.size() << " points");
for (size_t i = 0; i < reverse_transformed_cloud->points.size(); i++) {
output_cloud->points.push_back(reverse_transformed_cloud->points[i]);
}
}
sensor_msgs::PointCloud2 out;
toROSMsg(*output_cloud, out);
out.header = input->header;
pub_.publish(out); // for debugging
// for concatenater
size_t cluster_num = output_cloud->points.size() / max_points_ + 1;
ROS_INFO_STREAM("encoding into " << cluster_num << " clusters");
for (size_t i = 0; i < cluster_num; i++) {
size_t start_index = max_points_ * i;
size_t end_index = max_points_ * (i + 1) > output_cloud->points.size() ?
output_cloud->points.size(): max_points_ * (i + 1);
sensor_msgs::PointCloud2 cluster_out_ros;
pcl::PointCloud<pcl::PointXYZ>::Ptr
cluster_out_pcl(new pcl::PointCloud<pcl::PointXYZ>);
cluster_out_pcl->points.resize(end_index - start_index);
// build cluster_out_pcl
ROS_INFO_STREAM("make cluster from " << start_index << " to " << end_index);
for (size_t j = start_index; j < end_index; j++) {
cluster_out_pcl->points[j - start_index] = output_cloud->points[j];
}
// conevrt cluster_out_pcl into ros msg
toROSMsg(*cluster_out_pcl, cluster_out_ros);
cluster_out_ros.header = input->header;
cluster_out_ros.header.frame_id = (boost::format("%s %d %d") % (cluster_out_ros.header.frame_id) % (i) % (sequence_id_)).str();
pub_encoded_.publish(cluster_out_ros);
ros::Duration(1.0 / rate_).sleep();
}
}
void processCloud(const sensor_msgs::PointCloud2 msg)
{
//********* Retirive and process raw pointcloud************
std::cout<<"Recieved cloud"<<std::endl;
//std::cout<<"Create Octomap"<<std::endl;
//octomap::OcTree tree(res);
//std::cout<<"Load points "<<std::endl;
pcl::PCLPointCloud2 cloud;
pcl_conversions::toPCL(msg,cloud);
pcl::PointCloud<pcl::PointXYZ> pcl_pc;
pcl::fromPCLPointCloud2(cloud,pcl_pc);
//std::cout<<"Filter point clouds for NAN"<<std::endl;
std::vector<int> nan_indices;
pcl::removeNaNFromPointCloud(pcl_pc,pcl_pc,nan_indices);
//octomap::Pointcloud oct_pc;
//octomap::point3d origin(0.0f,0.0f,0.0f);
//std::cout<<"Adding point cloud to octomap"<<std::endl;
//octomap::point3d origin(0.0f,0.0f,0.0f);
//for(int i = 0;i<pcl_pc.points.size();i++){
//oct_pc.push_back((float) pcl_pc.points[i].x,(float) pcl_pc.points[i].y,(float) pcl_pc.points[i].z);
//}
//tree.insertPointCloud(oct_pc,origin,-1,false,false);
//*********** Remove the oldest data, update the data***************
cloud_seq_loaded.push_back(pcl_pc);
std::cout<<cloud_seq_loaded.size()<<std::endl;
if(cloud_seq_loaded.size()>2){
cloud_seq_loaded.pop_front();
}
//*********** Process currently observerd and buffered data*********
if(cloud_seq_loaded.size()==2){
//std::cout<< "Generating octomap"<<std::endl;
std::cout<<"Ransac"<<std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr prev_pc (new pcl::PointCloud<pcl::PointXYZ>);
*prev_pc = cloud_seq_loaded.front();
pcl::PointCloud<pcl::PointXYZ>::Ptr curr_pc (new pcl::PointCloud<pcl::PointXYZ>);
*curr_pc =pcl_pc;
pcl::RandomSample<pcl::PointXYZ> rs(true);
rs.setSample(20000);
rs.setInputCloud(curr_pc);
std::vector<int> indices;
rs.filter (indices);
pcl::PointCloud<pcl::PointXYZ> curr_cld_out;
rs.filter(curr_cld_out);
*curr_pc = curr_cld_out;
std::cout<<curr_pc->size()<<std::endl;
rs.setInputCloud(prev_pc);
rs.filter(indices);
pcl::PointCloud<pcl::PointXYZ> prev_cld_out;
rs.filter(prev_cld_out);
*prev_pc = prev_cld_out;
/*
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> esTrSVD;
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 trMat;
esTrSVD.estimateRigidTransformation(*prev_pc,*curr_pc,trMat);
std::cout<<trMat<<std::endl;
*/
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
//pcl::registration::TransformationEstimationPointToPlaneLLS < pcl::PointXYZ,pcl::PointXYZ,float>::Ptr trans_lls (new pcl::registration::TransformationEstimationPointToPlaneLLS < pcl::PointXYZ,pcl::PointXYZ,float>);
pcl::registration::TransformationEstimationLM< pcl::PointXYZ,pcl::PointXYZ,float >::Ptr trans_LM (new pcl::registration::TransformationEstimationLM< pcl::PointXYZ,pcl::PointXYZ,float >);
icp.setInputSource(prev_pc);
icp.setInputTarget(curr_pc);
icp.setTransformationEstimation (trans_LM);
//icp.setMaximumIterations (2);
//icp.setRANSACIterations(5);
icp.setTransformationEpsilon (1e-8);
icp.setMaxCorrespondenceDistance (0.5);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 trMat;
trMat = icp.getFinalTransformation();
//*curr_pc = pcl_pc;
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::transformPointCloud (*prev_pc, *transformed_cloud, trMat);
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
// Define R,G,B colors for the point cloud
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (prev_pc, 255, 255, 255);
// We add the point cloud to the viewer and pass the color handler
viewer.addPointCloud (prev_pc, source_cloud_color_handler, "original_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
viewer.addCoordinateSystem (1.0, 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
//viewer.setPosition(800, 400); // Setting visualiser window position
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
//std::cout << "Node center: " << it.getCoordinate() << std::endl;
//std::cout << "Node size: " << it.getSize() << std::endl;
//std::cout << "Node value: " << it->getValue() << std::endl;
}
//********** print out the statistics ******************
//**************Process Point cloud in octree data structure *****************
/*
//******************Traverse the tree ********************
for(octomap::OcTree::tree_iterator it =tree.begin_tree(), end = tree.end_tree();it!= end;it++){
//manipulate node, e.g.:
std::cout << "_____________________________________"<<std::endl;
std::cout << "Node center: " << it.getCoordinate() << std::endl;
std::cout << "Node size: " << it.getSize() << std::endl;
std::cout << "Node depth: "<<it.getDepth() << std::endl;
std::cout << "Is Leaf : "<< it.isLeaf()<< std::endl;
std::cout << "_____________________________________"<<std::endl;
}
//**********************************************************
*/
std::cout<<"finished"<<std::endl;
std::cout<<std::endl;
}
template<typename PointInT> int
pcl::TextureMapping<PointInT>::sortFacesByCamera (pcl::TextureMesh &tex_mesh, pcl::TextureMesh &sorted_mesh,
const pcl::texture_mapping::CameraVector &cameras, const double octree_voxel_size,
PointCloud &visible_pts)
{
if (tex_mesh.tex_polygons.size () != 1)
{
PCL_ERROR ("The mesh must contain only 1 sub-mesh!\n");
return (-1);
}
if (cameras.size () == 0)
{
PCL_ERROR ("Must provide at least one camera info!\n");
return (-1);
}
// copy mesh
sorted_mesh = tex_mesh;
// clear polygons from cleaned_mesh
sorted_mesh.tex_polygons.clear ();
pcl::PointCloud<pcl::PointXYZ>::Ptr original_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
// load points into a PCL format
pcl::fromPCLPointCloud2 (tex_mesh.cloud, *original_cloud);
// for each camera
for (size_t cam = 0; cam < cameras.size (); ++cam)
{
// get camera pose as transform
Eigen::Affine3f cam_trans = cameras[cam].pose;
// transform original cloud in camera coordinates
pcl::transformPointCloud (*original_cloud, *transformed_cloud, cam_trans.inverse ());
// find occlusions on transformed cloud
std::vector<int> visible, occluded;
removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded);
visible_pts = *filtered_cloud;
// find visible faces => add them to polygon N for camera N
// add polygon group for current camera in clean
std::vector<pcl::Vertices> visibleFaces_currentCam;
// iterate over the faces of the current mesh
for (size_t faces = 0; faces < tex_mesh.tex_polygons[0].size (); ++faces)
{
// check if all the face's points are visible
bool faceIsVisible = true;
std::vector<int>::iterator it;
// iterate over face's vertex
for (size_t current_pt_indice = 0; faceIsVisible && current_pt_indice < tex_mesh.tex_polygons[0][faces].vertices.size (); ++current_pt_indice)
{
// TODO this is far too long! Better create an helper function that raycasts here.
it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]);
if (it == occluded.end ())
{
// point is not occluded
// does it land on the camera's image plane?
pcl::PointXYZ pt = transformed_cloud->points[tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]];
Eigen::Vector2f dummy_UV;
if (!getPointUVCoordinates (pt, cameras[cam], dummy_UV))
{
// point is not visible by the camera
faceIsVisible = false;
}
}
else
{
faceIsVisible = false;
}
}
if (faceIsVisible)
{
// push current visible face into the sorted mesh
visibleFaces_currentCam.push_back (tex_mesh.tex_polygons[0][faces]);
// remove it from the unsorted mesh
tex_mesh.tex_polygons[0].erase (tex_mesh.tex_polygons[0].begin () + faces);
faces--;
}
}
sorted_mesh.tex_polygons.push_back (visibleFaces_currentCam);
}
// we should only have occluded and non-visible faces left in tex_mesh.tex_polygons[0]
// we need to add them as an extra polygon in the sorted mesh
sorted_mesh.tex_polygons.push_back (tex_mesh.tex_polygons[0]);
return (0);
}
// This is the main function
int
main (int argc, char** argv)
{
// Show help
if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help")) {
showHelp (argv[0]);
return 0;
}
// Fetch point cloud filename in arguments | Works with PCD and PLY files
std::vector<int> filenames;
bool file_is_pcd = false;
filenames = pcl::console::parse_file_extension_argument (argc, argv, ".ply");
if (filenames.size () != 1) {
filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
if (filenames.size () != 1) {
showHelp (argv[0]);
return -1;
} else {
file_is_pcd = true;
}
}
// Load file | Works with PCD and PLY files
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
if (file_is_pcd) {
if (pcl::io::loadPCDFile (argv[filenames[0]], *source_cloud) < 0) {
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
showHelp (argv[0]);
return -1;
}
} else {
if (pcl::io::loadPLYFile (argv[filenames[0]], *source_cloud) < 0) {
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
showHelp (argv[0]);
return -1;
}
}
/* Reminder: how transformation matrices work :
|-------> This column is the translation
| 1 0 0 x | \
| 0 1 0 y | }-> The identity 3x3 matrix (no rotation) on the left
| 0 0 1 z | /
| 0 0 0 1 | -> We do not use this line (and it has to stay 0,0,0,1)
METHOD #1: Using a Matrix4f
This is the "manual" method, perfect to understand but error prone !
*/
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
float theta = M_PI/4; // The angle of rotation in radians
transform_1 (0,0) = cos (theta);
transform_1 (0,1) = -sin(theta);
transform_1 (1,0) = sin (theta);
transform_1 (1,1) = cos (theta);
// (row, column)
// Define a translation of 2.5 meters on the x axis.
transform_1 (0,3) = 2.5;
// Print the transformation
printf ("Method #1: using a Matrix4f\n");
std::cout << transform_1 << std::endl;
/* METHOD #2: Using a Affine3f
This method is easier and less error prone
*/
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
// Define a translation of 2.5 meters on the x axis.
transform_2.translation() << 2.5, 0.0, 0.0;
// The same rotation matrix as before; theta radians arround Z axis
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
// Print the transformation
printf ("\nMethod #2: using an Affine3f\n");
std::cout << transform_2.matrix() << std::endl;
// Executing the transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);
// Visualization
printf( "\nPoint cloud colors : white = original point cloud\n"
" red = transformed point cloud\n");
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
// Define R,G,B colors for the point cloud
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);
// We add the point cloud to the viewer and pass the color handler
viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
viewer.addCoordinateSystem (1.0, "cloud", 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
//viewer.setPosition(800, 400); // Setting visualiser window position
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
return 0;
}
void callback(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
ros::Time whole_start = ros::Time::now();
ros::Time declare_types_start = ros::Time::now();
// filter
pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid;
pcl::PassThrough<sensor_msgs::PointCloud2> pass;
pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
pcl::ExtractIndices<pcl::Normal> extract_normals;
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> segmentation_from_normals;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree3 (new pcl::search::KdTree<pcl::PointXYZ> ());
// The plane and sphere coefficients
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());
// The plane and sphere inliers
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ());
// The point clouds
sensor_msgs::PointCloud2::Ptr voxelgrid_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr plane_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr cylinder_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
// The PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>);
// The cloud normals
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ()); // for plane
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal> ()); // for cylinder
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals3 (new pcl::PointCloud<pcl::Normal> ()); // for sphere
ros::Time declare_types_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Voxel grid Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// // Create VoxelGrid filtering
// voxel_grid.setInputCloud (cloud);
// voxel_grid.setLeafSize (0.01, 0.01, 0.01);
// voxel_grid.filter (*voxelgrid_filtered);
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*voxelgrid_filtered, *transformed_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Passthrough Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// pass through filter
// pass.setInputCloud (cloud);
// pass.setFilterFieldName ("z");
// pass.setFilterLimits (0, 1.5);
// pass.filter (*cloud_filtered);
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*cloud_filtered, *transformed_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Estimate point normals
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ros::Time estimate_start = ros::Time::now();
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*cloud, *transformed_cloud);
//
// // Estimate point normals
// normal_estimation.setSearchMethod (tree);
// normal_estimation.setInputCloud (transformed_cloud);
// normal_estimation.setKSearch (50);
// normal_estimation.compute (*cloud_normals);
//
// ros::Time estimate_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Create and processing the plane extraction
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ros::Time plane_start = ros::Time::now();
//
// // Create the segmentation object for the planar model and set all the parameters
// segmentation_from_normals.setOptimizeCoefficients (true);
// segmentation_from_normals.setModelType (pcl::SACMODEL_NORMAL_PLANE);
// segmentation_from_normals.setNormalDistanceWeight (0.1);
// segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
// segmentation_from_normals.setMaxIterations (100);
// segmentation_from_normals.setDistanceThreshold (0.03);
// segmentation_from_normals.setInputCloud (transformed_cloud);
// segmentation_from_normals.setInputNormals (cloud_normals);
//
// // Obtain the plane inliers and coefficients
// segmentation_from_normals.segment (*inliers_plane, *coefficients_plane);
// //std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
//
// // Extract the planar inliers from the input cloud
// extract_indices.setInputCloud (transformed_cloud);
// extract_indices.setIndices (inliers_plane);
// extract_indices.setNegative (false);
// extract_indices.filter (*cloud_plane);
//
// pcl::toROSMsg (*cloud_plane, *plane_output_cloud);
// plane_pub.publish(plane_output_cloud);
//
// ros::Time plane_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time plane_start = ros::Time::now();
pcl::fromROSMsg (*cloud, *transformed_cloud);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (transformed_cloud);
seg.segment (*inliers_plane, *coefficients_plane);
extract_indices.setInputCloud(transformed_cloud);
extract_indices.setIndices(inliers_plane);
extract_indices.setNegative(false);
extract_indices.filter(*cloud_plane);
pcl::toROSMsg (*cloud_plane, *plane_output_cloud);
plane_pub.publish(plane_output_cloud);
ros::Time plane_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Extract rest plane and passthrough filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time rest_pass_start = ros::Time::now();
// Create the filtering object
// Remove the planar inliers, extract the rest
extract_indices.setNegative (true);
extract_indices.filter (*remove_transformed_cloud);
transformed_cloud.swap (remove_transformed_cloud);
// publish result of Removal the planar inliers, extract the rest
pcl::toROSMsg (*transformed_cloud, *rest_output_cloud);
rest_pub.publish(rest_output_cloud);
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*rest_output_cloud, *cylinder_cloud);
// pass through filter
pass.setInputCloud (rest_output_cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.5);
pass.filter (*rest_cloud_filtered);
ros::Time rest_pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for cylinder features
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_cloud_filtered, *cylinder_cloud);
// Estimate point normals
normal_estimation.setSearchMethod (tree2);
normal_estimation.setInputCloud (cylinder_cloud);
normal_estimation.setKSearch (50);
normal_estimation.compute (*cloud_normals2);
// Create the segmentation object for sphere segmentation and set all the paopennirameters
segmentation_from_normals.setOptimizeCoefficients (true);
segmentation_from_normals.setModelType (pcl::SACMODEL_CYLINDER);
segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
segmentation_from_normals.setNormalDistanceWeight (0.1);
segmentation_from_normals.setMaxIterations (10000);
segmentation_from_normals.setDistanceThreshold (0.05);
segmentation_from_normals.setRadiusLimits (0, 0.5);
segmentation_from_normals.setInputCloud (cylinder_cloud);
segmentation_from_normals.setInputNormals (cloud_normals2);
// Obtain the sphere inliers and coefficients
segmentation_from_normals.segment (*inliers_cylinder, *coefficients_cylinder);
//std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Publish the sphere cloud
extract_indices.setInputCloud (cylinder_cloud);
extract_indices.setIndices (inliers_cylinder);
extract_indices.setNegative (false);
extract_indices.filter (*cylinder_output);
if (cylinder_output->points.empty ())
std::cerr << "Can't find the cylindrical component." << std::endl;
pcl::toROSMsg (*cylinder_output, *cylinder_output_cloud);
cylinder_pub.publish(cylinder_output_cloud);
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time sphere_start = ros::Time::now();
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_cloud_filtered, *sphere_cloud);
// Estimate point normals
normal_estimation.setSearchMethod (tree3);
normal_estimation.setInputCloud (sphere_cloud);
normal_estimation.setKSearch (50);
normal_estimation.compute (*cloud_normals3);
// Create the segmentation object for sphere segmentation and set all the paopennirameters
segmentation_from_normals.setOptimizeCoefficients (true);
//segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE);
segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE);
segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
segmentation_from_normals.setNormalDistanceWeight (0.1);
segmentation_from_normals.setMaxIterations (10000);
segmentation_from_normals.setDistanceThreshold (0.05);
segmentation_from_normals.setRadiusLimits (0, 0.2);
segmentation_from_normals.setInputCloud (sphere_cloud);
segmentation_from_normals.setInputNormals (cloud_normals3);
// Obtain the sphere inliers and coefficients
segmentation_from_normals.segment (*inliers_sphere, *coefficients_sphere);
//std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;
// Publish the sphere cloud
extract_indices.setInputCloud (sphere_cloud);
extract_indices.setIndices (inliers_sphere);
extract_indices.setNegative (false);
extract_indices.filter (*sphere_output);
if (sphere_output->points.empty ())
std::cerr << "Can't find the sphere component." << std::endl;
pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
sphere_pub.publish(sphere_output_cloud);
ros::Time sphere_end = ros::Time::now();
std::cout << "cloud size : " << cloud->width * cloud->height << std::endl;
std::cout << "plane size : " << transformed_cloud->width * transformed_cloud->height << std::endl;
//std::cout << "plane size : " << cloud_normals->width * cloud_normals->height << std::endl;
//std::cout << "cylinder size : " << cloud_normals2->width * cloud_normals2->height << std::endl;
std::cout << "sphere size : " << cloud_normals3->width * cloud_normals3->height << std::endl;
ros::Time whole_now = ros::Time::now();
printf("\n");
std::cout << "whole time : " << whole_now - whole_start << " sec" << std::endl;
std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl;
//std::cout << "estimate time : " << estimate_end - estimate_start << " sec" << std::endl;
std::cout << "plane time : " << plane_end - plane_start << " sec" << std::endl;
std::cout << "rest and pass time : " << rest_pass_end - rest_pass_start << " sec" << std::endl;
std::cout << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl;
printf("\n");
}
// I need to change the parameters to account for the
// transformation matrices. I need to save the best TM
// so I can combine it with the icp TM.
//
// Does kdtree search on each rotation of 45 degrees around
// the central point of the moved_cloud
Eigen::Matrix4f kdtree_search(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr moved_cloud, float radius)
{
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
// Total number of nearest neighbors
int num_matches = 0;
// Return the cloud with the most number of nearest neighbors
// within a given radius of the searchPoint
int max_neighbors = 0;
pcl::PointCloud<pcl::PointXYZ>::Ptr best_fit_cloud ;
Eigen::Matrix4f best_fit_transform = Eigen::Matrix4f::Identity();
Eigen::Vector4f centroid1 = compute_centroid(*moved_cloud);
cout << "centroid of source cloud - " << centroid1(0)
<< ", " << centroid1(1) << ", " << centroid1(2) << endl;
// Executing the transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr rotated_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
Eigen::Matrix4f transform_2 = Eigen::Matrix4f::Identity();
for (int i = 0; i < 8; i++)
{
float theta = 45 * i + 45;
transform_1 (0,0) = cos (theta);
transform_1 (0,2) = -sin (theta);
transform_1 (2,0) = sin (theta);
transform_1 (2,2) = cos (theta);
cout << "cloud with " << theta << " degrees of rotation" << endl;
pcl::transformPointCloud (*moved_cloud, *rotated_cloud, transform_1);
// Probably need to compute centroid of the new transformed cloud
// because the transformation seems to translate it
Eigen::Vector4f centroid2 = compute_centroid(*rotated_cloud);
cout << "centroid of rotated cloud - " << centroid2(0)
<< ", " << centroid2(1) << ", " << centroid2(2) << endl;
float distance_x = centroid1(0) - centroid2(0);
float distance_y = centroid1(1) - centroid2(1);
float distance_z = centroid1(2) - centroid2(2);
cout << "distance between centroids: (" << distance_x << ", " << distance_y << ", " << distance_z << ")" << endl;
transform_2 (0,3) = (distance_x);
transform_2 (1,3) = (distance_y);
transform_2 (2,3) = (distance_z);
pcl::transformPointCloud (*rotated_cloud, *transformed_cloud, transform_2);
// Rotate the cloud by 45 degrees each time
// May want to add some random translation as well.
// This would correspond to doing kdtree search on a number of
// clouds that are presumably close to the target point cloud.
kdtree.setInputCloud(transformed_cloud);
// Run the kdtree search on every 10th point of the moved_cloud
// Increase this number to speed up the search
// Test with different increments of i to see effect on speed
for (int j = 0; j < (*moved_cloud).points.size(); j += 10)
{
pcl::PointXYZ searchPoint = moved_cloud->points[j];
int num_neighbors = kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
num_matches += num_neighbors;
cout << "performed kdtree nearest neighbor search, found " << num_neighbors << " within " << radius << " radius" << endl;
}
cout << "num_matches = " << num_matches << endl;
cout << "max_neighbors = " << max_neighbors << endl;
if (num_matches > max_neighbors) {
max_neighbors = num_matches;
best_fit_cloud = transformed_cloud;
// are these transforms relative? or absolute?
// this currently calculates relative
// should be transform_2 * transform_1 if absolute
best_fit_transform = transform_2 * transform_1;
}
num_matches = 0;
printf( "\nPoint cloud colors : white = original point cloud\n"
" red = transformed point cloud\n"
" blue = moved point cloud\n"
" green = rotated point cloud\n");
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
// Define R,G,B colors for the point cloud
// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255); // white
// We add the point cloud to the viewer and pass the color handler
// viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> rotated_cloud_color_handler (rotated_cloud, 20, 245, 20); // green
viewer.addPointCloud (rotated_cloud, rotated_cloud_color_handler, "rotated_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> moved_cloud_color_handler (moved_cloud, 20, 230, 230); // blue
viewer.addPointCloud (moved_cloud, moved_cloud_color_handler, "moved_cloud");
viewer.addCoordinateSystem (1.0, 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
// viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "rotated_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "moved_cloud");
//viewer.setPosition(800, 400); // Setting visualiser window position
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
}
// check whether the translations are relative or absolute
pcl::PointCloud<pcl::PointXYZ>::Ptr best_transform_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::transformPointCloud (*moved_cloud, *best_transform_cloud, best_fit_transform);
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> best_transform_cloud_color_handler (best_transform_cloud, 245, 20, 20); // red
viewer.addPointCloud (best_transform_cloud, best_transform_cloud_color_handler, "best_transform_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> best_fit_cloud_color_handler (best_fit_cloud, 20, 245, 20); // green
viewer.addPointCloud (best_transform_cloud, best_fit_cloud_color_handler, "best_fit_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> input_cloud_color_handler (input_cloud, 255, 255, 255); // white
viewer.addPointCloud (input_cloud, input_cloud_color_handler, "input_cloud");
viewer.addCoordinateSystem (1.0, 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "best_transform_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "best_fit_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "input_cloud");
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
return best_fit_transform;
}
// Main function
int main (int argc, char** argv)
{
// Show help
if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help")) {
showHelp (argv[0]);
return 0;
}
// Fetch point cloud filename in arguments | Works with PCD files
std::vector<int> filenames;
bool file_is_pcd = false;
if (filenames.size () != 2) {
filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
if (filenames.size () != 2) {
showHelp (argv[0]);
return -1;
} else {
file_is_pcd = true;
}
}
// Load file | Works with PCD and PLY files
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
// The source cloud is the original cloud
if (pcl::io::loadPCDFile (argv[filenames[0]], *source_cloud) < 0) {
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
showHelp (argv[0]);
return -1;
}
// The moved cloud is the original cloud after a transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr moved_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::io::loadPCDFile (argv[filenames[1]], *moved_cloud);
// compute the distance between the source_cloud's centroid and
// the moved_cloud's centroid
/*
Eigen::Vector4f zero(4);
zero << 0,0,0,0;
unsigned int centroid1 = pcl::compute3DCentroid(*source_cloud, zero);
unsigned int centroid2 = pcl::compute3DCentroid(*moved_cloud, zero);
float distance = 0;
float centroid1d = (float)centroid1;
float centroid2d = (float)centroid2;
// ints or floats won't do. I need a 3D vector or something
// so i can translate the x,y,z points over to the new location.
// unfortunately, the only other compute centroid function I can
// find returns void.
distance = (centroid1d - centroid2d) / 1000.0;
cout << centroid1 << std::endl;
cout << centroid2 << std::endl;
cout << distance << std::endl;
*/
/*
If I'm going to translate the new cloud to the correct
centroid position later, I don't need this initial translation.
Eigen::Vector4f centroid2 = compute_centroid(*moved_cloud);
float distance_x = centroid2(0) - centroid1(0);
float distance_y = centroid2(1) - centroid1(1);
float distance_z = centroid2(2) - centroid1(2);
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
// float theta = 45; // The angle of rotation in radians
// transform_1 (2,0) = sin (theta);
// transform_1 (2,1) = cos (theta);
transform_1 (0,3) = abs(distance_x);
transform_1 (1,3) = abs(distance_y);
transform_1 (2,3) = abs(distance_z);
*/
// Executing the transformation
// pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
// pcl::transformPointCloud (*source_cloud, *transform_cloud, transform_1);
// transformed_cloud = kdtree_search(moved_cloud, transform_cloud);
/*
for (int i = 0; i < 8; i++)
{
// this transformation isn't giving me an overlay of the moved
// cloud at different angle rotations
// it's accompanied by a translation that puts the resulting
// cloud too far from the moved_cloud, so the kdtree estimate
// won't be too good
// However, if icp can correct for this, it might be ok
// ideally, I would want this to work as well as possible
// to speed up icp.
// I also am not passing the original centroid-translated
// point cloud to this kdtree search. I need to find a way to do that
//
// The issue here is that I'm not translating the cloud correctly
// based on the distance between the centroids. It's more complicated
// than just translating the cloud by the distance.
// Look at the picture Robbie drew. There's lin alg involved.
// passing in 0 for theta doesn't work because cos(0) = 1
// Add 45 degrees so that you don't do cos 0
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
Eigen::Matrix4f best_fit_transform = kdtree_search(source_cloud, moved_cloud, 0.04);
pcl::transformPointCloud (*moved_cloud, *transformed_cloud, best_fit_transform);
// Visualization
printf( "\nPoint cloud colors : white = original point cloud\n"
" red = transformed point cloud\n"
" blue = moved point cloud\n");
// " green = rotated point cloud\n");
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
// Define R,G,B colors for the point cloud
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255); // white
// We add the point cloud to the viewer and pass the color handler
viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");
/*
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> rotated_cloud_color_handler (rotated_cloud, 20, 245, 20); // green
viewer.addPointCloud (rotated_cloud, rotated_cloud_color_handler, "rotated_cloud");
*/
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> moved_cloud_color_handler (moved_cloud, 20, 230, 230); // blue
viewer.addPointCloud (moved_cloud, moved_cloud_color_handler, "moved_cloud");
viewer.addCoordinateSystem (1.0, 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
// viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "rotated_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "moved_cloud");
//viewer.setPosition(800, 400); // Setting visualiser window position
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
return 0;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, const Eigen::Matrix4f& transform) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::transformPointCloud(*cloud, *transformed_cloud, transform);
return transformed_cloud;
}
