int main(int argc, char** argv){ // Obtener argumentos: Archivo pcd y opcionalmente, angle_threshold if (argc < 2){ printf("Error: Debe especificar un archivo .pcd\n"); exit(1); } if (argc == 3){ PATCH_ANGLE_THRESHOLD = atof(argv[2]); } // Crear visualizador Viewer viewer = Viewer(BG_COLOR[0], BG_COLOR[1], BG_COLOR[2], 0.1); // Añadir texto de threshold stringstream thres_s; thres_s << "Threshold: " << PATCH_ANGLE_THRESHOLD; viewer.addText(thres_s.str(), "Threshold label", 1, 1, 1); // Cargar nube de puntos y visualizar PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>()); io::loadPCDFile(argv[1], *cloud); // viewer.drawPointCloud(cloud, "Object PointCloud", POINTCLOUD_COLOR[0], POINTCLOUD_COLOR[1], POINTCLOUD_COLOR[2]); // Separar objeto del gripper initBoxes(); PointCloud<PointXYZ>::Ptr gripper_pc (new PointCloud<PointXYZ>()), object_pc(new PointCloud<PointXYZ>()); isolateObject(cloud, gripper_pc, object_pc); viewer.drawPointCloud(gripper_pc, "gripper_pc", GRIPPER_COLOR[0], GRIPPER_COLOR[1], GRIPPER_COLOR[2],3); viewer.drawPointCloud(object_pc, "object_pc", OBJECT_COLOR[0], OBJECT_COLOR[1], OBJECT_COLOR[2],3); /*// Dibujar cajas for (int i=0; i<gripper_boxes.size(); i++){ stringstream box_name; box_name << "box_" << i; Box thisbox = gripper_boxes[i]; float min_x = thisbox.center[0] - thisbox.size[0]/2.0; float max_x = thisbox.center[0] + thisbox.size[0]/2.0; float min_y = thisbox.center[1] - thisbox.size[1]/2.0; float max_y = thisbox.center[1] + thisbox.size[1]/2.0; float min_z = thisbox.center[2] - thisbox.size[2]/2.0; float max_z = thisbox.center[2] + thisbox.size[2]/2.0; viewer.drawBox(box_name.str(), min_x, max_x, min_y, max_y, min_z, max_z, 1, 1, 1, 0.5); }*/ // Crear convex hull del objeto y visualizar Polymesh mesh = Polymesh(Util::getConvexHull(object_pc)); viewer.drawPolygonMesh(mesh.getPCLMesh(), "Object ConvexHull", CONVEXHULL_COLOR[0], CONVEXHULL_COLOR[1], CONVEXHULL_COLOR[2], CONVEXHULL_ALPHA); // Visualizar normales //viewer.drawPolygonMeshNormals(mesh, "ConvexHull Normals", NORMALS_COLOR[0], NORMALS_COLOR[1], NORMALS_COLOR[2]); // Obtener listado de parches y de sus respectivas áreas vector<vector<int> > patches; // todos los parches posibles, ORDENADOS desde el más grande al más pequeño vector<double> patches_areas; // sus areas correspondientes, en el orden adecuado mesh.getFlatPatches(PATCH_ANGLE_THRESHOLD, patches, patches_areas); // Obtener centro de masa PointXYZ cm = mesh.getCenterOfMass(); // Iterar hasta encontrar un parche estable. Priorizar parches grandes vector<int> best_patch; double best_patch_area; PointCloud<PointXYZ>::Ptr patch_plane(new PointCloud<PointXYZ>()); ModelCoefficients::Ptr patch_plane_coefs(new ModelCoefficients()); PointXYZ cm_proj; bool plane_found = false; for (int i=0; i<patches.size(); i++){ // Obtener plano representado por el parche mesh.flattenPatch(patches[i], *patch_plane, patch_plane_coefs); // proyectar centro de masa sobre plano cm_proj = Polymesh::projectPointOverFlatPointCloud(cm, patch_plane); // VERIFICACIÓN DE CONDICIONES: // - Es un plano estable? // * centro de masa se proyecta sobre parche? // - Puede el gripper llegar a esa posición? // * El gripper no es cortado por el plano de la superficie? // Dibujar plano if (Polymesh::isPointInConvexPolygon(cm_proj, *patch_plane) and not isPointCloudCutByPlane(gripper_pc, patch_plane_coefs, patch_plane->points[0])){ best_patch = patches[i]; best_patch_area = patches_areas[i]; printf("Se ha encontrado un plano estable (de area %.2f)\n", best_patch_area); viewer.addText("Gripper no cortado por el plano", "posicion_estable_label", 0, 1, 0); plane_found = true; viewer.viewer_->addPlane(*patch_plane_coefs, "Plano qlo", 0); break; } } if (not plane_found){ printf("No se ha podido encontrar un plano estable. Qué situación más rara!!\n"); exit(0); } viewer.drawPolygonVector(best_patch, mesh.getPCLMesh(), "flat_patch", FLAT_SURFACE_COLOR[0], FLAT_SURFACE_COLOR[1], FLAT_SURFACE_COLOR[2], FLAT_SURFACE_WIRE_COLOR[0], FLAT_SURFACE_WIRE_COLOR[1], FLAT_SURFACE_WIRE_COLOR[2], FLAT_SURFACE_WIRE_WIDTH, FLAT_SURFACE_ALPHA); viewer.drawPointCloud(patch_plane, "Flattened Patch", 0, 1, 0, 10); viewer.drawPoint(cm, mesh.getPointCloud(), "Center of Mass", CM_COLOR[0], CM_COLOR[1], CM_COLOR[2]); viewer.drawPoint(cm_proj, mesh.getPointCloud(), "Center of Mass projected", CM_COLOR[0]+(1-CM_COLOR[0])*LIGHT_FACTOR, CM_COLOR[1]+(1-CM_COLOR[1])*LIGHT_FACTOR, CM_COLOR[2]+(1-CM_COLOR[2])*LIGHT_FACTOR); viewer.addText("Centro de Masa proyectado", "centro_de_masa_proy_label", CM_COLOR[0]+(1-CM_COLOR[0])*LIGHT_FACTOR, CM_COLOR[1]+(1-CM_COLOR[1])*LIGHT_FACTOR, CM_COLOR[2]+(1-CM_COLOR[2])*LIGHT_FACTOR); viewer.addText("Centro de Masa", "centro_de_masa_label", CM_COLOR[0], CM_COLOR[1], CM_COLOR[2]); viewer.show(); return 0; }