00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <core/exception.h>
00025 #include <core/exceptions/system.h>
00026 #include <fvutils/writers/pnm.h>
00027 #include <fvutils/color/conversions.h>
00028
00029 #include <cstdio>
00030 #include <cstdlib>
00031
00032 using namespace fawkes;
00033
00034 namespace firevision {
00035 #if 0
00036 }
00037 #endif
00038
00039
00040
00041
00042
00043
00044
00045
00046 PNMWriter::PNMWriter(PNMFormat format)
00047 : Writer("pnm")
00048 {
00049 this->format = format;
00050
00051 buffer_size = calc_buffer_size();
00052 buffer = (unsigned char *)malloc(buffer_size);
00053 buffer_start = buffer;
00054 }
00055
00056
00057
00058
00059
00060
00061
00062 PNMWriter::PNMWriter(PNMFormat format, const char *filename, unsigned int width, unsigned int height)
00063 : Writer("pnm")
00064 {
00065 set_filename(filename);
00066
00067 this->format = format;
00068 this->width = width;
00069 this->height = height;
00070
00071 buffer_size = calc_buffer_size();
00072 buffer = (unsigned char *)malloc(buffer_size);
00073 buffer_start = buffer;
00074 }
00075
00076
00077 void
00078 PNMWriter::set_buffer(colorspace_t cspace, unsigned char *yuv422_planar_buf)
00079 {
00080 if (cspace != YUV422_PLANAR) {
00081 throw Exception("Unsupported colorspace, PNM can only write YUV422_PLANAR images");
00082 }
00083
00084 buffer = buffer_start;
00085 memset(buffer, 0, buffer_size);
00086
00087 buffer += write_header();
00088
00089 unsigned char *yp, *up, *vp;
00090 unsigned char y1, y2, u, v;
00091
00092 yp = yuv422_planar_buf;
00093 up = YUV422_PLANAR_U_PLANE(yuv422_planar_buf, width, height);
00094 vp = YUV422_PLANAR_V_PLANE(yuv422_planar_buf, width, height);
00095
00096
00097 if ( (format == PNM_PBM) ||
00098 (format == PNM_PBM_ASCII) ) {
00099
00100 unsigned char byte = 0;
00101 unsigned int num_bits = 0;
00102
00103 for (unsigned int i = 0; i < height; ++i) {
00104 byte = 0;
00105 num_bits = 0;
00106 for (unsigned int j = 0; j < width; ++j) {
00107 y1 = *yp++;
00108 if (y1 > 127) {
00109 y2 = 1;
00110 } else {
00111 y2 = 0;
00112 }
00113 if ( format == PNM_PBM ) {
00114 byte |= (y2 << (7-num_bits++));
00115 if (num_bits == 8) {
00116 *buffer++ = byte;
00117 byte = 0;
00118 num_bits = 0;
00119 }
00120 } else {
00121
00122 sprintf((char *)buffer, "%c ", y2);
00123 buffer += 2;
00124 }
00125 }
00126 if ((format == PNM_PBM) && (num_bits != 0)) {
00127 *buffer++ = byte;
00128 }
00129 }
00130 } else if ( (format == PNM_PGM) ||
00131 (format == PNM_PGM_ASCII) ) {
00132
00133 for (unsigned int i = 0; i < height; ++i) {
00134 for (unsigned int j = 0; j < width; ++j) {
00135 y1 = *yp++;
00136 if ( format == PNM_PGM ) {
00137 *buffer++ = y1;
00138 } else {
00139
00140 sprintf((char *)buffer, "%3c ", y1);
00141 buffer += 4;
00142 }
00143 }
00144 }
00145
00146 } else if ( format == PNM_PPM ) {
00147
00148 convert(YUV422_PLANAR, RGB, yuv422_planar_buf, buffer, width, height);
00149
00150 } else if (format == PNM_PPM_ASCII) {
00151
00152 unsigned char r, g, b;
00153
00154 for (unsigned int i = 0; i < height; ++i) {
00155 for (unsigned int j = 0; j < (width / 2); ++j) {
00156 y1 = *yp++;
00157 y2 = *yp++;
00158 u = *up++;
00159 v = *vp++;
00160
00161 pixel_yuv_to_rgb(y1, u, v, &r, &g, &b);
00162 sprintf((char *)buffer, "%3c %3c %3c ", r, g, b);
00163 buffer += 13;
00164
00165 pixel_yuv_to_rgb(y2, u, v, &r, &g, &b);
00166 sprintf((char *)buffer, "%3c %3c %3c ", r, g, b);
00167 buffer += 13;
00168 }
00169 }
00170 }
00171
00172 }
00173
00174
00175 const char *
00176 PNMWriter::format2string(PNMFormat format)
00177 {
00178 switch ( format ) {
00179 case PNM_PBM: return "P4";
00180 case PNM_PBM_ASCII: return "P1";
00181 case PNM_PGM: return "P5";
00182 case PNM_PGM_ASCII: return "P2";
00183 case PNM_PPM: return "P6";
00184 case PNM_PPM_ASCII: return "P3";
00185
00186 default:
00187 throw Exception("Unknown PNMFormat");
00188 }
00189 }
00190
00191 unsigned int
00192 PNMWriter::write_header(bool simulate)
00193 {
00194 unsigned int rv = 25;
00195
00196 if (! simulate) {
00197 switch ( format ) {
00198 case PNM_PBM:
00199 case PNM_PBM_ASCII:
00200 sprintf((char *)buffer, "%s %10u %10u\n", format2string(format), width, height);
00201 break;
00202
00203 case PNM_PGM:
00204 case PNM_PGM_ASCII:
00205 case PNM_PPM:
00206 case PNM_PPM_ASCII:
00207 sprintf((char *)buffer, "%s %10u %10u 255\n", format2string(format), width, height);
00208 break;
00209 default: break;
00210 }
00211 }
00212
00213 switch (format) {
00214 case PNM_PGM:
00215 case PNM_PGM_ASCII:
00216 case PNM_PPM:
00217 case PNM_PPM_ASCII:
00218 rv += 4;
00219 break;
00220 default: break;
00221 }
00222
00223 return rv;
00224 }
00225
00226
00227 void
00228 PNMWriter::write()
00229 {
00230 FILE *fp = fopen(filename, "wb");
00231 if (!fp) {
00232 throw Exception("Could not open file for writing");
00233 }
00234
00235 if (fwrite(buffer_start, buffer_size, 1, fp) != 1) {
00236 throw FileWriteException(filename, "Failed to write data");
00237 }
00238 fclose(fp);
00239
00240 }
00241
00242
00243 unsigned int
00244 PNMWriter::calc_buffer_size()
00245 {
00246 unsigned int rv = write_header(true);
00247
00248 unsigned int num_row_bytes = 0;
00249
00250 switch ( format ) {
00251 case PNM_PBM:
00252
00253 num_row_bytes = width / 8;
00254 if ((width % 8) != 0) {
00255
00256 num_row_bytes += 1;
00257 }
00258 break;
00259
00260 case PNM_PBM_ASCII:
00261
00262 num_row_bytes = 2 * width;
00263 break;
00264
00265 case PNM_PGM:
00266 num_row_bytes = width;
00267 break;
00268
00269 case PNM_PGM_ASCII:
00270 num_row_bytes = width * 4;
00271 break;
00272
00273 case PNM_PPM:
00274 num_row_bytes = 3 * width;
00275 break;
00276
00277 case PNM_PPM_ASCII:
00278
00279
00280
00281
00282
00283
00284 num_row_bytes = width * 13;
00285 break;
00286
00287 default: break;
00288 }
00289
00290 rv += num_row_bytes * height;
00291
00292 return rv;
00293 }
00294
00295 }