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 <filters/rectify.h>
00025
00026 #include <core/exceptions/software.h>
00027
00028 #include <fvutils/rectification/rectinfo_lut_block.h>
00029 #include <fvutils/rectification/rectinfo_block.h>
00030 #include <fvutils/color/yuv.h>
00031 #include <cstddef>
00032
00033 #include <cstdio>
00034
00035 namespace firevision {
00036 #if 0
00037 }
00038 #endif
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055 FilterRectify::FilterRectify(RectificationInfoBlock *rib, bool mark_zeros)
00056 : Filter("FilterRectify")
00057 {
00058 __rib = rib;
00059 __mark_zeros = mark_zeros;
00060 }
00061
00062
00063 #define FILTER_RECTIFY_ADVANCE_LINE \
00064 ldyp += dst_roi->line_step; \
00065 ldup += dst_roi->line_step / 2; \
00066 ldvp += dst_roi->line_step / 2; \
00067 dyp = ldyp; \
00068 dup = ldup; \
00069 dvp = ldvp;
00070
00071 #define FILTER_RECTIFY_ASSIGN \
00072 *dyp++ = py1; \
00073 *dyp++ = py2; \
00074 *dup++ = (pu1 + pu2) / 2; \
00075 *dvp++ = (pv1 + pv2) / 2; \
00076
00077
00078 void
00079 FilterRectify::apply()
00080 {
00081
00082
00083 register unsigned char *dyp = dst + (dst_roi->start.y * dst_roi->line_step) + (dst_roi->start.x * dst_roi->pixel_step);
00084
00085
00086 register unsigned char *dup = YUV422_PLANAR_U_PLANE(dst, dst_roi->image_width, dst_roi->image_height)
00087 + ((dst_roi->start.y * dst_roi->line_step) / 2 + (dst_roi->start.x * dst_roi->pixel_step) / 2) ;
00088
00089 register unsigned char *dvp = YUV422_PLANAR_V_PLANE(dst, dst_roi->image_width, dst_roi->image_height)
00090 + ((dst_roi->start.y * dst_roi->line_step) / 2 + (dst_roi->start.x * dst_roi->pixel_step) / 2);
00091
00092
00093 unsigned char *ldyp = dyp;
00094 unsigned char *ldup = dup;
00095 unsigned char *ldvp = dvp;
00096
00097 unsigned char py1=0, py2=0, pu1=0, pu2=0, pv1=0, pv2=0;
00098
00099 RectificationLutInfoBlock *rlib = dynamic_cast<RectificationLutInfoBlock *>(__rib);
00100
00101 if ( rlib ) {
00102 if ( (rlib->pixel_width() != dst_roi->image_width) ||
00103 (rlib->pixel_height() != dst_roi->image_height) ) {
00104 throw fawkes::IllegalArgumentException("Rectification LUT and image sizes do not match");
00105 }
00106
00107
00108 rectinfo_lut_16x16_entry_t *lut = rlib->lut_data() +
00109 dst_roi->start.y * rlib->pixel_width() +
00110 dst_roi->start.x;
00111
00112 rectinfo_lut_16x16_entry_t *llut = lut;
00113
00114 if ( __mark_zeros ) {
00115 for (unsigned int h = 0; h < dst_roi->height; ++h) {
00116 for (unsigned int w = 0; w < dst_roi->width; w += 2) {
00117 if ( lut->x == 0 && lut->y == 0 ) {
00118 py1 = YUV422_PLANAR_Y_AT(src[0], src_roi[0]->image_width, w, h);
00119 pu1 = 0;
00120 pv1 = 255;
00121 } else {
00122 YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
00123 lut->x, lut->y, py1, pu1, pv1);
00124 }
00125 ++lut;
00126
00127 if ( lut->x == 0 && lut->y == 0 ) {
00128 py2 = YUV422_PLANAR_Y_AT(src[0], src_roi[0]->image_width, w, h);
00129 pu2 = 0;
00130 pv2 = 255;
00131 } else {
00132 YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
00133 lut->x, lut->y, py2, pu2, pv2);
00134 }
00135 ++lut;
00136
00137 FILTER_RECTIFY_ASSIGN;
00138 }
00139
00140 FILTER_RECTIFY_ADVANCE_LINE;
00141 llut += rlib->pixel_width();
00142 lut = llut;
00143 }
00144 } else {
00145 for (unsigned int h = 0; h < dst_roi->height; ++h) {
00146 for (unsigned int w = 0; w < dst_roi->width; w += 2) {
00147 YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
00148 lut->x, lut->y, py1, pu1, pv1);
00149 ++lut;
00150 YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
00151 lut->x, lut->y, py2, pu2, pv2);
00152 ++lut;
00153
00154 FILTER_RECTIFY_ASSIGN;
00155 }
00156
00157 FILTER_RECTIFY_ADVANCE_LINE;
00158 llut += rlib->pixel_width();
00159 lut = llut;
00160 }
00161 }
00162 } else {
00163
00164 printf("Unknown info block\n");
00165
00166 uint16_t ur1_x = 0, ur1_y = 0,
00167 ur2_x = 0, ur2_y = 0;
00168
00169 if (__mark_zeros) {
00170 for (unsigned int h = 0; h < dst_roi->height; ++h) {
00171 for (unsigned int w = 0; w < dst_roi->width; w += 2) {
00172 __rib->mapping(w, h, &ur1_x, &ur1_y);
00173 __rib->mapping(w+1, h, &ur2_x, &ur2_y);
00174
00175 if ( (ur1_x == 0) && (ur1_y == 0) ) {
00176 py1 = YUV422_PLANAR_Y_AT(src[0], src_roi[0]->image_width, w, h);
00177 pu1 = 0;
00178 pv1 = 255;
00179 } else {
00180 YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
00181 ur1_x, ur1_y, py1, pu1, pv1);
00182 }
00183 if ( (ur2_x == 0) && (ur2_y == 0) ) {
00184 py2 = YUV422_PLANAR_Y_AT(src[0], src_roi[0]->image_width, w+1, h);
00185 pu2 = 0;
00186 pv2 = 255;
00187 } else {
00188 YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
00189 ur2_x, ur2_y, py2, pu2, pv2);
00190 }
00191
00192 FILTER_RECTIFY_ASSIGN;
00193 }
00194
00195 FILTER_RECTIFY_ADVANCE_LINE;
00196 }
00197 } else {
00198 for (unsigned int h = 0; h < dst_roi->height; ++h) {
00199 for (unsigned int w = 0; w < dst_roi->width; w += 2) {
00200 __rib->mapping(w, h, &ur1_x, &ur1_y);
00201 __rib->mapping(w+1, h, &ur2_x, &ur2_y);
00202
00203 YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
00204 ur1_x, ur1_y, py1, pu1, pv1);
00205 YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
00206 ur2_x, ur2_y, py2, pu2, pv2);
00207
00208 FILTER_RECTIFY_ASSIGN;
00209 }
00210
00211 FILTER_RECTIFY_ADVANCE_LINE;
00212 }
00213 }
00214
00215 }
00216 }
00217
00218 }