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
33namespace 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 */
50FilterRectify::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
70void
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
Expected parameter is missing.
Definition: software.h:80
FilterRectify(RectificationInfoBlock *rib, bool mark_zeros=true)
Constructor.
Definition: rectify.cpp:50
virtual void apply()
Apply the filter.
Definition: rectify.cpp:71
Filter interface.
Definition: filter.h:33
ROI ** src_roi
Source ROIs, dynamically allocated by Filter ctor.
Definition: filter.h:66
unsigned char ** src
Source buffers, dynamically allocated by Filter ctor.
Definition: filter.h:61
unsigned char * dst
Destination buffer.
Definition: filter.h:63
ROI * dst_roi
Destination ROI.
Definition: filter.h:68
unsigned int height
ROI height.
Definition: roi.h:119
fawkes::upoint_t start
ROI start.
Definition: roi.h:115
unsigned int line_step
line step
Definition: roi.h:125
unsigned int width
ROI width.
Definition: roi.h:117
unsigned int image_width
width of image that contains this ROI
Definition: roi.h:121
unsigned int pixel_step
pixel step
Definition: roi.h:127
unsigned int image_height
height of image that contains this ROI
Definition: roi.h:123
Rectification info block.
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).
Recitification Lookup Table Block.
uint16_t pixel_height()
Get height the LUT.
rectinfo_lut_16x16_entry_t * lut_data()
Get raw LUT data.
uint16_t pixel_width()
Get width of the LUT.
unsigned int x
x coordinate
Definition: types.h:36
unsigned int y
y coordinate
Definition: types.h:37
Data type used to build a rectification LUT.
Definition: rectinfo.h:141
uint16_t y
map to y pixel coordinate
Definition: rectinfo.h:143
uint16_t x
map to x pixel coordinate
Definition: rectinfo.h:142