From 1b6a8b93cb18ca8748e877175bcc9107177c0a84 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 3 Apr 2014 15:56:01 +0000 Subject: [PATCH] Added rtabmap-extractObject tool to extract objects laying on a plane, doing required transform to make sure that the plane and objects are transformed to (0,0,1) axis git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1287 f169173b-cf89-36c8-b27e-44dbe73f0c83 --- tools/CMakeLists.txt | 1 + tools/ExtractObject/CMakeLists.txt | 30 ++++ tools/ExtractObject/main.cpp | 243 +++++++++++++++++++++++++++++ 3 files changed, 274 insertions(+) create mode 100644 tools/ExtractObject/CMakeLists.txt create mode 100644 tools/ExtractObject/main.cpp diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index 9b43bc41..7c52dcdf 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -4,6 +4,7 @@ ADD_SUBDIRECTORY( ImagesJoiner ) ADD_SUBDIRECTORY( VocabularyComparison ) ADD_SUBDIRECTORY( OdometryViewer ) ADD_SUBDIRECTORY( DataRecorder ) +ADD_SUBDIRECTORY( ExtractObject ) IF(TARGET rtabmap_gui) ADD_SUBDIRECTORY( DatabaseViewer ) diff --git a/tools/ExtractObject/CMakeLists.txt b/tools/ExtractObject/CMakeLists.txt new file mode 100644 index 00000000..d5cd0fd1 --- /dev/null +++ b/tools/ExtractObject/CMakeLists.txt @@ -0,0 +1,30 @@ + +SET(SRC_FILES + main.cpp +) + +SET(INCLUDE_DIRS + ${PCL_INCLUDE_DIRS} + ${PROJECT_SOURCE_DIR}/utilite/include +) + +SET(LIBRARIES + ${PCL_LIBRARIES} +) + +add_definitions(${PCL_DEFINITIONS}) + +# Make sure the compiler can find include files from our library. +INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) + +# Add binary called "consoleApp" that is built from the source file "main.cpp". +# The extension is automatically found. +ADD_EXECUTABLE(extractObject ${SRC_FILES}) +TARGET_LINK_LIBRARIES(extractObject rtabmap_utilite ${LIBRARIES}) + +SET_TARGET_PROPERTIES( extractObject + PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-extractObject) + +INSTALL(TARGETS extractObject + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime + BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime) \ No newline at end of file diff --git a/tools/ExtractObject/main.cpp b/tools/ExtractObject/main.cpp new file mode 100644 index 00000000..a82eb94f --- /dev/null +++ b/tools/ExtractObject/main.cpp @@ -0,0 +1,243 @@ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +void showUsage() +{ + printf("\nUsage: extractObject [options] cloud.pcd\n" + "Options:\n" + " -p #.# plane distance threshold (default 0.02 m)\n" + " -a #.# plane angle tolerance from z axis (default PI/6)\n" + " -c #.# cluster tolerance (default 0.1 m)\n" + " -s # minimum cluster size (default 50 points)\n" + " -center_obj center the objects to their local reference\n" + " -save_plane save the plane inliers to \"extracted_plane.pcd\"\n"); +} + +int main(int argc, char *argv[]) +{ + if(argc < 2) + { + showUsage(); + return -1; + } + + std::string cloudPath = argv[argc-1]; + double planeDistanceThreshold = 0.02f; + double planeEpsAngle = 3.1416/6.0; + double clusterTolerance = 0.1f; + int minClusterSize = 50; + bool centerObject = false; + bool savePlane = false; + + for(int i=1; i::Ptr cloud(new pcl::PointCloud); + pcl::io::loadPCDFile(cloudPath, *cloud); + printf("done! (%d points)\n", (int)cloud->size()); + + // Extract plane + pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers (new pcl::PointIndices); + // Create the segmentation object + pcl::SACSegmentation seg; + // Optional + seg.setOptimizeCoefficients (true); + // Mandatory + seg.setModelType (pcl::SACMODEL_PLANE); + seg.setMethodType (pcl::SAC_RANSAC); + seg.setMaxIterations(1000); + seg.setDistanceThreshold (planeDistanceThreshold); + seg.setAxis(Eigen::Vector3f(0,0,1)); + seg.setEpsAngle (planeEpsAngle); + + seg.setInputCloud (cloud); + seg.segment (*inliers, *coefficients); + + printf("Plane coefficients: %f %f %f %f\n", coefficients->values[0], + coefficients->values[1], + coefficients->values[2], + coefficients->values[3]); + + float a = coefficients->values[0]; + float b = coefficients->values[1]; + float c = coefficients->values[2]; + float d = coefficients->values[3]; + + // Create a quaternion for rotation into XY plane + Eigen::Vector3f current(a, b, c); + Eigen::Vector3f target(0.0, 0.0, 1.0); + Eigen::Quaternion q; + q.setFromTwoVectors(current, target); + Eigen::Matrix4f trans; + trans.topLeftCorner<3,3>() = q.toRotationMatrix(); + + float planeShift = -d; + + // Create transformed point cloud (polygon is aligned with XY plane) + pcl::PointCloud::Ptr output (new pcl::PointCloud ()); + pcl::transformPointCloud(*cloud, *output, Eigen::Vector3f(-a*planeShift, -b*planeShift, -c*planeShift), q); + + if(savePlane) + { + pcl::io::savePCDFile("extracted_plane.pcd", *output, inliers->indices); + printf("Saved extracted_plane.pcd (%d points)\n", (int)inliers->indices.size()); + } + else + { + printf("Plane size = %d points\n", (int)inliers->indices.size()); + } + + // remove plane inliers + pcl::ExtractIndices extract; + extract.setNegative (true); + extract.setInputCloud (output); + extract.setIndices(inliers); + extract.filter (*output); + + // Get the biggest cluster + pcl::search::KdTree::Ptr kdTree(new pcl::search::KdTree); + kdTree->setInputCloud(output); + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance (clusterTolerance); + ec.setMinClusterSize (minClusterSize); + ec.setMaxClusterSize (200000); + ec.setSearchMethod (kdTree); + ec.setInputCloud (output); + ec.extract (cluster_indices); + + if(cluster_indices.size() == 0) + { + printf("No object found! (minimum cluster size=%d)\n", minClusterSize); + return 1; + } + + for(unsigned int i=0; i::Ptr object (new pcl::PointCloud ()); + pcl::copyPointCloud(*output, cluster_indices.at(i), *object); + + if(centerObject) + { + // recenter the object on xy plane, and set min z to 0 + Eigen::Vector4f min, max; + pcl::getMinMax3D(*object, min, max); + + pcl::transformPointCloud(*object, *object, Eigen::Vector3f(-(max[0]+min[0])/2.0f, -(max[1]+min[1])/2.0f, -min[2]), Eigen::Quaternion(0,0,0,0)); + } + + pcl::io::savePCDFile(uFormat("extracted_object%d.pcd", (int)i+1), *object); + printf("Saved extracted_object%d.pcd (%d points)\n", (int)i+1, (int)object->size()); + } + + return 0; +}