From 1268ff8dc214ab31a46ae6130e3849c6ab05f262 Mon Sep 17 00:00:00 2001 From: Razvan Mihalyi Date: Tue, 13 Nov 2012 16:49:04 +0100 Subject: [PATCH] update svn to r765 --- ...ba76b444ba88f86a19fa8184c016c7d2e.svn-base | 10 + ...db708baffd361e6e7f0fa8d88aaf822e5.svn-base | 134 +++++++++ ...7c02e234c8b8c637192d2b757a57ea212.svn-base | 275 ++++++++++++++++++ .svn/wc.db | Bin 629760 -> 631808 bytes include/normals/normals.h | 134 +++++++++ include/slam6d/pairingMode.h | 10 + src/normals/calc_normals.cc | 275 ++++++++++++++++++ 7 files changed, 838 insertions(+) create mode 100644 .svn/pristine/28/286a425ba76b444ba88f86a19fa8184c016c7d2e.svn-base create mode 100644 .svn/pristine/b1/b139234db708baffd361e6e7f0fa8d88aaf822e5.svn-base create mode 100644 .svn/pristine/e0/e0010b67c02e234c8b8c637192d2b757a57ea212.svn-base create mode 100644 include/normals/normals.h create mode 100644 include/slam6d/pairingMode.h create mode 100644 src/normals/calc_normals.cc diff --git a/.svn/pristine/28/286a425ba76b444ba88f86a19fa8184c016c7d2e.svn-base b/.svn/pristine/28/286a425ba76b444ba88f86a19fa8184c016c7d2e.svn-base new file mode 100644 index 0000000..ab9f970 --- /dev/null +++ b/.svn/pristine/28/286a425ba76b444ba88f86a19fa8184c016c7d2e.svn-base @@ -0,0 +1,10 @@ +#ifndef __PAIRINGMODE_H__ +#define __PAIRINGMODE_H__ + +enum PairingMode { + CLOSEST_POINT, + CLOSEST_POINT_ALONG_NORMAL, + CLOSEST_PLANE +}; + +#endif // PAIRINGMODE_H diff --git a/.svn/pristine/b1/b139234db708baffd361e6e7f0fa8d88aaf822e5.svn-base b/.svn/pristine/b1/b139234db708baffd361e6e7f0fa8d88aaf822e5.svn-base new file mode 100644 index 0000000..ae76431 --- /dev/null +++ b/.svn/pristine/b1/b139234db708baffd361e6e7f0fa8d88aaf822e5.svn-base @@ -0,0 +1,134 @@ +#ifndef NORMALS_H +#define NORMALS_H + +#include +#include +#if (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#include +#else +#include +#endif + +void calculateNormalsAKNN(std::vector &normals,vector &points, int k, + const double _rPos[3] ); +void calculateNormalsAdaptiveAKNN(vector &normals,vector &points, + int kmin, int kmax, const double _rPos[3]); +void calculateNormalsPANORAMA(vector &normals, + vector &points, + vector< vector< vector< cv::Vec3f > > > extendedMap, + const double _rPos[3]); +// TODO should be exported to separate library +/* + * retrieve a cv::Mat with x,y,z,r from a scan object + * functionality borrowed from scan_cv::convertScanToMat but this function + * does not allow a scanserver to be used, prints to stdout and can only + * handle a single scan + */ +static inline cv::Mat scan2mat(Scan *source) +{ + DataXYZ xyz = source->get("xyz"); + + DataReflectance xyz_reflectance = source->get("reflectance"); + + unsigned int nPoints = xyz.size(); + cv::Mat scan(nPoints,1,CV_32FC(4)); + scan = cv::Scalar::all(0); + + cv::MatIterator_ it; + + it = scan.begin(); + for(unsigned int i = 0; i < nPoints; i++){ + float x, y, z, reflectance; + x = xyz[i][0]; + y = xyz[i][1]; + z = xyz[i][2]; + if(xyz_reflectance.size() != 0) + { + reflectance = xyz_reflectance[i]; + + //normalize the reflectance + reflectance += 32; + reflectance /= 64; + reflectance -= 0.2; + reflectance /= 0.3; + if (reflectance < 0) reflectance = 0; + if (reflectance > 1) reflectance = 1; + } + + (*it)[0] = x; + (*it)[1] = y; + (*it)[2] = z; + if(xyz_reflectance.size() != 0) + (*it)[3] = reflectance; + else + (*it)[3] = 0; + + ++it; + } + return scan; +} +// TODO should be exported to separate library +/* + * convert a matrix of float values (range image) to a matrix of unsigned + * eight bit characters using different techniques + */ +static inline cv::Mat float2uchar(cv::Mat &source, bool logarithm, float cutoff) +{ + cv::Mat result(source.size(), CV_8U, cv::Scalar::all(0)); + float max = 0; + // find maximum value + if (cutoff == 0.0) { + // without cutoff, just iterate through all values to find the largest + for (cv::MatIterator_ it = source.begin(); + it != source.end(); ++it) { + float val = *it; + if (val > max) { + max = val; + } + } + } else { + // when a cutoff is specified, sort all the points by value and then + // specify the max so that values are larger than it + vector sorted(source.cols*source.rows); + int i = 0; + for (cv::MatIterator_ it = source.begin(); + it != source.end(); ++it, ++i) { + sorted[i] = *it; + } + std::sort(sorted.begin(), sorted.end()); + max = sorted[(int)(source.cols*source.rows*(1.0-cutoff))]; + cout << "A cutoff of " << cutoff << " resulted in a max value of " << max << endl; + } + + cv::MatIterator_ src = source.begin(); + cv::MatIterator_ dst = result.begin(); + cv::MatIterator_ end = source.end(); + if (logarithm) { + // stretch values from 0 to max logarithmically over 0 to 255 + // using the logarithm allows to represent smaller values with more + // precision and larger values with less + max = log(max+1); + for (; src != end; ++src, ++dst) { + float val = (log(*src+1)*255.0)/max; + if (val > 255) + *dst = 255; + else + *dst = (uchar)val; + } + } else { + // stretch values from 0 to max linearly over 0 to 255 + for (; src != end; ++src, ++dst) { + float val = (*src*255.0)/max; + if (val > 255) + *dst = 255; + else + *dst = (uchar)val; + } + } + return result; +} + + + +#endif // NORMALS_H diff --git a/.svn/pristine/e0/e0010b67c02e234c8b8c637192d2b757a57ea212.svn-base b/.svn/pristine/e0/e0010b67c02e234c8b8c637192d2b757a57ea212.svn-base new file mode 100644 index 0000000..1a88e9e --- /dev/null +++ b/.svn/pristine/e0/e0010b67c02e234c8b8c637192d2b757a57ea212.svn-base @@ -0,0 +1,275 @@ +/** + * + * Copyright (C) Jacobs University Bremen + * + * @author Vaibhav Kumar Mehta + * @file calc_normals.cc + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include "slam6d/fbr/panorama.h" +#include + +#include + + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +namespace po = boost::program_options; +using namespace std; + +enum normal_method {AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST}; + +/* + * validates normal calculation method specification + */ +void validate(boost::any& v, const std::vector& values, + normal_method*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + if(strcasecmp(arg.c_str(), "AKNN") == 0) v = AKNN; + else if(strcasecmp(arg.c_str(), "ADAPTIVE_AKNN") == 0) v = ADAPTIVE_AKNN; + else if(strcasecmp(arg.c_str(), "PANORAMA") == 0) v = PANORAMA; + else if(strcasecmp(arg.c_str(), "PANORAMA_FAST") == 0) v = PANORAMA_FAST; + else throw std::runtime_error(std::string("normal calculation method ") + arg + std::string(" is unknown")); +} + +/// validate IO types +void validate(boost::any& v, const std::vector& values, + IOType*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + try { + v = formatname_to_io_type(arg.c_str()); + } catch (...) { // runtime_error + throw std::runtime_error("Format " + arg + " unknown."); + } +} + +/// Parse commandline options +void parse_options(int argc, char **argv, int &start, int &end, bool &scanserver, int &max_dist, int &min_dist, string &dir, + IOType &iotype, int &k1, int &k2, normal_method &ntype,int &width,int &height) +{ + /// ---------------------------------- + /// set up program commandline options + /// ---------------------------------- + po::options_description cmd_options("Usage: calculateNormals where options are (default values in brackets)"); + cmd_options.add_options() + ("help,?", "Display this help message") + ("start,s", po::value(&start)->default_value(0), "Start at scan number ") + ("end,e", po::value(&end)->default_value(-1), "Stop at scan number ") + ("scanserver,S", po::value(&scanserver)->default_value(false), "Use the scanserver as an input method") + ("format,f", po::value(&iotype)->default_value(UOS), + "using shared library for input. (chose format from [uos|uosr|uos_map|" + "uos_rgb|uos_frames|uos_map_frames|old|rts|rts_map|ifp|" + "riegl_txt|riegl_rgb|riegl_bin|zahn|ply])") + ("max,M", po::value(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than 'units") + ("min,m", po::value(&min_dist)->default_value(-1),"neglegt all data points with a distance smaller than 'units") + ("normal,g", po::value(&ntype)->default_value(AKNN), "normal calculation method " + "(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)") + ("K1,k", po::value(&k1)->default_value(20), " value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation") + ("K2,K", po::value(&k2)->default_value(20), " value of Kmax for k-adaptation") + ("width,w", po::value(&width)->default_value(1280),"width of panorama image") + ("height,h", po::value(&height)->default_value(960),"height of panorama image") + ; + + po::options_description hidden("Hidden options"); + hidden.add_options() + ("input-dir", po::value(&dir), "input dir"); + + po::positional_options_description pd; + pd.add("input-dir", 1); + + po::options_description all; + all.add(cmd_options).add(hidden); + + po::variables_map vmap; + po::store(po::command_line_parser(argc, argv).options(all).positional(pd).run(), vmap); + po::notify(vmap); + + if (vmap.count("help")) { + cout << cmd_options << endl << endl; + cout << "SAMPLE COMMAND FOR CALCULATING NORMALS" << endl; + cout << " bin/normals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" < &points, vector &normals, int scanNumber) +{ + string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d"; + ofstream normptsout(ofilename.c_str()); + + for (size_t i=0; i::iterator it = Scan::allScans.begin(); + int scanNumber = 0; + + for( ; it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + + // apply optional filtering + scan->setRangeFilter(max_dist, min_dist); + + const double* rPos = scan->get_rPos(); + const double* rPosTheta = scan->get_rPosTheta(); + + /// read scan into points + DataXYZ xyz(scan->get("xyz")); + vector points; + points.reserve(xyz.size()); + vector normals; + normals.reserve(xyz.size()); + + for(unsigned int j = 0; j < xyz.size(); j++) { + points.push_back(Point(xyz[j][0], xyz[j][1], xyz[j][2])); + } + + if(ntype == AKNN) + calculateNormalsAKNN(normals,points, k1, rPos); + else if(ntype == ADAPTIVE_AKNN) + calculateNormalsAdaptiveAKNN(normals,points, k1, k2, rPos); + else + { + // create panorama + fbr::panorama fPanorama(width, height, fbr::EQUIRECTANGULAR, 1, 0, fbr::EXTENDED); + fPanorama.createPanorama(scan2mat(scan)); + + // the range image has to be converted from float to uchar + img = fPanorama.getRangeImage(); + img = float2uchar(img, 0, 0.0); + + if(ntype == PANORAMA) + calculateNormalsPANORAMA(normals,points,fPanorama.getExtendedMap(), rPos); + else if(ntype == PANORAMA_FAST) + cout << "PANORAMA_FAST is not working yet" << endl; + // calculateNormalsFAST(normals,points,img,fPanorama.getExtendedMap()); + } + + // pose file (repeated for the number of segments + writePoseFiles(normdir, rPos, rPosTheta, scanNumber); + // scan files for all segments + writeScanFiles(normdir, points,normals,scanNumber); + + scanNumber++; + } + + // shutdown everything + if (scanserver) + ClientInterface::destroy(); + + Scan::closeDirectory(); + + cout << "Normal program end" << endl; + + return 0; +} diff --git a/.svn/wc.db b/.svn/wc.db index 6090833ce7189d0b993a2d6cfe47822efdcef1ac..7fd82f195989ec0514c239a48b1b41274fef4859 100644 GIT binary patch delta 15022 zcma(&37n4A`tLdCU5A-53p3dnGh@i?8;UHEJw!q@tFeSk4bdf33}xwKPNhpIiZW$t zUYYNkv2?jn#0XI&C0m4R$@+hu=l#Bqy8VB@TYm3*zOy|0cFud|#SJkp{ySztJ?)^P z@R17}w}(cnpPGDsnLtB>rBfhs*P;MTo1FoC2^etP3l1%gg{6 z>abn~qE_sI=7o+x-)}1_poxUI+$&dk*SaRYTj}jB>-HW)Y)G1G*s99_t;y}MvJtCq z5ii%FchZ{m)En+|rvo$_!io;qS=Fg>^Op04~bhrJDk=t(#%41)kH+YQg%UYa^hy%AS!6q(qZ~dHoTUPYNB;dw%e%D5Cf0<|Zv$ztc%Yzwa ztZU5Pf8cNA?AG4*p?PeW7GAcox|i)<810HOPbqP= z_&F!!TSKz~O|I_ozW0?a{kcmh!bJ&QyMoF^SY@AIeFou@a@$>B0dS99{{lNE0Md;& z@?j7#fw&yAxP1F#Y=U6QKEKmK(DX2Ql{4xFk=qr5JqH}4V79?9n1ZZPgDv&0vOP6e zD+7L<31uE%pF51T0Z>GQ42WQ4gdj)T6Tyl=>@u|}`wYiHbn;eH25Lg?ZFdx#jKm{3 zvU?1z+tW~kIWahgbYZ(=*$X%ctxxuB;Q+@4cUrJD*k8vCw<}w+C=qyT?6?U}XN}1p zIVP`poMio3ysV$VUWG|0HQSxQ9zrq+QmqvB2DA~i?M`7c5e3u14ZfRV#mLF&>;*6L z0U5-+10jP=Lw?8)`*dXJssL>RypT8B(~13qWQTs*yE|Kn1K@U2>p^YAY`c4~ML{so zcQ6BcLP1E`n>ClMhp=%DPTM_%g&;v_81mSYY$n1%AANopn+k0*+8V=&FaQJm4`=ry zYb3qf*(4Z5Y`b&V!>XkyieQ%%cF7JXU_UYyet#%`8ObtzaFHz#y8K{9zPBheQJG4@R?>{3?4lLBz^}vFs%;=8|!Y{Dy=*jx`60$bPO(V4olW z?DrJ1c%+nAyHn&)N&^$w{b*RHr?M(|A!ge%jnzkaUx+c9$=k&!Xacxn4*Lqalhzy2 zwtFrchZObA7UA-tc_>qodC7eC0X9i5^FazhjTBYMQj{z(l^(oA)>=et^_>cG{eBVa zq_Sg*DjO|k`LGOqR=%|udO=p7L6H7L9$U9sqrVkSjGG_j{vv!^r|fiU5Q7jb`i4U3fJo5D8giy&)97M4#`3eDrbK{H)$o$W^bFpz$#})6@C>S&6H*fJJjL4 zx-6t~9mZJ&$Y7alwu$9<*T^NC*i8frK|`+H%-$;hf}KM}kPVu{d8~}v&ZgmPRDpXt zi$(R4J6zqxAO(;VY|m~s7MI<>mu_XBvs~ddcpV20>R8e1~i^s%sVxcG%yTtj0 z9ct^*n$^Q(uK8CQ+4erxhRJdJSdlS*9Y<`$ccQk5y4zu|d4EFkkZN)h! z=(X?&50|G-khMEu=vQ_GqX&s--8psy+vxRUM_H}xUFX>q!iKT$;6>orYiAXg*lHXg zCz*AH`BQ4zRS*r32N}-xTxGd1i3}|5Iz?BI^uHZbust_eS2d`)Qm81!$};6MWv5cO zxE9Zn4c_9xa={H#XJ5(bH_Y0fE#)16L~_DQI!BWQQ?lI#j}h!^`*;8kLg%JrnEx1< z!2Ln|9q9Nvh<^_+#~Fk9Km3gJ4&|$H0NFfWEkb0yFh0+r)Aoe%$7v!))8=)!FD%F# zb@)15L}4R>4@IE?%5WLqke5TdNsv}Y-e^dZDCBw?@wFI7$n9QeffyW=jau?Kgbh;& zf11RIA+LQrFM~N`%EnG3R<=sukPzhZc@}tO0`DR+l(o`G%Z!zSJMslEc%~yqLqP8B%1*p3!l85;*quWeLi+x`JO3KGF+kdtJ$NtV zk~nMBn`BDa>|$?DRTri0K71h58l>geZ0x6iv8W#?^u8RzOX1Fw!v~^RDFj6Jh0)$) zX_PoMp{SDgjl&LvWP8T(?mpY8SH#g19Lz-yW-`->yth}IVY1OwB9D}I;0!3?zG?U1 zp2piDBNQ)!XYifA_PH6zIMHN#X7cWSz1N%P=TK%&0Ad5@^S5y(#%Irb2QkU6Q3)a@ z_79|3fL9U|Sh9E|%_F%sTFO6%^;AxrUCOZ>a>TNX_asZgtQEh4V`)Y5j$DWWTuDq^ z&@Ir$O5Pu55{uER2pq}o;wqvQSJ|G`yeG}yad~RfB9}5@9gVHF69%kO)0OG zF9*;>=>87?@0;uSXc(l79$Cg0Lff&dGCmI{Vxc#@oWJgEYBtAs<}hUQ1w=t-u>Qpx z0a$S*9}AR;NEXenL*xn4IC>C=j2M1A$=dKHkv&<6C}~$*)eC^-2!% zlSn!Su$u>YW8@WWz)n2GBYlk9Ll5)z_=9Y+^m~2=+LT{Pf8fWU4GH8jyhs?3BI4~Y zkMb zsy2{~kMa;X=pt|Zr=jYL95XxmubqF%F?U!PCbwPYBVYh1NRq>^@I^QYcUT*K1`6V@ z@~O~4X1D1oU*e4&;c`3|G!p}Tq!4JEBo2=d?a@HU4r&?VO>ARX>oLT`I6%c)L=6E} z>xjuyLv(FZUzw!9zZHe{tt|_9`Sk3Asm)8{a+*Pu3MnkQ8LOWzZ#>NOx48_nVW5q`FFBG9{*+O8X=W42%6T0*e!DQ_S3ik(uT+AL`_(Kx$3@pbjYlhDx#-A`^Ov}MW}w!rta0bx z*|edvp3p}s+v%JK_q{Qs>jGV;eyyTSBoXqUZob;58q}Iu~$=Mlc8Chu= zd0ELx$x+`odpb9*akHGf^!%i(#FUKG%?y8gwuC z?4*>|05&%}BQY~OD?dMXT1t9SUV2_eeqw%BW^QI?R#tvya&lf;@J0uIc4B&BZbnLW z>#UTNw7e7motlx7ncX@$FEK4CEkA4MtjF(#pPZSVm71KEot2TEotm1Oot2rH4_!&E z0XmSDlbDpAlaZU8r>yjn-#jfpCp9BEIV&@#bxu-dMs`kiV(Zk_`T0q$Q?k-CGl$L^ z=)jjBjfb@O{IB9u42ba;F>sLM{C-hj=>~ZPim*!}0c?xk*Y#1XFsf|28JuL(x-IwBDDKH2Qj1Ia~5O2W~dv`obg=ei2xO21szQU*HF>* zoWiGuIu9Kk4-n;34b?ZGjptZ0u#t);?D$0^m28`mU(F~LlZj*7QED2-sY@-?a-2+b zwp6F0k5W+UldP_SHqmFLs2InbaFwEdiiA%{Q}4iwczPvW-41PXtPN**tUWtJ9f&SP zKD@t`3KeTx6=N1AHQU`*ZH1$BK61OAx(3=9Wo&nQH4_HN1I$juHf{^xNKB=3Rwv?p z4SqGIiwac$ObnNIx)1<@?CGlhfLdD8PrU#ya?0iX)naet3772$)8u0qoo^3T(W9M( zL(~Uw6750)vxyx_z$dfS%?JtPBRx54R}rGjmqxDIREAtrvll+9$_0<80rCH<_lWOO z#7Hh*9VX)K%em?}th6ZmO&Uv@C2>Mg8Y)|lQ?bTHnc9`()I5wURC*Rnhb_^t2`ZWw zElux+@-0*sfIx5~?I~18Aul0afXN3YtKJQ^)TXFdjeGNn`UeGBEP`PJ8zzrTR|ohY zcAcSq0Bz6-+6Ep?*zTDsoe`6ctD8b0R`81Y3XI~>p}cPu99G25BASR|&n$H(SP3e~ z{%5f&&|;R)QP0AQl(J&3x`gD7)!L$Yz`&%}Rg9F!R;Z9SU1V=)w6@2kwxp^DE1jEf zss0LOiAp;fN=Xe@s9T)HupzeBtW?J#@VaYMJmDkVU0tpR|+2>2uHn@a%d+-zD#|A;x@^#Anc~-3bYMGZSJYH$L z5ovXt;-=F^jJBsj%_Y`xQrZ?3&j`rWZf+swsKVK*4nu{LeiOE-7-y*VxVephI-a;) zrBe&C*;cy<(6L=A9tk))*sTuq1IdgBuj#!Pjf|qo&AkK&#cfwst9{XH5UPwiLf9zl z|4wamN858$eGI84Dxy!IQ8`qfJE2y1`)$v!j^$CbX?u<^P%ydawDD}*t~{@fBxA;X zS=)<#AC%WGqKZf@w&#-C810itx^PYX7qscDYv&&-9?nj>u5N-CQQ7l`ItSWhS7&ah zy+9|#V$WL?RVirXy#(w(e_Q?5u>qO?supV(-%&TB0(~0|L%zT?DEaQ`=)pCL5vZ#G zp<$7NZe@FfhDWi4r@9Cna*(ARMIvxx(IY@3gC{ZMo>T!-4NW4?P{&~Qz_9gm5JB_d zHf?vXh96q)K|Ul@g94Cn1chl(u6Scrm{!lPuCooaDTo*MO;Z|b=)xdzto?nE4cpyF zBNsXuqiw~xXoXd=S|`5)X1oIa{9wE`3r5MB4#sPH!6VitXcLem(qX?0DB1=klYnT_ zh}OV&T#7b>XbiVErD(K?A%Qn+r@5gY&xp@a)A`0y)n226TG|$jJjG&UK}YRnBnrQ9 zJeLM2ns?G(@^;vpI%$1?01`oYDhzh-qIJeWFT0`g(=J-9uLC~+g-g*@!)+#c)T3Rs zSE0Qp)CiYFO|9VaN3k-TSuOu-wfc?SW1{vBkmP9h&hr}X`TPbVFHhDW3L(9=dx|y+ClX_ArfQe~U}ulV zdRTgPm{{=tHXok+Hx2r-CQi!Fw)|IfW67q=wSZx z=h`9YBa1(NJnjmuH6kbatx65U6t&aV10hzGhMOijC8?;=h67G=i1=+3 zR1R&^cHwYGICk}H*9yHPRYD(8BX(==IV)`UZtZa#Cbwz6AA^V^n$!EW&EENT)d8&! zqQIgtau49|b_5wGL0mh68YbH8s-qfx>>?FzI0+L+{^a-|M&yW7&=GeWjhQ5FyN_#a ziAOa18YeW&FBI8+J)wQ!ptGxf)rM0@LW#Og1B53oYA%?F@w?=b28T%Y2bZ+n2!}Gr zt5+!4P^xWyRfAfFN*DK44Y%jS!oimT%$(a~A4F=aI~s<5^nLrcJC4?AW0=5oFgG+q z$%Veki^Fycy)z<&tW)j7ds5diZ%~+a8+tE)9BmM)Z-fC*bGSSks-u56ABDqoJOXtR z(IgA>|5hEH;w0MJ0qhtWq5lJ65?v8`oPTO@?OI2MW5y>*x;z^?baJUpTQ4 ztVqxYBQYesxMUpy7D@JEvO^c<0e6bN0VyJ2v)kx+Y(~+wRa+es3&MngUB?RyPA$`* zBauZWaK0mvg<7`Vo%H@FPEtu^8We=xd+Up#0~8)ECt^q4KKeZ9Kwq-mee?k&DKwer zenb&D+4+71(@C@a^}b%}VE;c5Ky(|dW5A&Bbat?gp~aEh5dAR(2WOjTpk!pf4I`Z* zrFO+|{RxCb#Kh(67|mVdLE{L1pK$<|1oCsN{4dpMIAhQxxKnN!;hIy^3$f_JWt^!BGMVK-k->kob zZ3;45^li{4Cou7PK+UZ>R9-}8$SBxB9kegeip&^$ z)=vEa*qb}B5A4_XKpS1cb|26y0G3lL%>6+>fs=4U^vV%G0rpcz_3;J_z-Dznyq1rL z>d*bFeuPwvA7`pg>jz9T*m+o9Yica> z@#Ky&QmKN7F-}1r6lc-p&5hYXKGHu*CrFN9G7KuYsDf+L#=rx9Upqwpr;V}F54Ejv zzaM8{JL8xSTv&Spot)4-WGwN|+0@ZE;9uOyIP7QB-Q7400}dxW4EmZ*GTqego)!I# z1O62o1{+8HpoSR7{W!iDW?<%VSQ&2o;2)^WHoo%@4 z9W6IcFwpw24f}iJkROt}h^Tc?Of>fRt!4cT;};)P&rGAMIgaH+k;q5NnlIt{G4jHL z;tBXn5gWh@iu=OIlf^yl2`?Mb=n@D|y64;jd#-W#F4B4T49qtk@()BjZ#0+PUN@+Y z!_(3FE`#bBRPrL@J3n=|mm1&u_Yy8(&kc|=l?;6#9z}Hq8Kl$drf0vSywFVYv zH2p&(5j6s0yzT9h1Imn-eZ!v`qy5kpl%v9Is0i_T5iOUM8x6{LK&c#F;cu6_jfP&Z z(X#UvGC0bQwAl#WYRm=zj`X)0214`InTEW%&47Z%{$RT?8yKfx`S4x?kKicEoI7lU zN~_wyVDDdKZ>%;bk}l+m%<1 z@u;VS>jvg_0($2S`qx%0I$=!iKQAWjrDAM3~PJRy-l;($stp z;9we-cbYnLF}u2>%*jYP4Rnv8Nx#PsnNEokYtkh|l3>pkCd4k1hiK`*BlcRFQ;1qsWQ9js@2B>|zpLzEfw!b+V*$0~mmk$m$vEC;WxHXt2 zIkq&!#LLPK!H*0hMUzY?4gk7Z3^%7DPTExsH|YfIu58 z*Bs3vG7z{cExV2~VbkJlxYF>PqQz)}i`D}heDe>afVA}GSQDeaBZzTc(m-ai+XVAn z7$CXMUIoQZ^ForVV+@7nc(kZiQ%$O2(GPA-B_^rr@=P-e$>Qdan2EoAGmyWzKK_7? zxn@zoUv8{sO@|w+PnVc40B|f;`FLsCxVwG4V zW{B}38!pd14Bv`lMWnF!b$*6_&%frCd@X;2zsx7_;qW=VHC($3;|lwY9b((rr|ccL zcRPvYvPanitT|k*P?WO(`jE0+`BYgBcL^Iu{QY^yZdv%D8Qo^6bDhd}mNNe7Ty;nY z-mqf#bVY<|R_xs#x|SCx7nFif=9hvNr{H4*KKkRMJw96EBLW`bDxiqIyL!Zgk+~z} zj&)|V{Bj-Gc{x5-;$s0m{(+BC_;?&2o$--`kB0cD0gnd<2WeJ4{|S!z&7XgM{pX)6 z|NOH+j{nGPAXk5AHe~hXzdthTCn`Gw5uT6XfZ(f0l9H@n@`wy@KK6~37@u%67@0i2A9dH_B_G9paK43=R z^vTS^C`?kT$t^6Bd239sx+*F zD*T}Qx~8VJ5841UT)wk~(Z%yw*nxp`NRV|I0#e^#>kyJf`KNt|Wx)@U|IkqD8qVDX zseNRab=j}r2n)_6->hSS97uu9BdqB#(b@AxSdspTr|z@nAOzg5&Z=v@SA3)O;+ zQ^eD7MSU1Q31P3Re7u?Ek~5na&p`biE>AbJ9`-Hi6m6BrVKEl!hSF=lmcY`r80!-+ z0^1X7(f3f&&ERZRs4=N}F zxtD4)>K6Bml+)Y6rR!@UR%nfTF3ii#!&vm6Z_sl<0D|b0`M+IF;5QU?*OSu(i&BNdV}J=k&qw* ztyq4aqJ-6I02^Vwk)lj*s7O}I!#OGuXtr*FDu$JAds&sG7kP&V($f;2o$%b)(FxvC zB5wKv#g9h9$Cr9m#$O=CE535W`!xpv*S5Sr{uX{S3EVem@K+?h&p@DUr617WTbmJ9 z+$T~l9A<|1oZj)Tczo023;sgDpD6du^1b5=fFuk>o&S1e=l|J5aKC6q<&7!K%g&oH zB6y=?xSM7>hWqv>YJ-@+NX&=N_XZ=Lo>|<>)#OMMIcf`Q;?=9=_X@fEEvuRQ8q9`~ z3)BvY3*;9;rwbI;l6Mkglm#G@7o(;}6c31mKYUZH2+~BdU#CF|aSr^Add74k8g_eXtCLVR=aBaRRkSX%wK^i;3i{TC5;?+V9FZ&}=54k)xT ueKPdJc4WA_&}tC*H-ONt&@YPA>9vXsMvSnD{UzK(x8@TZI4V#Yi((% zP|;lEy#(o(qNOOYYv@;rCB@g$Q2Cv6?t2gazfXJbyqPm+&VJ6!oqY6D>_RyWderQ?JAj2{c@|CvFjrwon5*NG zRoHu23v)eQve`Q!%j5c+LA$Uq4|5${_O-Vu&6zCDwK3ye{GF+X%i;~9oU5*!3b<}( zhGn^B(*PKHZJP>2tvw3O+pU4Vn`?8RiG;Y^>(+bcx+Z;<>+Q|z_%*}TkTlnT_4fc< zwI^X&BR2d)yp%xi^h(WQVd+f1C{5xQY+&^7dzWj=YC6 z5nP^XV#c|~9{V@`M$Y~@_6;-(2Wa70yNB*?olt0B5t1 zUsRqv@tPIq`sO@J2u13eeBoDU9=)Q6XT5td6PLR^1H>713DBchTxTyK9-@Bd<+0iV zB}2LIs`bZ^a)%7f3NpF6-TOX}wepYlp@KgFldMNYw`rKix8K5E}WIzNXBLq3xo(Pr-VwVl8vmbCAL?{2L&OlAbz3q-> zQ;>KhM|O&(c}J^i027Naqzl^}$38?Lv_9FTmV+E8l-6P!vA?_-Zs*o!(ITj1eDcI` zgT@XYHFj9dc*%OQ1X($eeF8uzHQSxYS|OQ)scZxG8MJX}+ueZ8#3h&xZg6_46)UGS zW*>T)4`@QnI~ba<>BtZHVYk){T@|qHfEV&+d)lynlkCt>yLMvhZ~)v+YMrT#E8FhQ zY-unIbnnZ+o=^}{c4al?%l%lgv!?Ct$I2l=Xc)5aT{aW(ppSkqfK7uo8Es$^5e8_0 z|0GriStIE^8BBt~g>Cl`)=stfr;1=j3M;Y$MzY_TygQ0D@S$8a-Kr~h2U(S+`=DMf zARgk9MX^a{j3y{FGkJbAL#IR{>~F`gS$?&Bl_=um$nk8JclC^9M!rJ=Pi8fNDzc%6 z6WRBO0Q)^DECHz{_DWM7YH45+tAoZ>G>zrK3s<&1(^+Md`R!PvhI}#)g-r+#rn8@+ z`=xD0jP0J!l98hBgGIP(wE*Qxa?e=EzQrc#W+6x-_`ISjgA`>Di{crqP?lXvY;~Uo zvOTwywFyjXmc}EjHfFrlO)lQYqU4b*Rz|+FjMW1?=*V*AGSmW9jM4@;V=w)z?47{y2-6~AU0#tUS}Zo8hD6fDsS2X0^kH5h?N z;)WLq=iWvZCwFXSlc58Q4xEVWr6iMWp=P+u-NM?3S5i{A=q=8RUE(t(MNATn#0#?G zR@TV%V%u2RZ7X|07Kd6v_Oh+)OVvRYCfonOo&XZa8?sL}`y0ARIm2xBg&86k2&#s{ zUg7P;K&8gw*TUt+J#2^~9gRD4SK~ciWR)LhMSCNaquqr+``PLU!75RIJ zWg-T{F3o4fsAn>BGl|E^`2B1;&Oi;j_p>;(9`d*chZ$rUlA`T7!p7sYI>*>v90yaA zlMaAnMzPs!13RK5^DthO*XJgGo%iAI z^QC+<{|~<;6j4Pq7G1?iFj56Hq3tX3e%1pmi>cKMTF-PxyDLnQX4bTeFbJj1?%cDVr{&6jKGv9jh_mgX3N z?K#U9S}^#=e_1}lT%p(E3p`w2yF&is1h+rg1&m1~mCRf00=7{*<42?8uLS|Wc#&JO zc#9fnmlm`C(Bc?iPu*d;08EyebC+$v0rJ8*_nAL$H+sO{gMN%iw&wvG3P5C{jY=ql zgRFmYjLh~tVja{namqXe{#&K|pd4Bj$_IgkM#?J7d5B!}$kcyW$zKCv$-(dH9PJ(K z({>v?RCH3bo>&`&%w)a^br1UKM!3)`34+7hKrAjaQtd*CKsuB=|bWarD zgfWVI_QP7Zg0o~)ZJti6VT$78NP-wnTPE-ZX$<-BiAgjz@-y;d(l|Q zl}0_@S-w?|zYRiiEJyxWk2hyOWxvF$VW^-y(5NxTWC{8ONjnM6(}a&iqPyP$8*0{q zZ}f`vVGE8y7{zCMTJpiDEcAcVWyZdLikaEv; z;=e#QhFd$gGk*j5B3`1pk`yUF-s#Gzj-x`L8}AL(3+Xj(F!s~BOM8F{D-5A^DLQ(F z@ZKm$3M`S`VYKUb8YNCmDAQzxWb8mpwkMf)@>xrzRE}QYteonsOg1@*clGKoOh!#3 z@<>eu*Fo$Rra!axblw6Pq3{qggCFv>Z_Pl)i6+}KlXvoKy3ztahqCEJAU0?r{{kT~ zoO>2J3zO`kGH_vHzo3By>q=sx8WceuNpK|BsFi#t%%=k7#!8L_mLrx`ybIY5=D>ut zd@Br)ydxK*0M`){w{;7&v5xmdNMbQ&J)tAn-C0kx;w;;TltbuuTbc3tt1MiO`eZ!n&Vt<6~fu>VwEEz8Kn$Wo7XN2#BR)QZ`@WZE7~h zSmrQf^F>5KTd?~*nlx#K8dBH|_-;~xXi{bLjgNQAqO^Ved;f0e3A7X>2QR^?aa9(#c`Wj)wPkg8N9z(Rl0jdZi0tFbaBP&m!=+L4H)sD&@SV2bLtteI?G1x9sNqp%g z%=T0g&9Uksjh2aa1`x;-Ee=5coEqZ4@FEqY#Rx2l$d3DTfjBfiR)Bsen7$h)cA=x8 z5M}LJ0)r0$KCUHTx9J4j+F~dYL@wW`@l%*4)iuarO$6jH4D7b236w-W1V(eQ)`y@u zhy?m6&qcl@V&%&%#RSAizTQ%NgQhdQoj`LUj@q>s>zv8*aeE}hyDR7*ngqg+dp*Q< z7_Nqaw5O;Grfpw;i{6et>+}{C$=-2m+xKm;5?3etEz{30D%;afxG)?+Lc_%W(n?61 z;X@qLp$*~?(FP_WYcgzvfOzSMV}w|QBBNm2r$20j=8h(ANN#1u&;kx;V?+~FQX$4W zD@9<<;*=LDq6v|NRe?E4tik0eV@?tnM4e2SG(|}7fDD)_Rzce_V`wAYl=UXe5E#jb zxk=-Im}WBtY$&~UJyU!J3D}Y8s0{K1niVX`ten6t#S%bA5q`)@athLP*;P)PbhJu* zfppQpxV7R_M4Ab;QOG($MIoh%b>bQVkuB|7FP1q1laZeTS?L?Zn=pn3X~UUQ4`hG< zN9+M3`Y=;Wfexx%0=Edv11L?~vqk)jwn5qX%O6NkP=`VG*alYgLAIFc)sJb@m^aM` z+^JF~>a~mdNjw2LBtu67IpS5cBqAYrKT5+fr~RTiY6j|Rh+~HYCU%FCL*h521SW*b z^+&}fXj8sPJtnZa!X2r6<2XsiVe+`>K_UbB$;dq*f)7ukr+8hq;2B>o5N*vMzE)9L zD@Al?t=LUF_;)cEESPxMSSZ@a+l68PPCg9GpF9uDr(Y74gCG;AX>l{7BdkVdHOaDh zc{yRfFyzmdM1of`!I%B2mY&O^gULl}MXV96v&yw?EbP{QitFgy33t#5$O!(rBd{KU zC<&FtU6BZKqJE=^Ra3^7ijQ!Rs^n6!7dMs^WtOVyR`A1inrfiyk;Dh;>K$lze;@3B z%YCw*14PG@32^%Vr7(mH4`u|vt^>R@#bV!}CmE_z)JkV^)Ln)bb+YBE*=6qYBCCpD35plk}2FDI*5g`t4#++=ka8ax$8 zBh#R0A23lxTcN3qJEE{s)J4D@<%$$_G!j;>J!turDXMqVC$*_6){EXir~Xaek7XKc zs>0-jG_{uxV}}{)x6lRwqoQ%+Y`bTwbS_E?J6;`Xgpsq=*)WR7WwOG0*hqPTsQxwS>J502gxAhjSCH_qkXX6^7?`|7MHenyt3p6`k@27f+8&o$ z8?BB+`*^wPPn|1Ns<|mvR$Z&^btc1>&EB|9O-AGuH>!A|MT#idsBZDeXOp@O>A`;2 z$?Q0}akC0Z+n$}N(n&k%0BbduJGV0J7 zb=+#dN;_#Xz`BPCQQ=_~H?58ij;Ou;NSY>q3v@k(Mn)mx@i9V#;F69m}K8(fk&zK#}3G)5h~)JGWRJ zMaGOfhvs+u4kb(Opo&N>w&$+;BHAaBbo-(DA86A_%%Q(kJdB)NqV9$lQQ75@nhtHU ztLu-{H$W%EVwZm?L{bD8HVfGQ;E8&|u>m>!6D`i3SE}wp1^Q}WLw?9Ks70RX=*%^8 zc+^#Z&@g+VTiG6=;n^asb37FsvX7-*L?W=}?Hr(y!IKzr zn6UZQHf?u^h94K6S-fAU29*h|5gevL9`y#EFs+hbT{o&|Q*m7^>8DoJ(1k(bSd{uO z8@4-2BNw_FtK}msT47$C*2eFEO=g2XznY-Ufl;!iQwiEJ@Q6)`+C(IYbl9T_R1ndQL~LU%iCe^ZliSr0!Ree4PmfTd#x=Fdf5$?+uLh#z7F{06)r^w4Yxt$ zQEzt8K7sbpP$OKXR<}a3U)O#wkI)0OgYY6R$xG7S^$VijaBVM+BQXUtfxWsTNW7Fi z-6J%|aB=kI(Hgn{@fz_4P!>N%o9-}dyT@o4u8Hb`buiX{yfzPjNsNDwN9}-MKw!yQ zTU8BVe<*gD_cWWqsZ@--;9|itc8eNntEpNHljZ&p5rNh4IMexOcb=rJ2JDWaNefnf*n69bgQHF-G&k>u{24K-au`FRD zF5{4NaxsXe!X|AA&KeGpVfQAjE^&t5cDxyk*33lX0B*wN&P7_Ryqc+DG2>$woMwwQ zj84Y zxf(_sYB$;jgjjhR7Ur}e&&ku0U`?{^gnbkr&g|0;<8W&@(}?QI+;52$$(-FegvUFyznhnrz1u(`^vF8+5(6WPR(O= zv`ok7;&j@sk)hd@fr5?+I{E`)El$w!;~ds>If;5-B!;9HUtfpVLz2By-=Paby1RkC z11Tb8bDQaSI75N5ZgU-T1mc83TF3A2PFYf=HIYRoP~4ixLM_|wHhNFA0a8h1BdF~< zb=8+a2PiyTPQs3g-Sh>}fxcwByXn11QfM+UJ%}Q5vf>_u)5(}U_3mEkV2{}wP;~68 zW5A&Bbfd41p~aD0Km9F42ghh=pk!n>2aryYQadL}e+MxUG4aE73}_HPKrz&1dq(K= z83H9NYmU~p!vO6YPE<5wWyBaqFctM)_LDJsdBj9p()fuErip~fiI6FJKax85wXB<} zli#KKRszN+>F||!yM1knJ^~{tL>nRov(wE8cqC5O=}RM+qV1lp2Otpfb7qGAE3`@4 zxij^PKm{57I-%B-r_*te^zNCj7m{f40BYL;{Q*oufx&4w4bUXB0qh>nEJ53&0Pb<= zw@B3J=6TEYBAA4CFYMeEdi5~;kx7!p90}(D`XC?+SCdOKzyjaN($gH6nC$I(4Sy_X znu{u+1t%>7xVm{{RgN9!fg|B0Oc`(Q(N|-eg3MlhAGFB{O#HH4CSQjlipb2(*Wc8< z7#k0REkMTu`X4@EqdA0(kq4=RwisHf8Eeluq`v}tW(Rk{N&P6a(Isqmft~}noU}jx zynY3NaBnmFf}a3;;6;6c0RvzRClMA+J?BsTBB>a^1mqRzr(l4hy!*O-0ePpBh`jsH z3_Q?(_0K3@sDr*r$c>z7kMtA%(d;Ms?>>AvrTWFE_yUZl3k4bps4ZG3B-EfJ5Ncrl zp{|@TqhmlhWuXH9IyXR0@r0Cux5kZqP=KK<$c#Z#@Ylx6Q6>%=HtqxvGJN zK;QR0QN~gK08rUZKCEu6@{#I}HX2f45Nlk6J}94JvTGW1gMAcjZcLaQew!Fn6j6=U zteLUYnE+qOni=Kf|C$-={8*bCb^L1vwJ-{O=)zhW=)1H`D`SNpW_N3&z(2W-an>&+ zcPHa43^<&0Ht1_C@xQyrGc$S`1^yX3`WhGgsQMX~{cHRX~VDFvqcpP~-7;WtmyV893ZtV88dV5shYzlchWT8AS7qvrku8@XWwMqm_Rk;sc|m z?6}0BS`1IoD!UA-Nl>FpjnjToo~$&^`IuU5JT>5wwPaAl>{rHdKjMe$jY~fGZ=O<; zvB|&!i@?7#>Y?~SW>36bvR9Vzv2S?0F~*N=Q8sGFhN=#4x6pK%yUU=Q1(eF99Dh68 zZB+H5jgf8ll66szquM_t-R24eHoiiUi=&w!%C{&v4H7Z|62*Y22s$6FLB zZk;tkrFGoEfbE}T?>cT!GNS#&Z@<7N&WG>m!UTu5V)f2B^ZNr-9i)cVe?N>xd{>yP;W+g7d;zJkVa=&|F#$X~V+kL|xcV zj%sgy1RX`~9bR=*SO@bCikKLP3<98{PBiM&Yn@HnFjJM(qARjTR6gozqE|bj>}JwU z2-JvtwFeDQ`$-S;>2YOGa}2Tq`VW_{_BFBoCUySPmw+6*=x5^HSBK!&2awW8K9hO@ zUA2?I9kCOb|vzk&gh>)iEFjMPjaxjKqXF(;sP z)SYHhO^G)B&op9^svysFGli@toy1K1{Wb&noA2Wf=$LP&2K+x4AP1$v1;}w3=7+d6 zTGc-pgcWsTyQTRavIyV0(2Pm6ksl-0RfH84V!KzFcab|9JxRJ=i{SRR>&)4Hw%e>X z*OTDF<&*X1MjYN?_ED#^p-Q5XqA2^6Q%aHYm<6*JS$)=)^=4nNop3E^y7GN^xRLzp8Khe1TJFy3nz}4IBIC$|G%rB+>{BV%1rQ~S@`%DJ_g{UGd`N)BL*L# z_$b{Bj~nQhipRuk)4S&dtIT%1p0;MTiSgSB!n^$avUC>(O2_GQ z%sps(UPr(SlDXzk?7)2tBv#Cbj%VdL=1UvKhS#8EFWT$)5JlyD^A33cx<>v%6N?+i zH4d2{&;qnCIs!85f7o1uqtmn(08d^^YVn2cSSpaViSG@w+Pj>iKb32QIzU+$dCkW2n5>QlP za)Y4>T^iI+m@s4lt1bV$X4b%)C8_do0jn0mGyFFcp>vqL{XJYKJ+&UL#SSbo8>^xh zUO}m7Z!a>xzyb!c`gVAcGC(w8vQDvyr4h1k=M11OW>o;fvQt$OuxEVbMJ1{Eu3%2e-bn6~sok6lFnYAou zS@46D(m&LCh_Htt=Z^}r?)kMIVZnjp=jAPs18KEpgp~$>&W1L^iu40st6-%g2HcCz zsc6mewWBIgKadE!;!0Kxv|CIL(!9)LWtHkyTj+o~md=eE;~?7Z?&?+-Xj3itVzf06 z`w_Dz+G-oZPAdY!vKXfn@d5lQ)bAbS+chkgOpdYI`5=Fau`u1}W;7ZOpPc5lwEAc;0-q-nL2NdhwKPCk zl4tknV8x>ek+7F_vWl^d*91QAZ2dqiBlQit!pie{SO*X>`ffo4e6skor?nRm(f8rV zkKqn~dN1psS1}`dTQ_hF6W_?UEjnGLsaxN%a8FJ&N6Z7f?!MM4#EaQ4xu5kZ(2AqXrey-E+~M zdz3+NQ2syOLsm?&THo%MZnhpqtV4otx2$k`}DIH}U=PV|ax(7aK zf40;sX +#include +#if (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2) +#include +#include +#else +#include +#endif + +void calculateNormalsAKNN(std::vector &normals,vector &points, int k, + const double _rPos[3] ); +void calculateNormalsAdaptiveAKNN(vector &normals,vector &points, + int kmin, int kmax, const double _rPos[3]); +void calculateNormalsPANORAMA(vector &normals, + vector &points, + vector< vector< vector< cv::Vec3f > > > extendedMap, + const double _rPos[3]); +// TODO should be exported to separate library +/* + * retrieve a cv::Mat with x,y,z,r from a scan object + * functionality borrowed from scan_cv::convertScanToMat but this function + * does not allow a scanserver to be used, prints to stdout and can only + * handle a single scan + */ +static inline cv::Mat scan2mat(Scan *source) +{ + DataXYZ xyz = source->get("xyz"); + + DataReflectance xyz_reflectance = source->get("reflectance"); + + unsigned int nPoints = xyz.size(); + cv::Mat scan(nPoints,1,CV_32FC(4)); + scan = cv::Scalar::all(0); + + cv::MatIterator_ it; + + it = scan.begin(); + for(unsigned int i = 0; i < nPoints; i++){ + float x, y, z, reflectance; + x = xyz[i][0]; + y = xyz[i][1]; + z = xyz[i][2]; + if(xyz_reflectance.size() != 0) + { + reflectance = xyz_reflectance[i]; + + //normalize the reflectance + reflectance += 32; + reflectance /= 64; + reflectance -= 0.2; + reflectance /= 0.3; + if (reflectance < 0) reflectance = 0; + if (reflectance > 1) reflectance = 1; + } + + (*it)[0] = x; + (*it)[1] = y; + (*it)[2] = z; + if(xyz_reflectance.size() != 0) + (*it)[3] = reflectance; + else + (*it)[3] = 0; + + ++it; + } + return scan; +} +// TODO should be exported to separate library +/* + * convert a matrix of float values (range image) to a matrix of unsigned + * eight bit characters using different techniques + */ +static inline cv::Mat float2uchar(cv::Mat &source, bool logarithm, float cutoff) +{ + cv::Mat result(source.size(), CV_8U, cv::Scalar::all(0)); + float max = 0; + // find maximum value + if (cutoff == 0.0) { + // without cutoff, just iterate through all values to find the largest + for (cv::MatIterator_ it = source.begin(); + it != source.end(); ++it) { + float val = *it; + if (val > max) { + max = val; + } + } + } else { + // when a cutoff is specified, sort all the points by value and then + // specify the max so that values are larger than it + vector sorted(source.cols*source.rows); + int i = 0; + for (cv::MatIterator_ it = source.begin(); + it != source.end(); ++it, ++i) { + sorted[i] = *it; + } + std::sort(sorted.begin(), sorted.end()); + max = sorted[(int)(source.cols*source.rows*(1.0-cutoff))]; + cout << "A cutoff of " << cutoff << " resulted in a max value of " << max << endl; + } + + cv::MatIterator_ src = source.begin(); + cv::MatIterator_ dst = result.begin(); + cv::MatIterator_ end = source.end(); + if (logarithm) { + // stretch values from 0 to max logarithmically over 0 to 255 + // using the logarithm allows to represent smaller values with more + // precision and larger values with less + max = log(max+1); + for (; src != end; ++src, ++dst) { + float val = (log(*src+1)*255.0)/max; + if (val > 255) + *dst = 255; + else + *dst = (uchar)val; + } + } else { + // stretch values from 0 to max linearly over 0 to 255 + for (; src != end; ++src, ++dst) { + float val = (*src*255.0)/max; + if (val > 255) + *dst = 255; + else + *dst = (uchar)val; + } + } + return result; +} + + + +#endif // NORMALS_H diff --git a/include/slam6d/pairingMode.h b/include/slam6d/pairingMode.h new file mode 100644 index 0000000..ab9f970 --- /dev/null +++ b/include/slam6d/pairingMode.h @@ -0,0 +1,10 @@ +#ifndef __PAIRINGMODE_H__ +#define __PAIRINGMODE_H__ + +enum PairingMode { + CLOSEST_POINT, + CLOSEST_POINT_ALONG_NORMAL, + CLOSEST_PLANE +}; + +#endif // PAIRINGMODE_H diff --git a/src/normals/calc_normals.cc b/src/normals/calc_normals.cc new file mode 100644 index 0000000..1a88e9e --- /dev/null +++ b/src/normals/calc_normals.cc @@ -0,0 +1,275 @@ +/** + * + * Copyright (C) Jacobs University Bremen + * + * @author Vaibhav Kumar Mehta + * @file calc_normals.cc + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include "slam6d/fbr/panorama.h" +#include + +#include + + +#ifdef _MSC_VER +#define strcasecmp _stricmp +#define strncasecmp _strnicmp +#else +#include +#endif + +namespace po = boost::program_options; +using namespace std; + +enum normal_method {AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST}; + +/* + * validates normal calculation method specification + */ +void validate(boost::any& v, const std::vector& values, + normal_method*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + if(strcasecmp(arg.c_str(), "AKNN") == 0) v = AKNN; + else if(strcasecmp(arg.c_str(), "ADAPTIVE_AKNN") == 0) v = ADAPTIVE_AKNN; + else if(strcasecmp(arg.c_str(), "PANORAMA") == 0) v = PANORAMA; + else if(strcasecmp(arg.c_str(), "PANORAMA_FAST") == 0) v = PANORAMA_FAST; + else throw std::runtime_error(std::string("normal calculation method ") + arg + std::string(" is unknown")); +} + +/// validate IO types +void validate(boost::any& v, const std::vector& values, + IOType*, int) { + if (values.size() == 0) + throw std::runtime_error("Invalid model specification"); + string arg = values.at(0); + try { + v = formatname_to_io_type(arg.c_str()); + } catch (...) { // runtime_error + throw std::runtime_error("Format " + arg + " unknown."); + } +} + +/// Parse commandline options +void parse_options(int argc, char **argv, int &start, int &end, bool &scanserver, int &max_dist, int &min_dist, string &dir, + IOType &iotype, int &k1, int &k2, normal_method &ntype,int &width,int &height) +{ + /// ---------------------------------- + /// set up program commandline options + /// ---------------------------------- + po::options_description cmd_options("Usage: calculateNormals where options are (default values in brackets)"); + cmd_options.add_options() + ("help,?", "Display this help message") + ("start,s", po::value(&start)->default_value(0), "Start at scan number ") + ("end,e", po::value(&end)->default_value(-1), "Stop at scan number ") + ("scanserver,S", po::value(&scanserver)->default_value(false), "Use the scanserver as an input method") + ("format,f", po::value(&iotype)->default_value(UOS), + "using shared library for input. (chose format from [uos|uosr|uos_map|" + "uos_rgb|uos_frames|uos_map_frames|old|rts|rts_map|ifp|" + "riegl_txt|riegl_rgb|riegl_bin|zahn|ply])") + ("max,M", po::value(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than 'units") + ("min,m", po::value(&min_dist)->default_value(-1),"neglegt all data points with a distance smaller than 'units") + ("normal,g", po::value(&ntype)->default_value(AKNN), "normal calculation method " + "(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)") + ("K1,k", po::value(&k1)->default_value(20), " value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation") + ("K2,K", po::value(&k2)->default_value(20), " value of Kmax for k-adaptation") + ("width,w", po::value(&width)->default_value(1280),"width of panorama image") + ("height,h", po::value(&height)->default_value(960),"height of panorama image") + ; + + po::options_description hidden("Hidden options"); + hidden.add_options() + ("input-dir", po::value(&dir), "input dir"); + + po::positional_options_description pd; + pd.add("input-dir", 1); + + po::options_description all; + all.add(cmd_options).add(hidden); + + po::variables_map vmap; + po::store(po::command_line_parser(argc, argv).options(all).positional(pd).run(), vmap); + po::notify(vmap); + + if (vmap.count("help")) { + cout << cmd_options << endl << endl; + cout << "SAMPLE COMMAND FOR CALCULATING NORMALS" << endl; + cout << " bin/normals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" < &points, vector &normals, int scanNumber) +{ + string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d"; + ofstream normptsout(ofilename.c_str()); + + for (size_t i=0; i::iterator it = Scan::allScans.begin(); + int scanNumber = 0; + + for( ; it != Scan::allScans.end(); ++it) { + Scan* scan = *it; + + // apply optional filtering + scan->setRangeFilter(max_dist, min_dist); + + const double* rPos = scan->get_rPos(); + const double* rPosTheta = scan->get_rPosTheta(); + + /// read scan into points + DataXYZ xyz(scan->get("xyz")); + vector points; + points.reserve(xyz.size()); + vector normals; + normals.reserve(xyz.size()); + + for(unsigned int j = 0; j < xyz.size(); j++) { + points.push_back(Point(xyz[j][0], xyz[j][1], xyz[j][2])); + } + + if(ntype == AKNN) + calculateNormalsAKNN(normals,points, k1, rPos); + else if(ntype == ADAPTIVE_AKNN) + calculateNormalsAdaptiveAKNN(normals,points, k1, k2, rPos); + else + { + // create panorama + fbr::panorama fPanorama(width, height, fbr::EQUIRECTANGULAR, 1, 0, fbr::EXTENDED); + fPanorama.createPanorama(scan2mat(scan)); + + // the range image has to be converted from float to uchar + img = fPanorama.getRangeImage(); + img = float2uchar(img, 0, 0.0); + + if(ntype == PANORAMA) + calculateNormalsPANORAMA(normals,points,fPanorama.getExtendedMap(), rPos); + else if(ntype == PANORAMA_FAST) + cout << "PANORAMA_FAST is not working yet" << endl; + // calculateNormalsFAST(normals,points,img,fPanorama.getExtendedMap()); + } + + // pose file (repeated for the number of segments + writePoseFiles(normdir, rPos, rPosTheta, scanNumber); + // scan files for all segments + writeScanFiles(normdir, points,normals,scanNumber); + + scanNumber++; + } + + // shutdown everything + if (scanserver) + ClientInterface::destroy(); + + Scan::closeDirectory(); + + cout << "Normal program end" << endl; + + return 0; +}