rectify.cpp

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