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