diff --git a/README.md b/README.md new file mode 100644 index 0000000..e02fa68 --- /dev/null +++ b/README.md @@ -0,0 +1,23 @@ +# Semantic Object Maps +Fork of the Lucia 2016 Winter School Semantic Object Maps Tutorial. + +## Online Changes +This repository contains an altered LVR Plain Classifier that is able to get its mesh data from ROS messages and is therefore online runnable. +These Messages can be generated by providing a ROS service on /kinfu/mesh_srv which needs to provide a TriangleMeshStamped and a constant GET which routes the request correctly. +The TriangleMeshStamped must be filled at least with the faces and vertices array. + +The LVR KFusion Node provides this service. + +## Compilation +To compile this package you have to have ROS-kinetic, the ROS package mesh_msgs and LVR compiled and installed. +## Execution +To launch the pipeline you have to: +1. Start the furniture classifier node: +``` +cd "YOUR_WORKSPACE"/src/semantic_object_maps/semantic_furniture_classifier/semantic_furniture_classifier_project/ +./build/install/semantic_furniture_classifier_project/bin/semantic_furniture_classifier_project com.github.semantic_furniture_classifier.semantic_furniture_classifier_project.SemanticFurnitureClassifierNode +``` +2. Start the service providing Node +3. Start the lvr_plane_classifier: +`rosrun lvr_plane_classifier lvr_plane_classifier_node` +(This has to be executed in a directory that contains a valid config.xml!) diff --git a/lvr_plane_classifier/CMakeLists.txt b/lvr_plane_classifier/CMakeLists.txt index 30721c4..44dd4ef 100644 --- a/lvr_plane_classifier/CMakeLists.txt +++ b/lvr_plane_classifier/CMakeLists.txt @@ -10,7 +10,8 @@ find_package(catkin REQUIRED COMPONENTS semantic_object_maps_msgs std_msgs tf -) + mesh_msgs + ) ## System dependencies are found with CMake's conventions find_package(LVR REQUIRED) diff --git a/lvr_plane_classifier/optimized_mesh.ply b/lvr_plane_classifier/optimized_mesh.ply new file mode 100644 index 0000000..677b779 Binary files /dev/null and b/lvr_plane_classifier/optimized_mesh.ply differ diff --git a/lvr_plane_classifier/src/LVRClassifierNode.cpp b/lvr_plane_classifier/src/LVRClassifierNode.cpp index 417cb10..3612034 100644 --- a/lvr_plane_classifier/src/LVRClassifierNode.cpp +++ b/lvr_plane_classifier/src/LVRClassifierNode.cpp @@ -12,6 +12,12 @@ #include #include #include +#include +#include +#include +#include +#include +#include using namespace lvr; @@ -89,30 +95,18 @@ void transformModel(ModelPtr& model, const tf::Transform& transform) * * @param filename The name of the loaded model ******************************************************************************/ -tf::Transform getTransfromFromCalibration(std::string filename) +tf::Transform getTransfromFromCalibration() { tf::Transform t; - // Construct name for calibration file - std::string calibrationFileName = ""; - size_t dot = filename.find_last_of("."); - if (dot != std::string::npos) - { - calibrationFileName = filename.substr(0, dot) + ".calibration"; - } - - // Try to load calibration - ifstream in(calibrationFileName.c_str()); - if(!in.good()) - { - ROS_WARN ("Could not open calibration file %s", calibrationFileName.c_str()); - return t; - } float qx, qy, qz, qw, x, y, z; - in >> qx >> qy >> qz >> qw >> x >> y >> z; - - ROS_INFO("Found calibration %s: %f %f %f %f %f %f %f", calibrationFileName.c_str(), qx, qy, qz, qw, x, y, z); - + x = 0.186; + y = 0.8; + z = -0.64; + qx = 1.25; + qy = -0.4; + qz = 0.6; + qw = 1; tf::Vector3 origin(x, y, z); tf::Quaternion quat(qx, qy, qz, qw); t = tf::Transform(quat, origin); @@ -184,15 +178,11 @@ void generateMarkerArray(vector& contour_markers, ve * @param contour_markers A pointer to model data * @param marker A mesh marker ******************************************************************************/ -void createMeshMarker(string filename, ModelPtr model, visualization_msgs::Marker& mesh_marker) +void createMeshMarker(ModelPtr model, visualization_msgs::Marker& mesh_marker) { // Construct name for calibration file - std::string stlFileName = ""; - size_t dot = filename.find_last_of("."); - if (dot != std::string::npos) - { - stlFileName = "/tmp/" + filename.substr(0, dot) + ".stl"; - } + std::string stlFileName = "meshFile"; + stlFileName = "/tmp/meshFile1.stl"; string stlRessource = "file://" + stlFileName; @@ -290,74 +280,109 @@ int main(int argc, char** argv) // and one for the visualization markers ros::init(argc, argv, "lvr_classifier_node"); ros::NodeHandle nh; - + ros::Rate loop_rate(0.2); ros::Publisher plane_publisher = nh.advertise("lvr_classified_planes", 1000); ros::Publisher contour_publisher = nh.advertise("lvr_plane_contours", 1000); ros::Publisher mesh_publisher = nh.advertise("lvr_mesh", 1000); + ros::ServiceClient service = nh.serviceClient("/kinfu/mesh_srv"); + while(ros::ok()) + { + int vertexSize = 0; + mesh_msgs::TriggerMesh srv; + srv.request.request = mesh_msgs::TriggerMesh::Request::GET; + MeshBufferPtr mBuffer(new MeshBuffer); + bool selvic = service.exists(); + ROS_INFO("Checking if service exists... : %d",selvic); + if(service.call(srv)) + { + uintArr faces(new uint[srv.response.mesh.mesh.triangles.size() * 3]); + floatArr vertices(new float[srv.response.mesh.mesh.vertices.size() * 3]); + int facePosition = 0; + int vertexPosition = 0; + int faceSize = srv.response.mesh.mesh.triangles.size(); + vertexSize = srv.response.mesh.mesh.vertices.size(); + for(int i = 0; i < faceSize; i++ ) + { + faces[facePosition++] = (uint) srv.response.mesh.mesh.triangles[i].vertex_indices[0]; + faces[facePosition++] = srv.response.mesh.mesh.triangles[i].vertex_indices[1]; + faces[facePosition++] = srv.response.mesh.mesh.triangles[i].vertex_indices[2]; + } + for(int i = 0; i < vertexSize; i++ ) + { + vertices[vertexPosition++] = srv.response.mesh.mesh.vertices[i].x; + vertices[vertexPosition++] = srv.response.mesh.mesh.vertices[i].y; + vertices[vertexPosition++] = srv.response.mesh.mesh.vertices[i].z; + } + + mBuffer->setFaceArray(faces,(size_t) faceSize); + mBuffer->setVertexArray(vertices, (size_t) vertexSize); + }else + { + ROS_ERROR("NO MESH RECEIVED!"); + return 1; + } - // Load data from given file - string modelFileName(argv[1]); - ModelPtr model = ModelFactory::readModel(modelFileName); - - // Search for a calibration for the given model - tf::Transform t = getTransfromFromCalibration(modelFileName); - - // Transform the model according to the loaded transformation - transformModel(model, t.inverse()); - - // Generate half edge mesh representation of the loaded mesh - HalfEdgeMesh mesh( model->m_mesh ); - - // Get a pointer to he planar regions and create a furniture classifier - vector* regionPtr = mesh.getRegionsPtr(); - FurnitureFeatureClassifier classifier(regionPtr); - mesh.setClassifier(&classifier); - - // Apply mesh optimization filters - mesh.cleanContours(CONTOUR_ITERATIONS); - mesh.setDepth(100); - - mesh.optimizePlanes( - PLANE_ITERATIONS, - NORMAL_THRESHOLD, - MIN_PLANE_SIZE, - SMALL_REGION_THRESHOLD, - true); - - mesh.fillHoles(FILL_HOLES); - mesh.optimizePlaneIntersections(); - mesh.restorePlanes(MIN_PLANE_SIZE); - - // Create a marker for each contour (we to generate them here since mesh.finalize - // will destroy the topology of the initial mesh when re-triangulating the - // contours - mesh.resetUsedFlags(); - vector contourMarkers; - generateMarkerArray(contourMarkers, regionPtr); - - // Finalize and reduce mesh, includes classification - mesh.finalizeAndRetesselate( - false, // Textures not yet supported in this tool - LINE_FUSION_THRESHOLD); - - // Create output model and save to file - ModelPtr out_model( new Model( mesh.meshBuffer() ) ); - ModelFactory::saveModel( out_model, "optimized_mesh.ply"); - - // Generate final messages from classifier and pre-computed markers - semantic_object_maps_msgs::PlanarPatchArray patch_array; - visualization_msgs::MarkerArray markerArray; - generateMessages(classifier, contourMarkers, markerArray, patch_array); - - // Create a mesh marker - visualization_msgs::Marker mesh_marker; - createMeshMarker(modelFileName, model, mesh_marker); - - // Publish stuff - plane_publisher.publish(patch_array); - contour_publisher.publish(markerArray); - mesh_publisher.publish(mesh_marker); - ros::spin(); - + ModelPtr model(new Model(mBuffer)); + // Search for a calibration for the given model + tf::Transform t = getTransfromFromCalibration(); + + // Transform the model according to the loaded transformation + transformModel(model, t.inverse()); + // Generate half edge mesh representation of the loaded mesh + HalfEdgeMesh mesh( model->m_mesh ); + // Get a pointer to he planar regions and create a furniture classifier + vector* regionPtr = mesh.getRegionsPtr(); + FurnitureFeatureClassifier classifier(regionPtr); + mesh.setClassifier(&classifier); + + // Apply mesh optimization filters + mesh.cleanContours(CONTOUR_ITERATIONS); + mesh.setDepth(100); + + mesh.optimizePlanes( + PLANE_ITERATIONS, + NORMAL_THRESHOLD, + MIN_PLANE_SIZE, + SMALL_REGION_THRESHOLD, + true); + + mesh.fillHoles(FILL_HOLES); + mesh.optimizePlaneIntersections(); + mesh.restorePlanes(MIN_PLANE_SIZE); + + // Create a marker for each contour (we to generate them here since mesh.finalize + // will destroy the topology of the initial mesh when re-triangulating the + // contours + mesh.resetUsedFlags(); + vector contourMarkers; + generateMarkerArray(contourMarkers, regionPtr); + + // Finalize and reduce mesh, includes classification + mesh.finalizeAndRetesselate( + false, // Textures not yet supported in this tool + LINE_FUSION_THRESHOLD); + + // Create output model and save to file + ModelPtr out_model( new Model( mesh.meshBuffer() ) ); + ModelFactory::saveModel( out_model, "optimized_mesh.ply"); + + // Generate final messages from classifier and pre-computed markers + semantic_object_maps_msgs::PlanarPatchArray patch_array; + visualization_msgs::MarkerArray markerArray; + generateMessages(classifier, contourMarkers, markerArray, patch_array); + + // Create a mesh marker + visualization_msgs::Marker mesh_marker; + createMeshMarker(model, mesh_marker); + + // Publish stuff + plane_publisher.publish(patch_array); + contour_publisher.publish(markerArray); + mesh_publisher.publish(mesh_marker); + ros::spin(); + //loop_rate.sleep(); + //Maybe-TODO: Broadcast idle + } return 0; } + diff --git a/semantic_furniture_classifier/semantic_furniture_classifier_project/owl/furniture.owl b/semantic_furniture_classifier/semantic_furniture_classifier_project/owl/furniture.owl index d34291d..58b56d3 100644 --- a/semantic_furniture_classifier/semantic_furniture_classifier_project/owl/furniture.owl +++ b/semantic_furniture_classifier/semantic_furniture_classifier_project/owl/furniture.owl @@ -1,922 +1,922 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - TablePlane - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.20 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.40 - - - - - - - - - - - - - - - - - - - - - - 0.60 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - MiddleShelfPlane - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.01 - - - - - - - - - - - - - - - - - - - - - - 0.5 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.48 - - - - - - - - - - - - - - - - - - - - - - 0.64 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - HighShelfPlane - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.01 - - - - - - - - - - - - - - - - - - - - - - 0.5 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.88 - - - - - - - - - - - - - - - - - - - - - - 0.98 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - LowShelfPlane - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.01 - - - - - - - - - - - - - - - - - - - - - - 0.5 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.08 - - - - - - - - - - - - - - - - - - - - - - 0.18 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + TablePlane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.20 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.40 + + + + + + + + + + + + + + + + + + + + + + 0.60 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + MiddleShelfPlane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.01 + + + + + + + + + + + + + + + + + + + + + + 0.5 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.48 + + + + + + + + + + + + + + + + + + + + + + 0.64 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + HighShelfPlane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.01 + + + + + + + + + + + + + + + + + + + + + + 0.5 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.88 + + + + + + + + + + + + + + + + + + + + + + 0.98 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + LowShelfPlane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.01 + + + + + + + + + + + + + + + + + + + + + + 0.5 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.08 + + + + + + + + + + + + + + + + + + + + + + 0.18 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +