void SurfaceReconstruction::operator()(const std::vector<cv::Vec6f>& triangles, const cv::Mat& depth, pcl::RangeImagePlanar::Ptr cloud) { mesh_builder_.reset(new MeshBuilder(cloud)); mesh2d = cv::Mat(depth.size(), CV_8UC1); mesh2d.setTo(0); for (const auto& t : triangles) { std::vector<cv::Point> pt(3); for (int j = 0; j < 3; ++j) pt[j] = cv::Point(t[2 * j], t[2 * j + 1]); int n_inner = 0; std::vector<bool> inner(3); for (int j = 0; j < 3; ++j) if (::is_inner(pt[j], depth) == true) { ++n_inner; inner[j] = true; } if (n_inner == 0) continue; else if (n_inner == 1) { if (inner[1] == true) { std::swap(pt[0], pt[1]); std::swap(pt[1], pt[2]); } else if (inner[2] == true) { std::swap(pt[0], pt[2]); std::swap(pt[1], pt[2]); } extend_point(pt, depth); mesh_builder_->insert(pt); //draw_triangle(pt, mesh2d); } else if (n_inner == 2) { if (inner[0] == false) { std::swap(pt[2], pt[0]); std::swap(pt[0], pt[1]); } else if (inner[1] == false) { std::swap(pt[2], pt[1]); std::swap(pt[0], pt[1]); } std::vector<cv::Point> pt2(3); if (::is_joinable(pt[0], pt[1], depth)) { extend_edge(pt, pt2, depth); } else { std::rotate_copy(pt.begin(), pt.begin() + 1, pt.end(), pt2.begin()); extend_point(pt, depth); extend_point(pt2, depth); } mesh_builder_->insert(pt); //draw_triangle(pt, mesh2d); mesh_builder_->insert(pt2); //draw_triangle(pt2, mesh2d); } else { const bool ab = ::is_joinable(pt[0], pt[1], depth); const bool bc = ::is_joinable(pt[1], pt[2], depth); const bool ac = ::is_joinable(pt[0], pt[2], depth); if ((ab && bc) || (ab && ac) || (bc && ac)) { mesh_builder_->insert(pt); //draw_triangle(pt, mesh2d); continue; } std::vector<cv::Point> pt2(3), pt3(3); if (ab) { std::rotate_copy(pt.begin(), pt.begin() + 2, pt.end(), pt2.begin()); extend_point(pt2, depth); extend_edge(pt, pt3, depth); } else if (bc) { std::rotate_copy(pt.begin(), pt.begin() + 1, pt.end(), pt2.begin()); extend_point(pt, depth); extend_edge(pt2, pt3, depth); } else if (ac) { std::rotate_copy(pt.begin(), pt.begin() + 1, pt.end(), pt2.begin()); extend_point(pt2, depth); std::rotate(pt.begin(), pt.begin() + 2, pt.end()); extend_edge(pt, pt3, depth); } else { std::rotate_copy(pt.begin(), pt.begin() + 1, pt.end(), pt2.begin()); extend_point(pt2, depth); std::rotate_copy(pt.begin(), pt.begin() + 2, pt.end(), pt3.begin()); extend_point(pt3, depth); extend_point(pt, depth); } mesh_builder_->insert(pt); //draw_triangle(pt, mesh2d); mesh_builder_->insert(pt2); //draw_triangle(pt2, mesh2d); mesh_builder_->insert(pt3); //draw_triangle(pt3, mesh2d); } } //cv::imshow("2D Mesh", mesh2d); }
int main(int argc, char *argv[]) { long quality; const char *size; char *x; int luma_width; int luma_height; int chroma_width; int chroma_height; int frame_width; int frame_height; const char *yuv_path; const char *jpg_path; FILE *yuv_fd; size_t yuv_size; unsigned char *yuv_buffer; JSAMPLE *image_buffer; struct jpeg_compress_struct cinfo; struct jpeg_error_mgr jerr; FILE *jpg_fd; JSAMPROW yrow_pointer[16]; JSAMPROW cbrow_pointer[8]; JSAMPROW crrow_pointer[8]; JSAMPROW *plane_pointer[3]; int y; if (argc != 5) { fprintf(stderr, "Required arguments:\n"); fprintf(stderr, "1. JPEG quality value, 0-100\n"); fprintf(stderr, "2. Image size (e.g. '512x512')\n"); fprintf(stderr, "3. Path to YUV input file\n"); fprintf(stderr, "4. Path to JPG output file\n"); return 1; } errno = 0; quality = strtol(argv[1], NULL, 10); if (errno != 0 || quality < 0 || quality > 100) { fprintf(stderr, "Invalid JPEG quality value!\n"); return 1; } size = argv[2]; x = strchr(size, 'x'); if (!x && x != size && x != (x + strlen(x) - 1)) { fprintf(stderr, "Invalid image size input!\n"); return 1; } luma_width = (int)strtol(size, NULL, 10); if (errno != 0) { fprintf(stderr, "Invalid image size input!\n"); return 1; } luma_height = (int)strtol(x + 1, NULL, 10); if (errno != 0) { fprintf(stderr, "Invalid image size input!\n"); return 1; } if (luma_width <= 0 || luma_height <= 0) { fprintf(stderr, "Invalid image size input!\n"); return 1; } chroma_width = (luma_width + 1) >> 1; chroma_height = (luma_height + 1) >> 1; /* Will check these for validity when opening via 'fopen'. */ yuv_path = argv[3]; jpg_path = argv[4]; yuv_fd = fopen(yuv_path, "r"); if (!yuv_fd) { fprintf(stderr, "Invalid path to YUV file!\n"); return 1; } fseek(yuv_fd, 0, SEEK_END); yuv_size = ftell(yuv_fd); fseek(yuv_fd, 0, SEEK_SET); /* Check that the file size matches 4:2:0 yuv. */ if (yuv_size != (size_t)luma_width*luma_height + 2*chroma_width*chroma_height) { fprintf(stderr, "Unexpected input format!\n"); return 1; } yuv_buffer = malloc(yuv_size); if (!yuv_buffer) { fprintf(stderr, "Memory allocation failure!\n"); return 1; } if (fread(yuv_buffer, yuv_size, 1, yuv_fd) != 1) { fprintf(stderr, "Error reading yuv file\n"); }; fclose(yuv_fd); frame_width = (luma_width + (16 - 1)) & ~(16 - 1); frame_height = (luma_height + (16 - 1)) & ~(16 - 1); image_buffer = malloc(frame_width*frame_height + 2*(frame_width/2)*(frame_height/2)); if (!image_buffer) { fprintf(stderr, "Memory allocation failure!\n"); return 1; } extend_edge(image_buffer, frame_width, frame_height, yuv_buffer, luma_width, luma_height, chroma_width, chroma_height); free(yuv_buffer); cinfo.err = jpeg_std_error(&jerr); jpeg_create_compress(&cinfo); jpg_fd = fopen(jpg_path, "wb"); if (!jpg_fd) { fprintf(stderr, "Invalid path to JPEG file!\n"); free(image_buffer); return 1; } jpeg_stdio_dest(&cinfo, jpg_fd); cinfo.image_width = luma_width; cinfo.image_height = luma_height; cinfo.input_components = 3; cinfo.in_color_space = JCS_YCbCr; jpeg_set_defaults(&cinfo); cinfo.raw_data_in = TRUE; cinfo.comp_info[0].h_samp_factor = 2; cinfo.comp_info[0].v_samp_factor = 2; cinfo.comp_info[0].dc_tbl_no = 0; cinfo.comp_info[0].ac_tbl_no = 0; cinfo.comp_info[0].quant_tbl_no = 0; cinfo.comp_info[1].h_samp_factor = 1; cinfo.comp_info[1].v_samp_factor = 1; cinfo.comp_info[1].dc_tbl_no = 1; cinfo.comp_info[1].ac_tbl_no = 1; cinfo.comp_info[1].quant_tbl_no = 1; cinfo.comp_info[2].h_samp_factor = 1; cinfo.comp_info[2].v_samp_factor = 1; cinfo.comp_info[2].dc_tbl_no = 1; cinfo.comp_info[2].ac_tbl_no = 1; cinfo.comp_info[2].quant_tbl_no = 1; jpeg_set_quality(&cinfo, quality, TRUE); cinfo.optimize_coding = TRUE; jpeg_start_compress(&cinfo, TRUE); plane_pointer[0] = yrow_pointer; plane_pointer[1] = cbrow_pointer; plane_pointer[2] = crrow_pointer; while (cinfo.next_scanline < cinfo.image_height) { int scanline; scanline = cinfo.next_scanline; for (y = 0; y < 16; y++) { yrow_pointer[y] = &image_buffer[frame_width*(scanline + y)]; } for (y = 0; y < 8; y++) { cbrow_pointer[y] = &image_buffer[frame_width*frame_height + (frame_width/2)*((scanline/2) + y)]; crrow_pointer[y] = &image_buffer[frame_width*frame_height + (frame_width/2)*(frame_height/2) + (frame_width/2)*((scanline/2) + y)]; } jpeg_write_raw_data(&cinfo, plane_pointer, 16); } jpeg_finish_compress(&cinfo); jpeg_destroy_compress(&cinfo); free(image_buffer); fclose(jpg_fd); return 0; }