00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <filters/unwarp.h>
00025
00026 #include <models/mirror/mirrormodel.h>
00027 #include <fvutils/color/yuv.h>
00028 #include <cstddef>
00029
00030
00031 namespace firevision {
00032 #if 0
00033 }
00034 #endif
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044 FilterUnwarp::FilterUnwarp(MirrorModel *mm)
00045 : Filter("FilterUnwarp")
00046 {
00047 this->mm = mm;
00048 }
00049
00050
00051 void
00052 FilterUnwarp::apply()
00053 {
00054
00055 register unsigned char *dyp = dst + (dst_roi->start.y * dst_roi->line_step) + (dst_roi->start.x * dst_roi->pixel_step);
00056
00057
00058 register unsigned char *dup = YUV422_PLANAR_U_PLANE(dst, dst_roi->image_width, dst_roi->image_height)
00059 + ((dst_roi->start.y * dst_roi->line_step) / 2 + (dst_roi->start.x * dst_roi->pixel_step) / 2) ;
00060
00061 register unsigned char *dvp = YUV422_PLANAR_V_PLANE(dst, dst_roi->image_width, dst_roi->image_height)
00062 + ((dst_roi->start.y * dst_roi->line_step) / 2 + (dst_roi->start.x * dst_roi->pixel_step) / 2);
00063
00064
00065 unsigned char *ldyp = dyp;
00066 unsigned char *ldup = dup;
00067 unsigned char *ldvp = dvp;
00068
00069 unsigned int warp1_x = 0, warp1_y = 0,
00070 warp2_x = 0, warp2_y = 0;
00071
00072 unsigned char py1=0, py2=0, pu1=0, pu2=0, pv1=0, pv2=0;
00073
00074 for (unsigned int h = 0; h < dst_roi->height; ++h) {
00075 for (unsigned int w = 0; w < dst_roi->width; w += 2) {
00076 mm->unwarp2warp( dst_roi->start.x + w, dst_roi->start.y + h,
00077 &warp1_x, &warp1_y );
00078 mm->unwarp2warp( dst_roi->start.x + w + 1, dst_roi->start.y + h + 1,
00079 &warp2_x, &warp2_y );
00080
00081 if ( (warp1_x < src_roi[0]->image_width) &&
00082 (warp1_y < src_roi[0]->image_height) ) {
00083
00084
00085 YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
00086 warp1_x, warp1_y,
00087 py1, pu1, pv1);
00088
00089 *dyp++ = py1;
00090 *dup = pu1;
00091 *dvp = pv1;
00092
00093
00094 if ( (warp2_x < src_roi[0]->image_width) &&
00095 (warp2_y < src_roi[0]->image_height) ) {
00096
00097 YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
00098 warp2_x, warp2_y,
00099 py2, pu2, pv2);
00100
00101 *dyp++ = py2;
00102 *dup = (*dup + pu2) / 2;
00103 *dvp = (*dvp + pv2) / 2;
00104 } else {
00105 *dyp++ = 0;
00106 }
00107 dup++;
00108 dvp++;
00109 } else {
00110
00111 *dyp++ = 0;
00112 *dup = 0;
00113 *dvp = 0;
00114
00115 if ( (warp2_x < src_roi[0]->image_width) &&
00116 (warp2_y < src_roi[0]->image_height) ) {
00117
00118 YUV422_PLANAR_YUV(src[0], src_roi[0]->image_width, src_roi[0]->image_height,
00119 warp2_x, warp2_y,
00120 py2, pu2, pv2);
00121
00122 *dyp++ = py2;
00123 *dup = pu2;
00124 *dvp = pv2;
00125 } else {
00126 *dyp++ = 0;
00127 }
00128
00129 dup++;
00130 dvp++;
00131 }
00132 }
00133
00134 ldyp += dst_roi->line_step;
00135 ldup += dst_roi->line_step;
00136 ldup += dst_roi->line_step;
00137 dyp = ldyp;
00138 dup = ldup;
00139 dvp = ldvp;
00140 }
00141 }
00142
00143 }