00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 // 00036 // C version of stereo algorithm 00037 // Basic column-oriented library 00038 // 00039 00040 // 00041 // input buffer sizes: 00042 // do_prefilter_norm - (xim+64)*4 00043 // do_stereo_y - (yim + yim + yim*dlen)*2 + yim*dlen*xwin + 6*64 00044 // ~ yim*dlen*(xwin+4) 00045 // do_stereo_d - (dlen*(yim-YKERN-ywin) + yim + yim*dlen)*2 + yim*dlen*xwin + 6*64 00046 // ~ yim*dlen*(xwin+5) 00047 00048 #ifndef STEREOLIBH 00049 #define STEREOLIBH 00050 00051 // alignment on allocation 00052 #ifdef __APPLE__ 00053 #include <malloc/malloc.h> 00054 #define MEMALIGN(x) malloc(x) 00055 #define MEMFREE(x) {if (x) free(x);} 00056 #else 00057 #include <malloc.h> 00058 #define MEMALIGN(x) memalign(16,x) 00059 #define MEMFREE(x) {if (x) free(x);} 00060 #endif 00061 00062 00063 #ifdef __cplusplus 00064 extern "C" { 00065 #endif 00066 00067 #ifdef WIN32 00068 //#ifndef GCC 00069 #include "pstdint.h" // MSVC++ doesn't have stdint.h 00070 #else 00071 #include <stdint.h> 00072 #endif 00073 #include <math.h> 00074 #include <string.h> 00075 #include <stdlib.h> 00076 00077 // kernel size is fixed 00078 #define KERNSIZE 3 00079 #define XKERN KERNSIZE 00080 #define YKERN KERNSIZE 00081 00082 // filtered disparity value 00083 // NOTE: lowest valid disparity is 1 00084 #define FILTEREDVAL 0 00085 00086 // prefilter 00087 00088 //#define do_prefilter do_prefilter_norm 00089 //#define do_prefilter do_prefilter_fast 00090 #define do_prefilter do_prefilter_xsobel 00091 00092 void 00093 do_prefilter_norm(uint8_t *im, // input image 00094 uint8_t *ftim, // feature image output 00095 int xim, int yim, // size of image 00096 uint8_t ftzero, // feature offset from zero 00097 uint8_t *buf // buffer storage 00098 ); 00099 00100 void 00101 do_prefilter_fast(uint8_t *im, // input image 00102 uint8_t *ftim, // feature image output 00103 int xim, int yim, // size of image 00104 uint8_t ftzero, // feature offset from zero 00105 uint8_t *buf // buffer storage 00106 ) 00107 #ifdef GCC 00108 // __attribute__ ((force_align_arg_pointer)) // align to 16 bytes 00109 #endif 00110 ; 00111 00112 void 00113 do_prefilter_xsobel(uint8_t *im, // input image 00114 uint8_t *ftim, // feature image output 00115 int xim, int yim, // size of image 00116 uint8_t ftzero, // feature offset from zero 00117 uint8_t *buf // buffer storage 00118 ); 00119 00120 00121 00122 00123 // algorithm requires buffers to be passed in 00124 00125 //#define do_stereo do_stereo_y 00126 //#define do_stereo do_stereo_d 00127 #define do_stereo do_stereo_d_fast 00128 //#define do_stereo do_stereo_so 00129 //#define do_stereo do_stereo_mw 00130 //#define do_stereo do_stereo_dp 00131 00132 // inner loop over disparities 00133 void 00134 do_stereo_d(uint8_t *lim, uint8_t *rim, // input feature images 00135 int16_t *disp, // disparity output 00136 int16_t *text, // texture output 00137 int xim, int yim, // size of images 00138 uint8_t ftzero, // feature offset from zero 00139 int xwin, int ywin, // size of corr window, usually square 00140 int dlen, // size of disparity search, multiple of 8 00141 int tfilter_thresh, // texture filter threshold 00142 int ufilter_thresh, // uniqueness filter threshold, percent 00143 uint8_t *buf // buffer storage 00144 ); 00145 00146 // inner loop over columns 00147 void 00148 do_stereo_y(uint8_t *lim, uint8_t *rim, // input feature images 00149 int16_t *disp, // disparity output 00150 int16_t *text, // texture output 00151 int xim, int yim, // size of images 00152 uint8_t ftzero, // feature offset from zero 00153 int xwin, int ywin, // size of corr window, usually square 00154 int dlen, // size of disparity search, multiple of 8 00155 int tfilter_thresh, // texture filter threshold 00156 int ufilter_thresh, // uniqueness filter threshold, percent 00157 uint8_t *buf // buffer storage 00158 ); 00159 00160 void 00161 do_stereo_d_fast(uint8_t *lim, uint8_t *rim, // input feature images 00162 int16_t *disp, // disparity output 00163 int16_t *text, // texture output 00164 int xim, int yim, // size of images 00165 uint8_t ftzero, // feature offset from zero 00166 int xwin, int ywin, // size of corr window, usually square 00167 int dlen, // size of disparity search, multiple of 8 00168 int tfilter_thresh, // texture filter threshold 00169 int ufilter_thresh, // uniqueness filter threshold, percent 00170 uint8_t *buf // buffer storage 00171 ); 00172 00173 00174 //December 2008 00175 //Additions by Federico Tombari 00176 //Stereo matching with regularization (Scanline Optimization) 00177 void do_stereo_so(uint8_t *lim, uint8_t *rim, // input feature images 00178 int16_t *disp, // disparity output 00179 int xim, int yim, // size of images 00180 uint8_t ftzero, // feature offset from zero 00181 int xwin, int ywin, // size of corr window, usually square 00182 int dlen, // size of disparity search, multiple of 8 00183 int pfilter_thresh, // texture filter threshold 00184 int ufilter_thresh, // uniqueness filter threshold, percent 00185 int smooth_thresh, // smoothness threshold 00186 int unique_c //uniqueness check 00187 ); 00188 00189 //Stereo matching using multiple windows 00190 void do_stereo_mw(uint8_t *lim, uint8_t *rim, // input feature images 00191 int16_t *disp, // disparity output 00192 int xim, int yim, // size of images 00193 uint8_t ftzero, // feature offset from zero 00194 int xwin, int ywin, // size of corr window, usually square 00195 int dlen, // size of disparity search, multiple of 8 00196 int pfilter_thresh, // texture filter threshold 00197 int ufilter_thresh, // uniqueness filter threshold, percent 00198 int unique_c //uniqueness check 00199 ); 00200 00201 //Stereo matching using Dynamic Programming 00202 void do_stereo_dp(uint8_t *lim, uint8_t *rim, // input feature images 00203 int16_t *disp, // disparity output 00204 int xim, int yim, // size of images 00205 uint8_t ftzero, // feature offset from zero 00206 int xwin, int ywin, // size of corr window, usually square 00207 int dlen, // size of disparity search, multiple of 8 00208 int pfilter_thresh, // texture filter threshold 00209 int ufilter_thresh, // uniqueness filter threshold, percent 00210 int smooth_thresh, // smoothness threshold 00211 int unique_c //uniqueness check 00212 ); 00213 00214 //Stereo matching using Local Smoothness 00215 void do_stereo_ls(uint8_t *lim, uint8_t *rim, // input feature images 00216 int16_t *disp, // disparity output 00217 int xim, int yim, // size of images 00218 uint8_t ftzero, // feature offset from zero 00219 int xwin, int ywin, // size of corr window, usually square 00220 int dlen, // size of disparity search, multiple of 8 00221 int pfilter_thresh, // texture filter threshold 00222 int ufilter_thresh, // uniqueness filter threshold, percent 00223 int smooth_thresh, // smoothness threshold 00224 int unique_c //uniqueness check 00225 ); 00226 00227 //Stereo matching using NCC and block-matching approach 00228 void do_stereo_ncc(uint8_t *lim, uint8_t *rim, // input feature images 00229 int16_t *disp, // disparity output 00230 int xim, int yim, // size of images 00231 uint8_t ftzero, // feature offset from zero 00232 int xwin, int ywin, // size of corr window, usually square 00233 int dlen, // size of disparity search, multiple of 8 00234 int pfilter_thresh, // texture filter threshold 00235 int ufilter_thresh, // uniqueness filter threshold, percent 00236 int unique_c //uniqueness check 00237 ); 00238 // 00239 // sparse stereo 00240 // corr window fixed at 15x15 00241 // returns disparity value in 1/16 pixel, -1 on failure 00242 // refpat is a 16x16 patch around the feature pixel 00243 // feature pixel is at coordinate 7,7 within the patch 00244 // rim is the right gradient image 00245 // x,y is the feature pixel coordinate 00246 // other parameters as in do_stereo 00247 // 00248 00249 int 00250 do_stereo_sparse(uint8_t *refpat, uint8_t *rim, // input feature images 00251 int x, int y, // position of feature pixel 00252 int xim, int yim, // size of images 00253 uint8_t ftzero, // feature offset from zero 00254 int dlen, // size of disparity search, multiple of 8 00255 int tfilter_thresh, // texture filter threshold 00256 int ufilter_thresh // uniqueness filter threshold, percent 00257 ); 00258 00259 00260 // SSE2 version 00261 int 00262 do_stereo_sparse_fast(uint8_t *refpat, uint8_t *rim, // input feature images 00263 int x, int y, // position of feature pixel 00264 int xim, int yim, // size of images 00265 uint8_t ftzero, // feature offset from zero 00266 int dlen, // size of disparity search, multiple of 8 00267 int tfilter_thresh, // texture filter threshold 00268 int ufilter_thresh // uniqueness filter threshold, percent 00269 ); 00270 00271 00272 // speckle filter 00273 // needs buffers: labels[image size], wbuf[image size], rtype[image size] 00274 void do_speckle(int16_t *disp, int16_t badval, int width, int height, 00275 int rdiff, int rcount, 00276 uint32_t *labels, uint32_t *wbuf, uint8_t *rtype); 00277 00278 00279 // 00280 // bilinear interpolation structure 00281 // embeds fixed-point interpolation coefficients 00282 // should be 8 bytes in size 00283 00284 #define INTOFFSET 8 // 8-bit fractional offset of interpolation fixed-point 00285 00286 typedef struct 00287 { 00288 uint32_t addr; // pixel addr, upper left 00289 uint8_t a1,a2,b1,b2; // coefficients 00290 } inttab_t; 00291 00292 void 00293 do_rectify_mono(uint8_t *dest, // destination image 00294 uint8_t *src, // source image 00295 int w, // image width 00296 int h, // image height 00297 inttab_t *rtab // interpolation table 00298 ); 00299 00300 00301 void 00302 do_rectify_mono_fast(uint8_t *dest, // destination image 00303 uint8_t *src, // source image 00304 int w, // image width 00305 int h, // image height 00306 inttab_t *rtab // interpolation table 00307 ); 00308 00309 00310 #ifdef __cplusplus 00311 } 00312 #endif 00313 00314 00315 #endif 00316 00317