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 <core/exceptions/software.h>
25 #include <fvfilters/rectify.h>
26 #include <fvutils/color/yuv.h>
27 #include <fvutils/rectification/rectinfo_block.h>
28 #include <fvutils/rectification/rectinfo_lut_block.h>
29 
30 #include <cstddef>
31 #include <cstdio>
32 
33 namespace firevision {
34 
35 /** @class FilterRectify <fvfilters/rectify.h>
36  * Rectify image.
37  * This filter can be used to use a rectification information block to rectify
38  * the given image. It has special support for RectificationLutInfoBlocks by using the
39  * raw data pointer for fast access. For other info blocks it will simply use the
40  * RectificationInfoBlock::mapping() method to get the information.
41  * @author Tim Niemueller
42  */
43 
44 /** Constructor.
45  * @param rib Rectification Information Block
46  * @param mark_zeros if set to true mappings in the rectification info block that point
47  * to (0, 0) are marked with red color (luminance value unchanged). This allows for easy
48  * spotting of dead regions and may explain images that look broken. Enabled by default.
49  */
50 FilterRectify::FilterRectify(RectificationInfoBlock *rib, bool mark_zeros) : Filter("FilterRectify")
51 {
52  rib_ = rib;
53  mark_zeros_ = mark_zeros;
54 }
55 
56 #define FILTER_RECTIFY_ADVANCE_LINE \
57  ldyp += dst_roi->line_step; \
58  ldup += dst_roi->line_step / 2; \
59  ldvp += dst_roi->line_step / 2; \
60  dyp = ldyp; \
61  dup = ldup; \
62  dvp = ldvp;
63 
64 #define FILTER_RECTIFY_ASSIGN \
65  *dyp++ = py1; \
66  *dyp++ = py2; \
67  *dup++ = (pu1 + pu2) / 2; \
68  *dvp++ = (pv1 + pv2) / 2;
69 
70 void
72 {
73  // destination y-plane
74  unsigned char *dyp =
76 
77  // destination u-plane
78  unsigned char *dup =
79  YUV422_PLANAR_U_PLANE(dst, dst_roi->image_width, dst_roi->image_height)
80  + ((dst_roi->start.y * dst_roi->line_step) / 2 + (dst_roi->start.x * dst_roi->pixel_step) / 2);
81  // v-plane
82  unsigned char *dvp =
83  YUV422_PLANAR_V_PLANE(dst, dst_roi->image_width, dst_roi->image_height)
84  + ((dst_roi->start.y * dst_roi->line_step) / 2 + (dst_roi->start.x * dst_roi->pixel_step) / 2);
85 
86  // line starts
87  unsigned char *ldyp = dyp; // destination y-plane
88  unsigned char *ldup = dup; // u-plane
89  unsigned char *ldvp = dvp; // v-plane
90 
91  unsigned char py1 = 0, py2 = 0, pu1 = 0, pu2 = 0, pv1 = 0, pv2 = 0;
92 
93  RectificationLutInfoBlock *rlib = dynamic_cast<RectificationLutInfoBlock *>(rib_);
94 
95  if (rlib) {
96  if ((rlib->pixel_width() != dst_roi->image_width)
97  || (rlib->pixel_height() != dst_roi->image_height)) {
98  throw fawkes::IllegalArgumentException("Rectification LUT and image sizes do not match");
99  }
100 
101  // we have an rectification LUT info block
103  rlib->lut_data() + dst_roi->start.y * rlib->pixel_width() + dst_roi->start.x;
104 
105  rectinfo_lut_16x16_entry_t *llut = lut;
106 
107  if (mark_zeros_) {
108  for (unsigned int h = 0; h < dst_roi->height; ++h) {
109  for (unsigned int w = 0; w < dst_roi->width; w += 2) {
110  if (lut->x == 0 && lut->y == 0) {
111  py1 = YUV422_PLANAR_Y_AT(src[0], src_roi[0]->image_width, w, h);
112  pu1 = 0;
113  pv1 = 255;
114  } else {
115  YUV422_PLANAR_YUV(src[0],
116  src_roi[0]->image_width,
117  src_roi[0]->image_height,
118  lut->x,
119  lut->y,
120  py1,
121  pu1,
122  pv1);
123  }
124  ++lut;
125 
126  if (lut->x == 0 && lut->y == 0) {
127  py2 = YUV422_PLANAR_Y_AT(src[0], src_roi[0]->image_width, w, h);
128  pu2 = 0;
129  pv2 = 255;
130  } else {
131  YUV422_PLANAR_YUV(src[0],
132  src_roi[0]->image_width,
133  src_roi[0]->image_height,
134  lut->x,
135  lut->y,
136  py2,
137  pu2,
138  pv2);
139  }
140  ++lut;
141 
142  FILTER_RECTIFY_ASSIGN;
143  }
144 
145  FILTER_RECTIFY_ADVANCE_LINE;
146  llut += rlib->pixel_width();
147  lut = llut;
148  }
149  } else {
150  for (unsigned int h = 0; h < dst_roi->height; ++h) {
151  for (unsigned int w = 0; w < dst_roi->width; w += 2) {
152  YUV422_PLANAR_YUV(src[0],
153  src_roi[0]->image_width,
154  src_roi[0]->image_height,
155  lut->x,
156  lut->y,
157  py1,
158  pu1,
159  pv1);
160  ++lut;
161  YUV422_PLANAR_YUV(src[0],
162  src_roi[0]->image_width,
163  src_roi[0]->image_height,
164  lut->x,
165  lut->y,
166  py2,
167  pu2,
168  pv2);
169  ++lut;
170 
171  FILTER_RECTIFY_ASSIGN;
172  }
173 
174  FILTER_RECTIFY_ADVANCE_LINE;
175  llut += rlib->pixel_width();
176  lut = llut;
177  }
178  }
179  } else {
180  printf("Unknown info block\n");
181 
182  uint16_t ur1_x = 0, ur1_y = 0, ur2_x = 0, ur2_y = 0;
183 
184  if (mark_zeros_) {
185  for (unsigned int h = 0; h < dst_roi->height; ++h) {
186  for (unsigned int w = 0; w < dst_roi->width; w += 2) {
187  rib_->mapping(w, h, &ur1_x, &ur1_y);
188  rib_->mapping(w + 1, h, &ur2_x, &ur2_y);
189 
190  if ((ur1_x == 0) && (ur1_y == 0)) {
191  py1 = YUV422_PLANAR_Y_AT(src[0], src_roi[0]->image_width, w, h);
192  pu1 = 0;
193  pv1 = 255;
194  } else {
195  YUV422_PLANAR_YUV(src[0],
196  src_roi[0]->image_width,
197  src_roi[0]->image_height,
198  ur1_x,
199  ur1_y,
200  py1,
201  pu1,
202  pv1);
203  }
204  if ((ur2_x == 0) && (ur2_y == 0)) {
205  py2 = YUV422_PLANAR_Y_AT(src[0], src_roi[0]->image_width, w + 1, h);
206  pu2 = 0;
207  pv2 = 255;
208  } else {
209  YUV422_PLANAR_YUV(src[0],
210  src_roi[0]->image_width,
211  src_roi[0]->image_height,
212  ur2_x,
213  ur2_y,
214  py2,
215  pu2,
216  pv2);
217  }
218 
219  FILTER_RECTIFY_ASSIGN;
220  }
221 
222  FILTER_RECTIFY_ADVANCE_LINE;
223  }
224  } else {
225  for (unsigned int h = 0; h < dst_roi->height; ++h) {
226  for (unsigned int w = 0; w < dst_roi->width; w += 2) {
227  rib_->mapping(w, h, &ur1_x, &ur1_y);
228  rib_->mapping(w + 1, h, &ur2_x, &ur2_y);
229 
230  YUV422_PLANAR_YUV(
231  src[0], src_roi[0]->image_width, src_roi[0]->image_height, ur1_x, ur1_y, py1, pu1, pv1);
232  YUV422_PLANAR_YUV(
233  src[0], src_roi[0]->image_width, src_roi[0]->image_height, ur2_x, ur2_y, py2, pu2, pv2);
234 
235  FILTER_RECTIFY_ASSIGN;
236  }
237 
238  FILTER_RECTIFY_ADVANCE_LINE;
239  }
240  }
241  }
242 }
243 
244 } // end namespace firevision
fawkes::IllegalArgumentException
Definition: software.h:85
firevision::ROI::width
unsigned int width
ROI width.
Definition: roi.h:123
firevision::FilterRectify::apply
virtual void apply()
Definition: rectify.cpp:77
firevision::RectificationLutInfoBlock::pixel_width
uint16_t pixel_width()
Get width of the LUT.
Definition: rectinfo_lut_block.cpp:116
firevision::Filter::src
unsigned char ** src
Source buffers, dynamically allocated by Filter ctor.
Definition: filter.h:73
firevision::ROI::image_width
unsigned int image_width
width of image that contains this ROI
Definition: roi.h:127
firevision::ROI::image_height
unsigned int image_height
height of image that contains this ROI
Definition: roi.h:129
firevision::FilterRectify::FilterRectify
FilterRectify(RectificationInfoBlock *rib, bool mark_zeros=true)
Constructor.
Definition: rectify.cpp:56
firevision::RectificationInfoBlock::mapping
virtual void mapping(uint16_t x, uint16_t y, uint16_t *to_x, uint16_t *to_y)=0
firevision::RectificationLutInfoBlock
Definition: rectinfo_lut_block.h:37
firevision::RectificationLutInfoBlock::lut_data
rectinfo_lut_16x16_entry_t * lut_data()
Get raw LUT data.
Definition: rectinfo_lut_block.cpp:135
firevision::_rectinfo_lut_16x16_entry_t::x
uint16_t x
map to x pixel coordinate
Definition: rectinfo.h:142
firevision::ROI::height
unsigned int height
ROI height.
Definition: roi.h:125
firevision::RectificationLutInfoBlock::pixel_height
uint16_t pixel_height()
Get height the LUT.
Definition: rectinfo_lut_block.cpp:125
firevision::Filter::src_roi
ROI ** src_roi
Source ROIs, dynamically allocated by Filter ctor.
Definition: filter.h:78
fawkes::upoint_t::y
unsigned int y
y coordinate
Definition: types.h:37
firevision::ROI::pixel_step
unsigned int pixel_step
pixel step
Definition: roi.h:133
firevision::Filter::dst_roi
ROI * dst_roi
Destination ROI.
Definition: filter.h:80
firevision::_rectinfo_lut_16x16_entry_t::y
uint16_t y
map to y pixel coordinate
Definition: rectinfo.h:143
firevision::_rectinfo_lut_16x16_entry_t
Data type used to build a rectification LUT.
Definition: rectinfo.h:140
firevision::ROI::start
fawkes::upoint_t start
ROI start.
Definition: roi.h:121
firevision::ROI::line_step
unsigned int line_step
line step
Definition: roi.h:131
fawkes::upoint_t::x
unsigned int x
x coordinate
Definition: types.h:36
firevision::Filter::dst
unsigned char * dst
Destination buffer.
Definition: filter.h:75