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