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;
}
