bool PBImage::writePBI(string fileName, const YUVImage& image, int pan, int tilt) {
protobuf::ProtoBufFrame frame;
protobuf::ProtoBufFrame_Camera* camera = frame.add_camera();
protobuf::ProtoBufFrame_Camera_CameraType* camera_type =
camera->mutable_type();
camera_type->set_location(protobuf::HEAD_LEFT);
camera_type->set_name("unknown");
camera_type->set_sensor_size_width(1600.0 * 0.0022);
camera_type->set_sensor_size_height(1200.0 * 0.0022);
protobuf::Vector* camera_position = camera->mutable_camera_position();
camera_position->set_x(pan);
camera_position->set_y(tilt);
camera_position->set_z(0);
protobuf::ImageData* imageData = camera->mutable_image_data();
imageData->set_format(protobuf::YUV422_IMAGE);
imageData->set_data(image.getImage(),
(size_t) (uint32_t) (image.getHeight() * image.getWidth() * 2));
imageData->set_compressed(false);
imageData->set_width(image.getWidth());
imageData->set_height(image.getHeight());
fstream output(fileName.c_str(), ios::out | ios::trunc | ios::binary);
if (!output.good()) {
Debugger::ERR("PBImage", "Could not open output stream");
}
if (!frame.IsInitialized()) {
Debugger::ERR("PBImage", "ProtoBuf is not initialized!");
string missing = frame.InitializationErrorString();
if (missing != "") {
Debugger::ERR("PBImage", "Missing field in annotation: %s",
missing.c_str());
}
}
size_t datasize = (size_t) (uint32_t) (frame.ByteSize());
char* data = (char*) malloc(datasize);
if (!frame.SerializeToArray(data, (int) datasize)) {
Debugger::ERR("PBImage", "Failed to write PBI!");
free(data);
output.flush();
output.close();
return false;
}
output.write(data, (streamsize) datasize);
free(data);
output.flush();
output.close();
Debugger::INFO("PBImage", "Successfully wrote PBI %s!", fileName.c_str());
return true;
}
void Serializer<YUVImage>::serialize(const YUVImage& representation,
ostream& stream) {
if (representation.getImage() != NULL) {
protobuf::YUVImage img;
img.set_height(representation.getHeight());
img.set_width(representation.getWidth());
img.set_data(representation.getImage(),
(size_t) (uint32_t) (representation.getWidth()
* representation.getHeight() * SIZE_OF_YUV422_PIXEL));
OstreamOutputStream buf(&stream);
img.SerializeToZeroCopyStream(&buf);
}
}
size_t MJPEGStreamer::compress_yuyv_to_jpeg(const YUVImage& src, uint8_t* buffer, size_t size, int quality) const {
struct jpeg_compress_struct cinfo;
struct jpeg_error_mgr jerr;
JSAMPROW row_pointer[1];
uint8_t *line_buffer;
const uint8_t *yuyv;
int z;
static size_t written;
line_buffer = (uint8_t*)calloc((size_t)(uint32_t)(src.getWidth() * 3), (size_t)1);
if (line_buffer == NULL) {
return 0;
}
yuyv = (const uint8_t*)src.getImage();
cinfo.err = jpeg_std_error (&jerr);
jpeg_create_compress (&cinfo);
/* jpeg_stdio_dest (&cinfo, file); */
dest_buffer(&cinfo, buffer, size, &written);
cinfo.image_width = (uint32_t)src.getWidth();
cinfo.image_height = (uint32_t)src.getHeight();
cinfo.input_components = 3;
cinfo.in_color_space = JCS_RGB;
jpeg_set_defaults (&cinfo);
jpeg_set_quality (&cinfo, quality, TRUE);
jpeg_start_compress (&cinfo, TRUE);
z = 0;
while (cinfo.next_scanline < (uint32_t)src.getHeight()) {
int x;
uint8_t *ptr = line_buffer;
for (x = 0; x < src.getWidth(); x++) {
int r, g, b;
int y, u, v;
if (!z)
y = yuyv[0] << 8;
else
y = yuyv[2] << 8;
u = yuyv[1] - 128;
v = yuyv[3] - 128;
//lint -e{702}
r = (y + (359 * v)) >> 8;
//lint -e{702}
g = ((y - (88 * u)) - (183 * v)) >> 8;
//lint -e{702}
b = (y + (454 * u)) >> 8;
*(ptr++) = (uint8_t)((r > 255) ? 255 : ((r < 0) ? 0 : r));
*(ptr++) = (uint8_t)((g > 255) ? 255 : ((g < 0) ? 0 : g));
*(ptr++) = (uint8_t)((b > 255) ? 255 : ((b < 0) ? 0 : b));
if (z++) {
z = 0;
yuyv += 4;
}
}
row_pointer[0] = line_buffer;
jpeg_write_scanlines (&cinfo, row_pointer, 1);
}
jpeg_finish_compress (&cinfo);
jpeg_destroy_compress (&cinfo);
free (line_buffer);
return (written);
}
