Fawkes API  Fawkes Development Version
rectify.cpp
1 
2 /***************************************************************************
3  * rectify.cpp - Implementation of recification filter
4  *
5  * Created: Wed Nov 07 10:51:45 2007
6  * Copyright 2007 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 #include <fvfilters/rectify.h>
25 
26 #include <core/exceptions/software.h>
27 
28 #include <fvutils/rectification/rectinfo_lut_block.h>
29 #include <fvutils/rectification/rectinfo_block.h>
30 #include <fvutils/color/yuv.h>
31 #include <cstddef>
32 
33 #include <cstdio>
34 
35 namespace firevision {
36 #if 0 /* just to make Emacs auto-indent happy */
37 }
38 #endif
39 
40 /** @class FilterRectify <fvfilters/rectify.h>
41  * Rectify image.
42  * This filter can be used to use a rectification information block to rectify
43  * the given image. It has special support for RectificationLutInfoBlocks by using the
44  * raw data pointer for fast access. For other info blocks it will simply use the
45  * RectificationInfoBlock::mapping() method to get the information.
46  * @author Tim Niemueller
47  */
48 
49 /** Constructor.
50  * @param rib Rectification Information Block
51  * @param mark_zeros if set to true mappings in the rectification info block that point
52  * to (0, 0) are marked with red color (luminance value unchanged). This allows for easy
53  * spotting of dead regions and may explain images that look broken. Enabled by default.
54  */
56  : Filter("FilterRectify")
57 {
58  __rib = rib;
59  __mark_zeros = mark_zeros;
60 }
61 
62 
63 #define FILTER_RECTIFY_ADVANCE_LINE \
64  ldyp += dst_roi->line_step; \
65  ldup += dst_roi->line_step / 2; \
66  ldvp += dst_roi->line_step / 2; \
67  dyp = ldyp; \
68  dup = ldup; \
69  dvp = ldvp;
70 
71 #define FILTER_RECTIFY_ASSIGN \
72  *dyp++ = py1; \
73  *dyp++ = py2; \
74  *dup++ = (pu1 + pu2) / 2; \
75  *dvp++ = (pv1 + pv2) / 2; \
76 
77 
78 void
80 {
81 
82  // destination y-plane
83  register unsigned char *dyp = dst + (dst_roi->start.y * dst_roi->line_step) + (dst_roi->start.x * dst_roi->pixel_step);
84 
85  // destination u-plane
86  register unsigned char *dup = YUV422_PLANAR_U_PLANE(dst, dst_roi->image_width, dst_roi->image_height)
87  + ((dst_roi->start.y * dst_roi->line_step) / 2 + (dst_roi->start.x * dst_roi->pixel_step) / 2) ;
88  // v-plane
89  register unsigned char *dvp = YUV422_PLANAR_V_PLANE(dst, dst_roi->image_width, dst_roi->image_height)
90  + ((dst_roi->start.y * dst_roi->line_step) / 2 + (dst_roi->start.x * dst_roi->pixel_step) / 2);
91 
92  // line starts
93  unsigned char *ldyp = dyp; // destination y-plane
94  unsigned char *ldup = dup; // u-plane
95  unsigned char *ldvp = dvp; // v-plane
96 
97  unsigned char py1=0, py2=0, pu1=0, pu2=0, pv1=0, pv2=0;
98 
99  RectificationLutInfoBlock *rlib = dynamic_cast<RectificationLutInfoBlock *>(__rib);
100 
101  if ( rlib ) {
102  if ( (rlib->pixel_width() != dst_roi->image_width) ||
103  (rlib->pixel_height() != dst_roi->image_height) ) {
104  throw fawkes::IllegalArgumentException("Rectification LUT and image sizes do not match");
105  }
106 
107  // we have an rectification LUT info block
108  rectinfo_lut_16x16_entry_t *lut = rlib->lut_data() +
109  dst_roi->start.y * rlib->pixel_width() +
110  dst_roi->start.x;
111 
112  rectinfo_lut_16x16_entry_t *llut = lut;
113 
114  if ( __mark_zeros ) {
115  for (unsigned int h = 0; h < dst_roi->height; ++h) {
116  for (unsigned int w = 0; w < dst_roi->width; w += 2) {
117  if ( lut->x == 0 && lut->y == 0 ) {
118  py1 = YUV422_PLANAR_Y_AT(src[0], src_roi[0]->image_width, w, h);
119  pu1 = 0;
120  pv1 = 255;
121  } else {
122  YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
123  lut->x, lut->y, py1, pu1, pv1);
124  }
125  ++lut;
126 
127  if ( lut->x == 0 && lut->y == 0 ) {
128  py2 = YUV422_PLANAR_Y_AT(src[0], src_roi[0]->image_width, w, h);
129  pu2 = 0;
130  pv2 = 255;
131  } else {
132  YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
133  lut->x, lut->y, py2, pu2, pv2);
134  }
135  ++lut;
136 
137  FILTER_RECTIFY_ASSIGN;
138  }
139 
140  FILTER_RECTIFY_ADVANCE_LINE;
141  llut += rlib->pixel_width();
142  lut = llut;
143  }
144  } else {
145  for (unsigned int h = 0; h < dst_roi->height; ++h) {
146  for (unsigned int w = 0; w < dst_roi->width; w += 2) {
147  YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
148  lut->x, lut->y, py1, pu1, pv1);
149  ++lut;
150  YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
151  lut->x, lut->y, py2, pu2, pv2);
152  ++lut;
153 
154  FILTER_RECTIFY_ASSIGN;
155  }
156 
157  FILTER_RECTIFY_ADVANCE_LINE;
158  llut += rlib->pixel_width();
159  lut = llut;
160  }
161  }
162  } else {
163 
164  printf("Unknown info block\n");
165 
166  uint16_t ur1_x = 0, ur1_y = 0,
167  ur2_x = 0, ur2_y = 0;
168 
169  if (__mark_zeros) {
170  for (unsigned int h = 0; h < dst_roi->height; ++h) {
171  for (unsigned int w = 0; w < dst_roi->width; w += 2) {
172  __rib->mapping(w, h, &ur1_x, &ur1_y);
173  __rib->mapping(w+1, h, &ur2_x, &ur2_y);
174 
175  if ( (ur1_x == 0) && (ur1_y == 0) ) {
176  py1 = YUV422_PLANAR_Y_AT(src[0], src_roi[0]->image_width, w, h);
177  pu1 = 0;
178  pv1 = 255;
179  } else {
180  YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
181  ur1_x, ur1_y, py1, pu1, pv1);
182  }
183  if ( (ur2_x == 0) && (ur2_y == 0) ) {
184  py2 = YUV422_PLANAR_Y_AT(src[0], src_roi[0]->image_width, w+1, h);
185  pu2 = 0;
186  pv2 = 255;
187  } else {
188  YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
189  ur2_x, ur2_y, py2, pu2, pv2);
190  }
191 
192  FILTER_RECTIFY_ASSIGN;
193  }
194 
195  FILTER_RECTIFY_ADVANCE_LINE;
196  }
197  } else {
198  for (unsigned int h = 0; h < dst_roi->height; ++h) {
199  for (unsigned int w = 0; w < dst_roi->width; w += 2) {
200  __rib->mapping(w, h, &ur1_x, &ur1_y);
201  __rib->mapping(w+1, h, &ur2_x, &ur2_y);
202 
203  YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
204  ur1_x, ur1_y, py1, pu1, pv1);
205  YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
206  ur2_x, ur2_y, py2, pu2, pv2);
207 
208  FILTER_RECTIFY_ASSIGN;
209  }
210 
211  FILTER_RECTIFY_ADVANCE_LINE;
212  }
213  }
214 
215  }
216 }
217 
218 } // end namespace firevision
uint16_t pixel_width()
Get width of the LUT.
FilterRectify(RectificationInfoBlock *rib, bool mark_zeros=true)
Constructor.
Definition: rectify.cpp:55
Recitification Lookup Table Block.
fawkes::upoint_t start
ROI start.
Definition: roi.h:119
unsigned int y
y coordinate
Definition: types.h:36
unsigned int x
x coordinate
Definition: types.h:35
unsigned int width
ROI width.
Definition: roi.h:121
unsigned int image_width
width of image that contains this ROI
Definition: roi.h:125
Data type used to build a rectification LUT.
Definition: rectinfo.h:142
unsigned char ** src
Source buffers, dynamically allocated by Filter ctor.
Definition: filter.h:65
unsigned int image_height
height of image that contains this ROI
Definition: roi.h:127
Filter interface.
Definition: filter.h:35
virtual void mapping(uint16_t x, uint16_t y, uint16_t *to_x, uint16_t *to_y)=0
Get mapping (to_x, to_y) for (x, y).
ROI ** src_roi
Source ROIs, dynamically allocated by Filter ctor.
Definition: filter.h:70
uint16_t pixel_height()
Get height the LUT.
Rectification info block.
unsigned int height
ROI height.
Definition: roi.h:123
virtual void apply()
Apply the filter.
Definition: rectify.cpp:79
unsigned int line_step
line step
Definition: roi.h:129
uint16_t y
map to y pixel coordinate
Definition: rectinfo.h:144
rectinfo_lut_16x16_entry_t * lut_data()
Get raw LUT data.
unsigned char * dst
Destination buffer.
Definition: filter.h:67
Expected parameter is missing.
Definition: software.h:82
unsigned int pixel_step
pixel step
Definition: roi.h:131
ROI * dst_roi
Destination ROI.
Definition: filter.h:72
uint16_t x
map to x pixel coordinate
Definition: rectinfo.h:143