C++ Viewer::addText方法代码示例

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