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); }
int main (int argc, char** argv) { bool second_rotation_performed = false; bool visualize_results = true; boost::posix_time::ptime t_s = boost::posix_time::microsec_clock::local_time(); std::vector<int> filenames; filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); if (filenames.size () != 1) { std::cout << "Usage: input_file_path.pcd\n"; exit (-1); } std::string input_filename = argv[filenames.at(0)]; pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr original_cloud(new pcl::PointCloud<pcl::PointXYZRGB>); // pcl::PointCloud<pcl::PointXYZRGB>::Ptr final (new pcl::PointCloud<pcl::PointXYZRGB>); if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (input_filename, *original_cloud) == -1) //* load the file { PCL_ERROR ("Couldn't read input file\n"); exit (-1); } std::cout << "Loaded " << original_cloud->width * original_cloud->height << " data points from input pcd" << std::endl; boost::posix_time::ptime t_file_loaded = boost::posix_time::microsec_clock::local_time(); // Copy the original cloud to input cloud, which can be modified later during plane extraction pcl::copyPointCloud<pcl::PointXYZRGB, pcl::PointXYZRGB>(*original_cloud, *input_cloud); CuboidMatcher cm; cm.setInputCloud(input_cloud); cm.setDebug(true); cm.setSaveIntermediateResults(true); Cuboid cuboid; cm.execute(cuboid); boost::posix_time::ptime t_algorithm_done = boost::posix_time::microsec_clock::local_time(); printDuration(t_file_loaded, t_algorithm_done, "Algorithm Runtime"); std::cout << "Algorithm done. Visualize results" << std::endl; std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> intermediate_clouds; intermediate_clouds = cm.getIntermediateClouds(); if(!visualize_results) exit(0); // Atleast one intermediate clouds means, that the user want's to display them here. std::cout << "The algorithm did " << intermediate_clouds.size() << " transformation(s)" << std::endl; // Create the viewports for the rotated object pcl::visualization::PCLVisualizer viewer; // Visualize original pointcloud int v1(0); viewer.createViewPort(0.0, 0.0, 0.3, 1.0, v1); viewer.addCoordinateSystem(1.0,v1); // Visualize the found inliers int v2(1); viewer.createViewPort(0.3, 0.0, 0.6, 1.0, v2); viewer.addCoordinateSystem(1.0,v2); int v3(2); viewer.createViewPort(0.6, 0.0, 1.0, 1.0, v3); viewer.addCoordinateSystem(0.5,v3); // Fill the viewpoints with the desired clouds // pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(original_cloud); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> red_pts (original_cloud, 255,0,0); // viewer.addPointCloud<pcl::PointXYZRGB> (original_cloud, rgb, "sample cloud1", v1); viewer.addPointCloud<pcl::PointXYZRGB> (original_cloud, red_pts, "sample cloud1", v1); if( intermediate_clouds.size() >= 1) { // pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_v2(intermediate_clouds.at(0)); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> red_v2 (intermediate_clouds.at(0), 255,0,0); // viewer.addPointCloud<pcl::PointXYZRGB> (intermediate_clouds.at(0), rgb_v2, "first rotated cloud", v2); viewer.addPointCloud<pcl::PointXYZRGB> (intermediate_clouds.at(0), red_v2, "first rotated cloud", v2); } if( intermediate_clouds.size() >= 2) { // pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_v3(intermediate_clouds.at(1)); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> red_v3 (intermediate_clouds.at(1), 255,0,0); viewer.addPointCloud<pcl::PointXYZRGB> (intermediate_clouds.at(1), red_v3, "second rotated cloud", v3); // viewer.addPointCloud<pcl::PointXYZRGB> (intermediate_clouds.at(1), rgb_v3, "second rotated cloud", v3); } // Draw the bounding box from the given corner points drawBoundingBoxLines(viewer, cuboid.corner_points, v1); viewer.addCube ( Eigen::Vector3f(0,0,0), Eigen::Quaternion<float>::Identity(), cuboid.length1, cuboid.length2, cuboid.length3, "matched_cuboid", v2 ); viewer.addCube ( cuboid.center, cuboid.orientation, cuboid.length1, cuboid.length2, cuboid.length3, "matched_cuboid_centered", v1 ); viewer.spin(); /* boost::posix_time::ptime t_extraction_done = boost::posix_time::microsec_clock::local_time(); // -------------------------------------------- // -----Open 3D viewer and add point cloud----- // -------------------------------------------- // // Store the possible different colors for the segmented planes std::vector<std::vector<int> > color_sequence; std::vector<int> green; green.push_back(0); green.push_back(255); green.push_back(0); std::vector<int> red; red.push_back(255); red.push_back(0); red.push_back(0); std::vector<int> blue; blue.push_back(0); blue.push_back(0); blue.push_back(255); color_sequence.push_back(green); color_sequence.push_back(red); color_sequence.push_back(blue); pcl::visualization::PCLVisualizer viewer; // Visualize original pointcloud int v1(0); viewer.createViewPort(0.0, 0.0, 0.2, 1.0, v1); viewer.addCoordinateSystem(1.0,v1); pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(original_cloud); viewer.addPointCloud<pcl::PointXYZRGB> (original_cloud, rgb, "sample cloud1", v1); // Visualize the found inliers int v2(1); viewer.createViewPort(0.2, 0.0, 0.6, 1.0, v2); viewer.addCoordinateSystem(1.0,v2); int v3(2); viewer.createViewPort(0.6, 0.0, 1.0, 1.0, v3); viewer.addCoordinateSystem(0.5,v3); // For each segmented plane for (int i = 0; i < detected_planes.size(); i++) { // Select a color from the color_sequence vector for the discovered plane int color_idx = i % 3; pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> single_color (detected_planes.at(i).getPoints(), color_sequence.at(color_idx).at(0), color_sequence.at(color_idx).at(1), color_sequence.at(color_idx).at(2)); std::stringstream cloud_name("plane_cloud_"); cloud_name << i; std::stringstream plane_name("model_plane_"); plane_name << i; viewer.addPointCloud<pcl::PointXYZRGB> (detected_planes.at(i).getPoints(), single_color, cloud_name.str(), v2); viewer.addPlane (*detected_planes.at(i).getCoefficients(), plane_name.str(), v2); // Display the centroid of the planes pcl::PointXYZRGB c; c.x = detected_planes.at(i).getCentroid()(0); c.y = detected_planes.at(i).getCentroid()(1); c.z = detected_planes.at(i).getCentroid()(2); pcl::PointCloud<pcl::PointXYZRGB>::Ptr centroid_cloud(new pcl::PointCloud<pcl::PointXYZRGB>); centroid_cloud->push_back(c); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> centroid_yellow (centroid_cloud, 255,255,0); std::stringstream centroid_name("centroid_"); centroid_name << i; viewer.addPointCloud<pcl::PointXYZRGB> (centroid_cloud, centroid_yellow, centroid_name.str(), v2); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, centroid_name.str()); // Display the plane normal with the camera origin as the origin { pcl::PointXYZRGB origin; origin.x=0; origin.y=0; origin.z=0; // Get the normal from the plane pcl::PointXYZRGB dest; dest.x = detected_planes.at(i).getCoefficients()->values.at(0); dest.y = detected_planes.at(i).getCoefficients()->values.at(1); dest.z = detected_planes.at(i).getCoefficients()->values.at(2); pcl::PointCloud<pcl::PointXYZRGB>::Ptr origin_cloud (new pcl::PointCloud<pcl::PointXYZRGB>); origin_cloud->push_back(origin); pcl::PointCloud<pcl::PointXYZRGB>::Ptr origin_cloud_projected (new pcl::PointCloud<pcl::PointXYZRGB>); // Project the origin point to the plane pcl::ProjectInliers<pcl::PointXYZRGB> proj; proj.setModelType (pcl::SACMODEL_PLANE); proj.setInputCloud (origin_cloud); proj.setModelCoefficients (detected_planes.at(i).getCoefficients()); proj.filter (*origin_cloud_projected); if(origin_cloud_projected->points.size()!=1) { std::cout << "Projection fail" << std::endl; return 0; } std::stringstream plane_name("norm_plane_"); plane_name << i; viewer.addLine(origin_cloud_projected->points.at(0),dest, color_sequence.at(color_idx).at(0), color_sequence.at(color_idx).at(1), color_sequence.at(color_idx).at(2), plane_name.str(),v2); Eigen::Vector3f norm_origin( origin_cloud_projected->points.at(0).x, origin_cloud_projected->points.at(0).y, origin_cloud_projected->points.at(0).z ); detected_planes.at(i).setNormOrigin(norm_origin); } } // Build a coordinate system for biggest plane // Step 1) Visualize the coordinate system // // Transform the destination of the norm vector, to let it point in a 90° Angle from the // centroid to its destination Eigen::Vector3f newDest = moveVectorBySubtraction( detected_planes.at(0).getCoefficientsAsVector3f(), // The Centroid of the plane cloud detected_planes.at(0).getCentroidAsVector3f(), // And the center of the norm origin detected_planes.at(0).getNormOrigin() ); // Translate the norm vector of the second plane to the centroid of the first plane viewer.addLine( getPointXYZFromVector4f(detected_planes.at(0).getCentroid()), getPointXYZFromVector3f(newDest), 0, 255, 0, "f1",v2); Eigen::Vector3f secondDest = moveVectorBySubtraction( detected_planes.at(1).getCoefficientsAsVector3f(), // The Centroid of the plane cloud detected_planes.at(0).getCentroidAsVector3f(), // And the center of the norm origin detected_planes.at(1).getNormOrigin() ); // Translate the norm vector of the second plane to the centroid of the first plane viewer.addLine( getPointXYZFromVector4f(detected_planes.at(0).getCentroid()), getPointXYZFromVector3f(secondDest), 255, 0, 0, "f2",v2); // Create the third axis by using the cross product Eigen::Vector3f thirdDest = newDest.cross(secondDest); viewer.addLine( getPointXYZFromVector4f(detected_planes.at(0).getCentroid()), getPointXYZFromVector3f(thirdDest), 0, 0, 255, "f3",v2); // // ROTATE THE OBJECT to align it with the x-y and the x-z plane // TODO: dynamic // pcl::PointCloud<pcl::PointXYZRGB>::Ptr rotated_cloud(new pcl::PointCloud<pcl::PointXYZRGB>); // Align the front face with the cameras front plane { for (int i = 0; i < detected_planes.size(); i++) { std::cout << "Angle between Normal " << i << " and x-y Normal "; Eigen::Vector3f n2( 0,0,1); float angle = detected_planes.at(i).angleBetween(n2); std::cout << ": " << angle << " RAD, " << ((angle * 180) / M_PI) << " DEG"; std::cout << std::endl; } std::cout << "Angle between Normal 0 and x-y Normal "; Eigen::Vector3f n2( 0,0,1); float angle = detected_planes.at(0).angleBetween(n2); pcl::PointXYZRGB dest; dest.x = detected_planes.at(0).getCoefficients()->values.at(0); dest.y = detected_planes.at(0).getCoefficients()->values.at(1); dest.z = detected_planes.at(0).getCoefficients()->values.at(2); // Translate the first plane's origin to the camera origin translatePointCloud(original_cloud, -detected_planes.at(0).getCentroid()[0], -detected_planes.at(0).getCentroid()[1], -detected_planes.at(0).getCentroid()[2], rotated_cloud); // M Eigen::Vector3f plane_normal(dest.x, dest.y, dest.z); // Compute the necessary rotation to align a face of the object with the camera's // imaginary image plane // N Eigen::Vector3f camera_normal; camera_normal(0)=0; camera_normal(1)=0; camera_normal(2)=1; camera_normal = CuboidMatcher::reduceNormAngle(plane_normal, camera_normal); // float dotproduct3 = camera_normal.dot(plane_normal); // if(acos(dotproduct3)> M_PI/2) // { // std::cout << "NORM IS ABOVE 90 DEG! TURN IN THE OTHER DIRECTION" << std::endl; // camera_normal = -camera_normal; // // rotated_normal_of_second_plane = - rotated_normal_of_second_plane; // } Eigen::Matrix< float, 4, 4 > rotationBox = rotateAroundCrossProductOfNormals(camera_normal, plane_normal); pcl::transformPointCloud (*rotated_cloud, *rotated_cloud, rotationBox); // Draw the rotated object in the first window pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_r(rotated_cloud); viewer.addPointCloud<pcl::PointXYZRGB> (rotated_cloud, rgb_r, "rotated_cloud", v2); // Rotate the normal of the second plane with the first rotation matrix Eigen::Vector3f normal_of_second_plane = detected_planes.at(1).getCoefficientsAsVector3f(); Eigen::Vector3f rotated_normal_of_second_plane; rotated_normal_of_second_plane = removeTranslationVectorFromMatrix(rotationBox) * normal_of_second_plane; // Now align the second normal (which has been rotated) with the x-z plane Eigen::Vector3f xz_plane; xz_plane(0)=0; xz_plane(1)=1; xz_plane(2)=0; std::cout << "Angle between rotated normal and xz-plane "; float dotproduct2 = xz_plane.dot(rotated_normal_of_second_plane); std::cout << ": " << acos(dotproduct2) << " RAD, " << ((acos(dotproduct2) * 180) / M_PI) << " DEG"; std::cout << std::endl; if(acos(dotproduct2)> M_PI/2) { std::cout << "NORM IS ABOVE 90 DEG! TURN IN THE OTHER DIRECTION" << std::endl; rotated_normal_of_second_plane = - rotated_normal_of_second_plane; } float angle_between_xz_and_second_normal = ((acos(dotproduct2) * 180) / M_PI); Eigen::Matrix< float, 4, 4 > secondRotation; if(angle_between_xz_and_second_normal > MIN_ANGLE && angle_between_xz_and_second_normal < 180-MIN_ANGLE) { // Rotate the original centroid Eigen::Vector4f offset_between_centroids = detected_planes.at(1).getCentroid() - detected_planes.at(0).getCentroid(); Eigen::Vector3f rotated_offset = removeTranslationVectorFromMatrix(rotationBox) * getVector3fFromVector4f(offset_between_centroids); // translatePointCloud(rotated_cloud, // - rotated_offset[0], // - rotated_offset[1], // - rotated_offset[2], // rotated_cloud); secondRotation = rotateAroundCrossProductOfNormals(xz_plane, rotated_normal_of_second_plane); pcl::transformPointCloud (*rotated_cloud, *rotated_cloud, secondRotation); second_rotation_performed = true; } else { secondRotation = Eigen::Matrix< float, 4, 4 >::Identity(); second_rotation_performed = false; std::cout << "No second rotation, since the angle is too small: "<< angle_between_xz_and_second_normal << "DEG" << std::endl; } pcl::PointXYZ origin(0,0,0); viewer.addLine(origin, getPointXYZFromVector3f(rotated_normal_of_second_plane) , 255, 0, 0, "normal1",v2); // Draw the rotated object pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_r2(rotated_cloud); viewer.addPointCloud<pcl::PointXYZRGB> (rotated_cloud, rgb_r2, "rotated_cloud_second", v3); // Compute the bounding box for the rotated object pcl::PointXYZRGB min_pt, max_pt; pcl::getMinMax3D(*rotated_cloud, min_pt, max_pt); // Compute the bounding box edge points pcl::PointCloud<pcl::PointXYZRGB>::Ptr manual_bounding_box(new pcl::PointCloud<pcl::PointXYZRGB>); computeCuboidCornersWithMinMax3D(rotated_cloud,manual_bounding_box); // Draw the bounding box edge points pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> red_pts (manual_bounding_box, 255,0,0); viewer.addPointCloud<pcl::PointXYZRGB> (manual_bounding_box, red_pts, "bb", v3); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "bb"); // Now reverse all the transformations to get the bounding box around the actual object pcl::PointCloud<pcl::PointXYZRGB>::Ptr bounding_box_on_object(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> white_pts (bounding_box_on_object, 255,255,255); // Inverse the last rotation pcl::transformPointCloud (*manual_bounding_box, *bounding_box_on_object, secondRotation.transpose()); // Translate back to the first centroid, if this was done if(second_rotation_performed) { Eigen::Vector4f offset_between_centroids = detected_planes.at(1).getCentroid() - detected_planes.at(0).getCentroid(); Eigen::Vector3f rotated_offset = removeTranslationVectorFromMatrix(rotationBox) * getVector3fFromVector4f(offset_between_centroids); // translatePointCloud(bounding_box_on_object, // rotated_offset[0], // Translation is now NOT negative // rotated_offset[1], // rotated_offset[2], // bounding_box_on_object); } // Translated to first centroid // Inverse the second rotation pcl::transformPointCloud (*bounding_box_on_object, *bounding_box_on_object, rotationBox.transpose()); // And translate back to the original position translatePointCloud(bounding_box_on_object, detected_planes.at(0).getCentroid()[0], detected_planes.at(0).getCentroid()[1], detected_planes.at(0).getCentroid()[2], bounding_box_on_object); viewer.addPointCloud<pcl::PointXYZRGB> (bounding_box_on_object, white_pts, "bb_real", v3); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "bb_real"); drawBoundingBoxLines(viewer, bounding_box_on_object, v3); // And finally, render the original cloud pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_v3(original_cloud); viewer.addPointCloud<pcl::PointXYZRGB> (original_cloud, rgb_v3, "original_cloud", v3); std::cout << "Cuboid statistics" << std::endl; Cuboid c = computeCuboidFromBorderPoints(bounding_box_on_object); std::cout << "Width: " << c.length1 << " Height: " << c.length2 << " Depth: " << c.length3 << " Volume: " << c.volume << " m^3" << std::endl; } boost::posix_time::ptime t_done = boost::posix_time::microsec_clock::local_time(); printDuration(t_s, t_file_loaded, "Loaded file"); printDuration(t_file_loaded, t_extraction_done, "Plane Extraction, Normal Estimation"); printDuration(t_extraction_done, t_done, "Rotation and Visualization"); printDuration(t_s, t_done, "Overall"); // // Create a plane x-y plane that originates in the kinect camera frame // pcl::ModelCoefficients::Ptr kinect_plane_coefficients (new pcl::ModelCoefficients); // Let the Normal of the plane point along the z-Axis kinect_plane_coefficients->values.push_back(0); // x kinect_plane_coefficients->values.push_back(0); // y kinect_plane_coefficients->values.push_back(1); // z kinect_plane_coefficients->values.push_back(0); // d viewer.addPlane (*kinect_plane_coefficients, "kinect_plane", v2); viewer.spin(); */ return (0); }