Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * rectify.cpp - Implementation of recification filter 00004 * 00005 * Created: Wed Nov 07 10:51:45 2007 00006 * Copyright 2007 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 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 /* just to make Emacs auto-indent happy */ 00037 } 00038 #endif 00039 00040 /** @class FilterRectify <filters/rectify.h> 00041 * Rectify image. 00042 * This filter can be used to use a rectification information block to rectify 00043 * the given image. It has special support for RectificationLutInfoBlocks by using the 00044 * raw data pointer for fast access. For other info blocks it will simply use the 00045 * RectificationInfoBlock::mapping() method to get the information. 00046 * @author Tim Niemueller 00047 */ 00048 00049 /** Constructor. 00050 * @param rib Rectification Information Block 00051 * @param mark_zeros if set to true mappings in the rectification info block that point 00052 * to (0, 0) are marked with red color (luminance value unchanged). This allows for easy 00053 * spotting of dead regions and may explain images that look broken. Enabled by default. 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 // destination y-plane 00083 register unsigned char *dyp = dst + (dst_roi->start.y * dst_roi->line_step) + (dst_roi->start.x * dst_roi->pixel_step); 00084 00085 // destination u-plane 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 // v-plane 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 // line starts 00093 unsigned char *ldyp = dyp; // destination y-plane 00094 unsigned char *ldup = dup; // u-plane 00095 unsigned char *ldvp = dvp; // v-plane 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 // we have an rectification LUT info block 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 } // end namespace firevision