SSF_Core.cpp
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <stephan dot weiss at ieee dot org>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #include <ssf_core/SSF_Core.h>
00033 #include "calcQ.h"
00034 #include <ssf_core/eigen_utils.h>
00035 
00036 namespace ssf_core
00037 {
00038 
00039 SSF_Core::SSF_Core()
00040 {
00041   initialized_ = false;
00042   predictionMade_ = false;
00043 
00045   ros::NodeHandle nh("ssf_core");
00046   ros::NodeHandle pnh("~");
00047 
00048   pubState_ = nh.advertise<sensor_fusion_comm::DoubleArrayStamped> ("state_out", 1);
00049   pubCorrect_ = nh.advertise<sensor_fusion_comm::ExtEkf> ("correction", 1);
00050   pubPose_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped> ("pose", 1);
00051   pubPoseCrtl_ = nh.advertise<sensor_fusion_comm::ExtState> ("ext_state", 1);
00052   msgState_.data.resize(nFullState_, 0);
00053 
00054   subImu_ = nh.subscribe("imu_state_input", 1 /*N_STATE_BUFFER*/, &SSF_Core::imuCallback, this);
00055   subState_ = nh.subscribe("hl_state_input", 1 /*N_STATE_BUFFER*/, &SSF_Core::stateCallback, this);
00056 
00057   msgCorrect_.state.resize(HLI_EKF_STATE_SIZE, 0);
00058   hl_state_buf_.state.resize(HLI_EKF_STATE_SIZE, 0);
00059 
00060   qvw_inittimer_ = 1;
00061 
00062   pnh.param("data_playback", data_playback_, false);
00063   reconfServer_ = new ReconfigureServer(ros::NodeHandle("~"));
00064   ReconfigureServer::CallbackType f = boost::bind(&SSF_Core::Config, this, _1, _2);
00065   reconfServer_->setCallback(f);
00066   //register dyn config list
00067   registerCallback(&SSF_Core::DynConfig, this);
00068 }
00069 
00070 SSF_Core::~SSF_Core()
00071 {
00072   delete reconfServer_;
00073 }
00074 
00075 void SSF_Core::initialize(const Eigen::Matrix<double, 3, 1> & p, const Eigen::Matrix<double, 3, 1> & v,
00076                           const Eigen::Quaternion<double> & q, const Eigen::Matrix<double, 3, 1> & b_w,
00077                           const Eigen::Matrix<double, 3, 1> & b_a, const double & L,
00078                           const Eigen::Quaternion<double> & q_wv, const Eigen::Matrix<double, N_STATE, N_STATE> & P,
00079                           const Eigen::Matrix<double, 3, 1> & w_m, const Eigen::Matrix<double, 3, 1> & a_m,
00080                           const Eigen::Matrix<double, 3, 1> & g, const Eigen::Quaternion<double> & q_ci,
00081                           const Eigen::Matrix<double, 3, 1> & p_ci)
00082 {
00083   initialized_ = false;
00084   predictionMade_ = false;
00085   qvw_inittimer_ = 1;
00086 
00087   // init state buffer
00088   for (int i = 0; i < N_STATE_BUFFER; i++)
00089   {
00090     StateBuffer_[i].reset();
00091   }
00092 
00093   idx_state_ = 0;
00094   idx_P_ = 0;
00095   idx_time_ = 0;
00096 
00097   State & state = StateBuffer_[idx_state_];
00098   state.p_ = p;
00099   state.v_ = v;
00100   state.q_ = q;
00101   state.b_w_ = b_w;
00102   state.b_a_ = b_a;
00103   state.L_ = L;
00104   state.q_wv_ = q_wv;
00105   state.q_ci_ = q_ci;
00106   state.p_ci_ = p_ci;
00107   state.w_m_ = w_m;
00108   state.q_int_ = state.q_wv_;
00109   state.a_m_ = a_m;
00110   state.time_ = ros::Time::now().toSec();
00111 
00112   if (P.maxCoeff() == 0 && P.minCoeff() == 0)
00113     StateBuffer_[idx_P_].P_ <<
00114         0.016580786012789, 0.012199934386656, -0.001458808893504, 0.021111179657363, 0.007427567799788, 0.000037801439852, 0.001171469788518, -0.001169015812942, 0.000103349776558, -0.000003813309102, 0.000015542937454, -0.000004252270155, -0.000344432741256, -0.000188322508425, -0.000003798930056, 0.002878474013131, 0.000479648737527, 0.000160244196007, 0.000012449379372, -0.000025211583296, -0.000029240408089, -0.000001069329869, -0.001271299967766, -0.000133670678392, -0.003059838896447
00115       , 0.012906597122666, 0.050841902184280, -0.001973897835999, 0.017928487134657, 0.043154792703685, 0.000622902345606, 0.002031938336114, 0.000401913571459, -0.000231214341523, -0.000016591523613, 0.000011431341737, 0.000007932426867, 0.000311267088246, -0.000201092426841, 0.000004838759439, 0.008371265702599, -0.000186683528686, 0.000139783403254, 0.000070116051011, -0.000021128179249, -0.000028597234778, -0.000006006222525, -0.002966959059502, 0.000313165520973, 0.003179854597069
00116       , -0.001345477564898, -0.000886479514041, 0.014171550800995, -0.002720150074738, 0.005673098074032, 0.007935105430084, 0.000687618072508, 0.000684952051662, 0.000022000355078, -0.000008608300759, -0.000000799656033, 0.000001107610267, -0.000106383032603, -0.000356814673233, -0.000068763009837, -0.000051146093497, -0.000091362447823, 0.000293945574578, -0.000256092019589, 0.000042269002771, -0.000009567778418, -0.000017167287470, 0.004592386869817, -0.001581055638926, 0.000227387610329
00117       , 0.020963436713918, 0.016241565921214, -0.002606622877434, 0.043695944809847, 0.008282523689966, -0.001656117837207, 0.001638402584126, -0.002060006975745, -0.001362992588971, -0.000001331527123, 0.000032032914797, 0.000004134961242, 0.000341541553429, -0.000100600014193, 0.000025055557965, 0.003723777310732, -0.000161259841873, 0.000175908029926, -0.000010843973378, -0.000001022919132, -0.000020982262562, -0.000009716850289, -0.002231080476166, -0.001033766890345, -0.003628168927273
00118       , 0.009314922877817, 0.046059780658109, 0.003565024589881, 0.015262116382857, 0.065035219304194, -0.001635353752413, 0.002492076189539, 0.001255538625264, -0.000034886338628, -0.000029672138211, 0.000006695719137, 0.000006779584634, 0.000273857318856, 0.000241559075524, 0.000026819562998, 0.007341077421410, -0.000245364703147, -0.000214640089519, 0.000072765069578, -0.000031941424035, 0.000014164172022, -0.000014177340183, -0.000530959567309, 0.000080230949640, 0.003376885297505
00119       , -0.000029025742686, 0.000535037190485, 0.007958782884182, -0.001871298319530, -0.002083832757411, 0.012983170487598, 0.000132746916981, 0.000083483650298, 0.000020140288935, -0.000001280987614, 0.000000838029756, -0.000000023238638, -0.000309256650920, 0.000094250769772, -0.000143135502707, 0.000262797080980, 0.000133734202454, 0.000025809353285, 0.000051787574678, 0.000002954414724, -0.000012648552708, -0.000004097271489, 0.002381975267107, -0.001036906319084, 0.000115868771739
00120       , 0.001237915701080, 0.002441754382058, 0.000642141528976, 0.001714303831639, 0.003652445463202, 0.000133021899909, 0.000491964329936, 0.000029132708361, 0.000054571029310, -0.000003531797659, 0.000002108308557, -0.000000655503604, -0.000036221301269, -0.000080404390258, -0.000002011184920, 0.000409618760249, 0.000006455600111, 0.000037893047554, 0.000004332215700, -0.000003727533693, 0.000000308858737, -0.000004128771100, 0.000121407327690, -0.000116077155506, -0.000044599164311
00121       , -0.001129210933568, 0.000810737713225, 0.000687013243217, -0.002320565048774, 0.001923423915051, 0.000083505758388, 0.000045906211371, 0.000464144924949, -0.000074174151652, -0.000001593433385, -0.000002820148135, 0.000001999456261, 0.000068256370057, -0.000050158974131, -0.000000228078959, 0.000046796063511, -0.000043197112362, 0.000007902785285, 0.000000020609692, 0.000001805172252, 0.000002146994103, 0.000005750401157, 0.000309103513087, 0.000176510147723, 0.000423690330719
00122       , 0.000118011626188, -0.000151939328593, -0.000003895302246, -0.001370909458095, 0.000050912424428, 0.000014452281684, 0.000048567151385, -0.000077773340951, 0.000550829253488, -0.000001499983629, -0.000001785224358, -0.000005364537487, 0.000036601273545, 0.000003384325422, -0.000000535444414, -0.000032994187143, -0.000004973649389, -0.000005428744590, 0.000002850997192, -0.000006378420798, -0.000000001181394, -0.000014301726522, 0.000038455607205, 0.000110350938971, -0.000142528866262
00123       , -0.000005270401860, -0.000021814853820, -0.000010366987197, -0.000002004330853, -0.000038399333509, -0.000001674413901, -0.000004404646641, -0.000002139516677, -0.000001756665835, 0.000002030485308, -0.000000003944807, 0.000000005740984, 0.000000210906625, 0.000000302650227, 0.000000014520529, -0.000003266286919, -0.000000158321546, -0.000000508006293, -0.000000000135721, -0.000000498539464, 0.000000163904942, 0.000000129053161, -0.000003222034988, 0.000000064481380, -0.000001109329693
00124       , 0.000016356223202, 0.000012074093112, -0.000001861055809, 0.000034349032581, 0.000006058258467, 0.000000706161071, 0.000001988651054, -0.000003017460220, -0.000001874017262, -0.000000012182671, 0.000002030455681, -0.000000019800818, 0.000000488355222, 0.000001489016879, 0.000000028100385, 0.000002786101595, -0.000000046249993, 0.000000097139883, 0.000000389735880, -0.000000195417410, -0.000000460262829, 0.000000210319469, -0.000002235134510, -0.000002851445699, -0.000002883729469
00125       , -0.000003154072126, 0.000010432789869, 0.000002047297121, 0.000005626984656, 0.000009913025254, 0.000000398401049, -0.000000326490919, 0.000002058769308, -0.000005291111547, 0.000000001086789, 0.000000001772501, 0.000002006545689, 0.000000044716134, 0.000000414518295, -0.000000135444520, 0.000001531318739, -0.000000211673436, 0.000000405677050, -0.000000796855836, -0.000000266538355, -0.000000133632439, -0.000000338622240, -0.000000150597295, -0.000000563086699, 0.000003088758497
00126       , -0.000348907202366, 0.000314489658858, -0.000097981489533, 0.000332751125893, 0.000276947396796, -0.000311267592250, -0.000035302086269, 0.000070545012901, 0.000036626247889, 0.000000400828580, 0.000000087733422, 0.000000120709451, 0.001026573886639, 0.000013867120528, 0.000031828760993, 0.000009746783802, -0.000458840039830, -0.000019468671558, -0.000043520866307, 0.000007245947338, 0.000003901799711, -0.000004201599512, -0.000047176373840, 0.000119567188660, 0.000003684858444
00127       , -0.000190283000907, -0.000192352300127, -0.000359131551235, -0.000107453347870, 0.000258576553615, 0.000091496162086, -0.000081280254994, -0.000048304910474, 0.000002800928601, 0.000000908905402, 0.000001125333299, 0.000000471832044, 0.000019874619416, 0.001029579153516, 0.000011053406779, 0.000021449316681, 0.000016006639334, -0.000412772225495, 0.000006993477540, 0.000002648721730, 0.000004792699830, -0.000004141354722, -0.000083992422256, 0.000015935718681, -0.000000338251253
00128       , -0.000004368584055, 0.000003124910665, -0.000067807653083, 0.000024474336501, 0.000022105549875, -0.000144033820704, -0.000002164571960, -0.000000083713348, -0.000000674226005, 0.000000019237635, 0.000000025526504, -0.000000057252892, 0.000032366581999, 0.000010736184803, 0.000111095066893, 0.000000615680626, -0.000015341510438, -0.000007700695237, -0.000023026256094, 0.000000638926195, 0.000000960343604, 0.000000817586113, -0.000026575050709, 0.000013993827719, -0.000002316938385
00129       , 0.002973222331656, 0.008292388147295, -0.000211655385599, 0.003951267473552, 0.006718811356807, 0.000277369882917, 0.000349425829596, -0.000014812000602, -0.000045952715508, -0.000002513020002, 0.000002692914948, 0.000001078825296, 0.000009897987444, 0.000020034595279, 0.000000809851157, 0.001554211174363, 0.000023959770856, -0.000037670361809, -0.000009320812655, -0.000004598853139, -0.000006284196194, -0.000000693801636, -0.000469324632849, 0.000014818785588, 0.000277219840791
00130       , 0.000476557664133, -0.000191539372645, -0.000089666716294, -0.000163721235917, -0.000235017605089, 0.000134712473215, 0.000007671308678, -0.000041648250772, -0.000005375975547, 0.000000156986772, 0.000000504340505, -0.000000198574002, -0.000458130878121, 0.000014584188938, -0.000015616513739, 0.000023678958593, 0.000535136781135, -0.000016449781236, 0.000040831795426, -0.000013702650244, -0.000000627377616, -0.000004196881223, 0.000002230529685, -0.000050724631819, -0.000004714535751
00131       , 0.000162219848991, 0.000116427796874, 0.000292562152669, 0.000173404902614, -0.000249216364740, 0.000026816594889, 0.000036367682776, 0.000005763510102, -0.000005320926337, -0.000000071291000, -0.000000112152457, 0.000000334342568, -0.000022684595881, -0.000410859858969, -0.000007890929454, -0.000040454023111, -0.000011131820455, 0.000458907544194, -0.000005285694195, 0.000002246982110, -0.000002222041169, 0.000001951461640, 0.000047488638766, -0.000029510929794, 0.000005816436594
00132       , 0.000010794825884, 0.000058045653749, -0.000260506684499, -0.000007544850373, 0.000048451414581, 0.000048500128303, 0.000002555777025, -0.000001118968589, 0.000001725856751, 0.000000113523451, 0.000000356160739, -0.000000287211392, -0.000041197824317, 0.000004749859562, -0.000021745597805, -0.000011794173035, 0.000040317421040, -0.000001104681255, 0.000325476240984, 0.000006084247746, -0.000006253095726, -0.000005627495374, 0.000013663440542, -0.000012536337446, 0.000000477230568
00133       , -0.000028222744852, -0.000029726624789, 0.000042365440829, -0.000004529013669, -0.000041974513687, 0.000002547416367, -0.000004149622895, 0.000001656132079, -0.000006464228083, -0.000000593440587, -0.000000063566120, -0.000000230872869, 0.000007212338790, 0.000002222629345, 0.000000642817161, -0.000006111733946, -0.000013813495990, 0.000002643879751, 0.000005887006479, 0.000020142991502, -0.000000692093175, -0.000000188761575, 0.000017519903352, -0.000002456326732, 0.000001576856355
00134       , -0.000026132063406, -0.000024675067133, -0.000008452766004, -0.000014350608058, 0.000014404004024, -0.000011620075371, 0.000000539065468, 0.000001829895964, -0.000000462890431, 0.000000223093202, -0.000000499925964, -0.000000094710754, 0.000003954308159, 0.000004249241909, 0.000000876422290, -0.000005419924437, -0.000001021458192, -0.000002052781175, -0.000007397128908, -0.000000347703730, 0.000021540076832, 0.000001455562847, 0.000005351749933, 0.000020079632692, 0.000006997090317
00135       , 0.000001606076924, 0.000001031428045, -0.000015843471685, -0.000005357648114, -0.000007152430254, -0.000003359339850, -0.000003466742259, 0.000005980188844, -0.000014512044407, 0.000000136766387, 0.000000188396487, -0.000000299050190, -0.000004280062694, -0.000005018186182, 0.000000751147421, 0.000000382366121, -0.000004319412270, 0.000002858658354, -0.000005774838189, -0.000000199234914, 0.000001477444848, 0.000021955531390, -0.000005912741153, 0.000006848954650, 0.000000718992109
00136       , -0.001250410021685, -0.002465752118803, 0.004640769479530, -0.002397333962665, 0.000543954908379, 0.002370095810071, 0.000159513911164, 0.000327435894035, 0.000051354259180, -0.000002658607585, -0.000001766738193, -0.000000182288648, -0.000049404478395, -0.000084546262756, -0.000026628375388, -0.000398670523051, 0.000000139079122, 0.000048715190023, 0.000014902392001, 0.000017378375266, 0.000005675773452, -0.000005943594846, 0.013030218966816, 0.002362333360404, 0.000426396397327
00137       , -0.000130856879780, 0.000387010914370, -0.001570485481930, -0.001207751008090, 0.000021063199750, -0.001030927710933, -0.000109925957135, 0.000181001368406, 0.000107869867108, 0.000000177851848, -0.000002935702240, -0.000000493441232, 0.000119019560571, 0.000014103264454, 0.000013824858652, 0.000027253599949, -0.000051452899775, -0.000028435304764, -0.000013422029969, -0.000002043413021, 0.000020290127027, 0.000006914337519, 0.002362694187196, 0.016561843614191, 0.000974154946980
00138       , -0.002974278550351, 0.003344054784873, 0.000125156378167, -0.003468124255435, 0.003442635413150, 0.000109148337164, -0.000076026755915, 0.000385370025486, -0.000148952839125, -0.000000760036995, -0.000002603545684, 0.000003064524894, 0.000001812974918, -0.000002381321630, -0.000002469614200, 0.000309057481545, -0.000004492645187, 0.000007689077401, 0.000001009062356, 0.000001877737433, 0.000007317725714, 0.000000467906597, 0.000372138697091, 0.000966188804360, 0.011550623767300;
00139   else
00140     StateBuffer_[idx_P_].P_ = P;
00141 
00142         // constants
00143   g_ = g;
00144 
00145   // buffer for vision failure check
00146   qvw_inittimer_ = 1;
00147   qbuff_ = Eigen::Matrix<double, nBuff_, 4>::Constant(0);
00148 
00149   // init external propagation
00150   msgCorrect_.header.stamp = ros::Time::now();
00151   msgCorrect_.header.seq = 0;
00152   msgCorrect_.angular_velocity.x = 0;
00153   msgCorrect_.angular_velocity.y = 0;
00154   msgCorrect_.angular_velocity.z = 0;
00155   msgCorrect_.linear_acceleration.x = 0;
00156   msgCorrect_.linear_acceleration.y = 0;
00157   msgCorrect_.linear_acceleration.z = 0;
00158   msgCorrect_.state[0] = state.p_[0];
00159   msgCorrect_.state[1] = state.p_[1];
00160   msgCorrect_.state[2] = state.p_[2];
00161   msgCorrect_.state[3] = state.v_[0];
00162   msgCorrect_.state[4] = state.v_[1];
00163   msgCorrect_.state[5] = state.v_[2];
00164   msgCorrect_.state[6] = state.q_.w();
00165   msgCorrect_.state[7] = state.q_.x();
00166   msgCorrect_.state[8] = state.q_.y();
00167   msgCorrect_.state[9] = state.q_.z();
00168   msgCorrect_.state[10] = state.b_w_[0];
00169   msgCorrect_.state[11] = state.b_w_[1];
00170   msgCorrect_.state[12] = state.b_w_[2];
00171   msgCorrect_.state[13] = state.b_a_[0];
00172   msgCorrect_.state[14] = state.b_a_[1];
00173   msgCorrect_.state[15] = state.b_a_[2];
00174   msgCorrect_.flag = sensor_fusion_comm::ExtEkf::initialization;
00175   pubCorrect_.publish(msgCorrect_);
00176 
00177   // increase state pointers
00178   idx_state_++;
00179   idx_P_++;
00180 
00181   initialized_ = true;
00182 }
00183 
00184 
00185 void SSF_Core::imuCallback(const sensor_msgs::ImuConstPtr & msg)
00186 {
00187 
00188   if (!initialized_)
00189     return; // // early abort // //
00190 
00191   StateBuffer_[idx_state_].time_ = msg->header.stamp.toSec();
00192 
00193   static int seq = 0;
00194 
00195   // get inputs
00196   StateBuffer_[idx_state_].a_m_ << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z;
00197   StateBuffer_[idx_state_].w_m_ << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z;
00198 
00199   // remove acc spikes (TODO: find a cleaner way to do this)
00200   static Eigen::Matrix<double, 3, 1> last_am = Eigen::Matrix<double, 3, 1>(0, 0, 0);
00201   if (StateBuffer_[idx_state_].a_m_.norm() > 50)
00202     StateBuffer_[idx_state_].a_m_ = last_am;
00203   else
00204     last_am = StateBuffer_[idx_state_].a_m_;
00205 
00206   if (!predictionMade_)
00207   {
00208     if (fabs(StateBuffer_[(unsigned char)(idx_state_)].time_ - StateBuffer_[(unsigned char)(idx_state_ - 1)].time_) > 5)
00209     {
00210       ROS_WARN_STREAM_THROTTLE(2, "large time-gap re-initializing to last state\n");
00211       StateBuffer_[(unsigned char)(idx_state_ - 1)].time_ = StateBuffer_[(idx_state_)].time_;
00212       return; // // early abort // // (if timegap too big)
00213     }
00214   }
00215 
00216   propagateState(StateBuffer_[idx_state_].time_ - StateBuffer_[(unsigned char)(idx_state_ - 1)].time_);
00217   predictProcessCovariance(StateBuffer_[idx_P_].time_ - StateBuffer_[(unsigned char)(idx_P_ - 1)].time_);
00218 
00219   checkForNumeric((double*)(&StateBuffer_[idx_state_ - 1].p_[0]), 3, "prediction p");
00220 
00221   predictionMade_ = true;
00222 
00223   msgPose_.header.stamp = msg->header.stamp;
00224   msgPose_.header.seq = msg->header.seq;
00225 
00226   StateBuffer_[(unsigned char)(idx_state_ - 1)].toPoseMsg(msgPose_);
00227   pubPose_.publish(msgPose_);
00228 
00229   msgPoseCtrl_.header = msgPose_.header;
00230   StateBuffer_[(unsigned char)(idx_state_ - 1)].toExtStateMsg(msgPoseCtrl_);
00231   pubPoseCrtl_.publish(msgPoseCtrl_);
00232 
00233   seq++;
00234 }
00235 
00236 
00237 void SSF_Core::stateCallback(const sensor_fusion_comm::ExtEkfConstPtr & msg)
00238 {
00239 
00240   if (!initialized_)
00241     return; // // early abort // //
00242 
00243   StateBuffer_[idx_state_].time_ = msg->header.stamp.toSec();
00244 
00245   static int seq = 0;
00246 
00247   // get inputs
00248   StateBuffer_[idx_state_].a_m_ << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z;
00249   StateBuffer_[idx_state_].w_m_ << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z;
00250 
00251   // remove acc spikes (TODO: find a cleaner way to do this)
00252   static Eigen::Matrix<double, 3, 1> last_am = Eigen::Matrix<double, 3, 1>(0, 0, 0);
00253   if (StateBuffer_[idx_state_].a_m_.norm() > 50)
00254     StateBuffer_[idx_state_].a_m_ = last_am;
00255   else
00256     last_am = StateBuffer_[idx_state_].a_m_;
00257 
00258   if (!predictionMade_)
00259   {
00260     if (fabs(StateBuffer_[(idx_state_)].time_ - StateBuffer_[(unsigned char)(idx_state_ - 1)].time_) > 5)
00261     {
00262       ROS_WARN_STREAM_THROTTLE(2, "large time-gap re-initializing to last state\n");
00263       StateBuffer_[(unsigned char)(idx_state_ - 1)].time_ = StateBuffer_[(idx_state_)].time_;
00264       StateBuffer_[(unsigned char)(idx_state_)].time_ = 0;
00265       return; // // early abort // // (if timegap too big)
00266     }
00267   }
00268 
00269   int32_t flag = msg->flag;
00270   if (data_playback_)
00271     flag = sensor_fusion_comm::ExtEkf::ignore_state;
00272 
00273   bool isnumeric = true;
00274   if (flag == sensor_fusion_comm::ExtEkf::current_state)
00275     isnumeric = checkForNumeric(&msg->state[0], 10, "before prediction p,v,q");
00276 
00277   isnumeric = checkForNumeric((double*)(&StateBuffer_[idx_state_].p_[0]), 3, "before prediction p");
00278 
00279   if (flag == sensor_fusion_comm::ExtEkf::current_state && isnumeric) // state propagation is made externally, so we read the actual state
00280   {
00281     StateBuffer_[idx_state_].p_ = Eigen::Matrix<double, 3, 1>(msg->state[0], msg->state[1], msg->state[2]);
00282     StateBuffer_[idx_state_].v_ = Eigen::Matrix<double, 3, 1>(msg->state[3], msg->state[4], msg->state[5]);
00283     StateBuffer_[idx_state_].q_ = Eigen::Quaternion<double>(msg->state[6], msg->state[7], msg->state[8], msg->state[9]);
00284     StateBuffer_[idx_state_].q_.normalize();
00285 
00286     // zero props:
00287     StateBuffer_[idx_state_].b_w_ = StateBuffer_[(unsigned char)(idx_state_ - 1)].b_w_;
00288     StateBuffer_[idx_state_].b_a_ = StateBuffer_[(unsigned char)(idx_state_ - 1)].b_a_;
00289     StateBuffer_[idx_state_].L_ = StateBuffer_[(unsigned char)(idx_state_ - 1)].L_;
00290     StateBuffer_[idx_state_].q_wv_ = StateBuffer_[(unsigned char)(idx_state_ - 1)].q_wv_;
00291     StateBuffer_[idx_state_].q_ci_ = StateBuffer_[(unsigned char)(idx_state_ - 1)].q_ci_;
00292     StateBuffer_[idx_state_].p_ci_ = StateBuffer_[(unsigned char)(idx_state_ - 1)].p_ci_;
00293     idx_state_++;
00294 
00295     hl_state_buf_ = *msg;
00296   }
00297   else if (flag == sensor_fusion_comm::ExtEkf::ignore_state || !isnumeric) // otherwise let's do the state prop. here
00298     propagateState(StateBuffer_[idx_state_].time_ - StateBuffer_[(unsigned char)(idx_state_ - 1)].time_);
00299 
00300   predictProcessCovariance(StateBuffer_[idx_P_].time_ - StateBuffer_[(unsigned char)(idx_P_ - 1)].time_);
00301 
00302   isnumeric = checkForNumeric((double*)(&StateBuffer_[idx_state_ - 1].p_[0]), 3, "prediction p");
00303   isnumeric = checkForNumeric((double*)(&StateBuffer_[idx_state_ - 1].P_(0)), N_STATE * N_STATE, "prediction done P");
00304 
00305   predictionMade_ = true;
00306 
00307   msgPose_.header.stamp = msg->header.stamp;
00308   msgPose_.header.seq = msg->header.seq;
00309 
00310   StateBuffer_[(unsigned char)(idx_state_ - 1)].toPoseMsg(msgPose_);
00311   pubPose_.publish(msgPose_);
00312 
00313   msgPoseCtrl_.header = msgPose_.header;
00314   StateBuffer_[(unsigned char)(idx_state_ - 1)].toExtStateMsg(msgPoseCtrl_);
00315   pubPoseCrtl_.publish(msgPoseCtrl_);
00316 
00317   seq++;
00318 }
00319 
00320 
00321 void SSF_Core::propagateState(const double dt)
00322 {
00323   typedef const Eigen::Matrix<double, 4, 4> ConstMatrix4;
00324   typedef const Eigen::Matrix<double, 3, 1> ConstVector3;
00325   typedef Eigen::Matrix<double, 4, 4> Matrix4;
00326 
00327   // get references to current and previous state
00328   State & cur_state = StateBuffer_[idx_state_];
00329   State & prev_state = StateBuffer_[(unsigned char)(idx_state_ - 1)];
00330 
00331   // zero props:
00332   cur_state.b_w_ = prev_state.b_w_;
00333   cur_state.b_a_ = prev_state.b_a_;
00334   cur_state.L_ = prev_state.L_;
00335   cur_state.q_wv_ = prev_state.q_wv_;
00336   cur_state.q_ci_ = prev_state.q_ci_;
00337   cur_state.p_ci_ = prev_state.p_ci_;
00338 
00339 //  Eigen::Quaternion<double> dq;
00340   Eigen::Matrix<double, 3, 1> dv;
00341   ConstVector3 ew = cur_state.w_m_ - cur_state.b_w_;
00342   ConstVector3 ewold = prev_state.w_m_ - prev_state.b_w_;
00343   ConstVector3 ea = cur_state.a_m_ - cur_state.b_a_;
00344   ConstVector3 eaold = prev_state.a_m_ - prev_state.b_a_;
00345   ConstMatrix4 Omega = omegaMatJPL(ew);
00346   ConstMatrix4 OmegaOld = omegaMatJPL(ewold);
00347   Matrix4 OmegaMean = omegaMatJPL((ew + ewold) / 2);
00348 
00349   // zero order quaternion integration
00350   //    cur_state.q_ = (Eigen::Matrix<double,4,4>::Identity() + 0.5*Omega*dt)*StateBuffer_[(unsigned char)(idx_state_-1)].q_.coeffs();
00351 
00352   // first order quaternion integration, this is kind of costly and may not add a lot to the quality of propagation...
00353   int div = 1;
00354   Matrix4 MatExp;
00355   MatExp.setIdentity();
00356   OmegaMean *= 0.5 * dt;
00357   for (int i = 1; i < 5; i++)
00358   {
00359     div *= i;
00360     MatExp = MatExp + OmegaMean / div;
00361     OmegaMean *= OmegaMean;
00362   }
00363 
00364   // first oder quat integration matrix
00365   ConstMatrix4 quat_int = MatExp + 1.0 / 48.0 * (Omega * OmegaOld - OmegaOld * Omega) * dt * dt;
00366 
00367   // first oder quaternion integration
00368   cur_state.q_.coeffs() = quat_int * prev_state.q_.coeffs();
00369   cur_state.q_.normalize();
00370 
00371   // first oder quaternion integration
00372   cur_state.q_int_.coeffs() = quat_int * prev_state.q_int_.coeffs();
00373   cur_state.q_int_.normalize();
00374 
00375   dv = (cur_state.q_.toRotationMatrix() * ea + prev_state.q_.toRotationMatrix() * eaold) / 2;
00376   cur_state.v_ = prev_state.v_ + (dv - g_) * dt;
00377   cur_state.p_ = prev_state.p_ + ((cur_state.v_ + prev_state.v_) / 2 * dt);
00378   idx_state_++;
00379 }
00380 
00381 
00382 void SSF_Core::predictProcessCovariance(const double dt)
00383 {
00384 
00385   typedef const Eigen::Matrix<double, 3, 3> ConstMatrix3;
00386   typedef const Eigen::Matrix<double, 3, 1> ConstVector3;
00387   typedef Eigen::Vector3d Vector3;
00388 
00389   // noises
00390   ConstVector3 nav = Vector3::Constant(config_.noise_acc /* / sqrt(dt) */);
00391   ConstVector3 nbav = Vector3::Constant(config_.noise_accbias /* * sqrt(dt) */);
00392 
00393   ConstVector3 nwv = Vector3::Constant(config_.noise_gyr /* / sqrt(dt) */);
00394   ConstVector3 nbwv = Vector3::Constant(config_.noise_gyrbias /* * sqrt(dt) */);
00395 
00396   ConstVector3 nqwvv = Eigen::Vector3d::Constant(config_.noise_qwv);
00397   ConstVector3 nqciv = Eigen::Vector3d::Constant(config_.noise_qci);
00398   ConstVector3 npicv = Eigen::Vector3d::Constant(config_.noise_pic);
00399 
00400   // bias corrected IMU readings
00401   ConstVector3 ew = StateBuffer_[idx_P_].w_m_ - StateBuffer_[idx_P_].b_w_;
00402   ConstVector3 ea = StateBuffer_[idx_P_].a_m_ - StateBuffer_[idx_P_].b_a_;
00403 
00404   ConstMatrix3 a_sk = skew(ea);
00405   ConstMatrix3 w_sk = skew(ew);
00406   ConstMatrix3 eye3 = Eigen::Matrix<double, 3, 3>::Identity();
00407 
00408   ConstMatrix3 C_eq = StateBuffer_[idx_P_].q_.toRotationMatrix();
00409 
00410   const double dt_p2_2 = dt * dt * 0.5; // dt^2 / 2
00411   const double dt_p3_6 = dt_p2_2 * dt / 3.0; // dt^3 / 6
00412   const double dt_p4_24 = dt_p3_6 * dt * 0.25; // dt^4 / 24
00413   const double dt_p5_120 = dt_p4_24 * dt * 0.2; // dt^5 / 120
00414 
00415   ConstMatrix3 Ca3 = C_eq * a_sk;
00416   ConstMatrix3 A = Ca3 * (-dt_p2_2 * eye3 + dt_p3_6 * w_sk - dt_p4_24 * w_sk * w_sk);
00417   ConstMatrix3 B = Ca3 * (dt_p3_6 * eye3 - dt_p4_24 * w_sk + dt_p5_120 * w_sk * w_sk);
00418   ConstMatrix3 D = -A;
00419   ConstMatrix3 E = eye3 - dt * w_sk + dt_p2_2 * w_sk * w_sk;
00420   ConstMatrix3 F = -dt * eye3 + dt_p2_2 * w_sk - dt_p3_6 * (w_sk * w_sk);
00421   ConstMatrix3 C = Ca3 * F;
00422 
00423   // discrete error state propagation Matrix Fd according to:
00424   // Stephan Weiss and Roland Siegwart.
00425   // Real-Time Metric State Estimation for Modular Vision-Inertial Systems.
00426   // IEEE International Conference on Robotics and Automation. Shanghai, China, 2011
00427   Fd_.setIdentity();
00428   Fd_.block<3, 3> (0, 3) = dt * eye3;
00429   Fd_.block<3, 3> (0, 6) = A;
00430   Fd_.block<3, 3> (0, 9) = B;
00431   Fd_.block<3, 3> (0, 12) = -C_eq * dt_p2_2;
00432 
00433   Fd_.block<3, 3> (3, 6) = C;
00434   Fd_.block<3, 3> (3, 9) = D;
00435   Fd_.block<3, 3> (3, 12) = -C_eq * dt;
00436 
00437   Fd_.block<3, 3> (6, 6) = E;
00438   Fd_.block<3, 3> (6, 9) = F;
00439 
00440   calc_Q(dt, StateBuffer_[idx_P_].q_, ew, ea, nav, nbav, nwv, nbwv, config_.noise_scale, nqwvv, nqciv, npicv, Qd_);
00441   StateBuffer_[idx_P_].P_ = Fd_ * StateBuffer_[(unsigned char)(idx_P_ - 1)].P_ * Fd_.transpose() + Qd_;
00442 
00443   idx_P_++;
00444 }
00445 
00446 
00447 bool SSF_Core::getStateAtIdx(State* timestate, unsigned char idx)
00448 {
00449   if (!predictionMade_)
00450   {
00451     timestate->time_ = -1;
00452     return false;
00453   }
00454 
00455   *timestate = StateBuffer_[idx];
00456 
00457   return true;
00458 }
00459 
00460 unsigned char SSF_Core::getClosestState(State* timestate, ros::Time tstamp, double delay)
00461 {
00462   if (!predictionMade_)
00463   {
00464     timestate->time_ = -1;
00465     return false;
00466   }
00467 
00468   unsigned char idx = (unsigned char)(idx_state_ - 1);
00469   double timedist = 1e100;
00470   double timenow = tstamp.toSec() - delay - config_.delay;
00471 
00472   while (fabs(timenow - StateBuffer_[idx].time_) < timedist) // timedist decreases continuously until best point reached... then rises again
00473   {
00474     timedist = fabs(timenow - StateBuffer_[idx].time_);
00475     idx--;
00476   }
00477   idx++; // we subtracted one too much before....
00478 
00479   static bool started = false;
00480   if (idx == 1 && !started)
00481     idx = 2;
00482   started = true;
00483 
00484   if (StateBuffer_[idx].time_ == 0)
00485     return false; // // early abort // //  not enough predictions made yet to apply measurement (too far in past)
00486 
00487   propPToIdx(idx); // catch up with covariance propagation if necessary
00488 
00489   *timestate = StateBuffer_[idx];
00490 
00491   return idx;
00492 }
00493 
00494 void SSF_Core::propPToIdx(unsigned char idx)
00495 {
00496   // propagate cov matrix until idx
00497   if (idx<idx_state_ && (idx_P_<=idx || idx_P_>idx_state_))     //need to propagate some covs
00498           while (idx!=(unsigned char)(idx_P_-1))
00499                   predictProcessCovariance(StateBuffer_[idx_P_].time_-StateBuffer_[(unsigned char)(idx_P_-1)].time_);
00500 }
00501 
00502 bool SSF_Core::applyCorrection(unsigned char idx_delaystate, const ErrorState & res_delayed, double fuzzythres)
00503 {
00504   static int seq_m = 0;
00505   if (config_.fixed_scale)
00506   {
00507     correction_(15) = 0; //scale
00508   }
00509 
00510   if (config_.fixed_bias)
00511   {
00512     correction_(9) = 0; //acc bias x
00513     correction_(10) = 0; //acc bias y
00514     correction_(11) = 0; //acc bias z
00515     correction_(12) = 0; //gyro bias x
00516     correction_(13) = 0; //gyro bias y
00517     correction_(14) = 0; //gyro bias z
00518   }
00519 
00520   if (config_.fixed_calib)
00521   {
00522     correction_(19) = 0; //q_ic roll
00523     correction_(20) = 0; //q_ic pitch
00524     correction_(21) = 0; //q_ic yaw
00525     correction_(22) = 0; //p_ci x
00526     correction_(23) = 0; //p_ci y
00527     correction_(24) = 0; //p_ci z
00528   }
00529 
00530   // state update:
00531 
00532   // store old values in case of fuzzy tracking
00533   // TODO: what to do with attitude? augment measurement noise?
00534 
00535   State & delaystate = StateBuffer_[idx_delaystate];
00536 
00537   const Eigen::Matrix<double, 3, 1> buff_bw = delaystate.b_w_;
00538   const Eigen::Matrix<double, 3, 1> buff_ba = delaystate.b_a_;
00539   const double buff_L = delaystate.L_;
00540   const Eigen::Quaternion<double> buff_qwv = delaystate.q_wv_;
00541   const Eigen::Quaternion<double> buff_qci = delaystate.q_ci_;
00542   const Eigen::Matrix<double, 3, 1> buff_pic = delaystate.p_ci_;
00543 
00544   delaystate.p_ = delaystate.p_ + correction_.block<3, 1> (0, 0);
00545   delaystate.v_ = delaystate.v_ + correction_.block<3, 1> (3, 0);
00546   delaystate.b_w_ = delaystate.b_w_ + correction_.block<3, 1> (9, 0);
00547   delaystate.b_a_ = delaystate.b_a_ + correction_.block<3, 1> (12, 0);
00548   delaystate.L_ = delaystate.L_ + correction_(15);
00549   if (delaystate.L_ < 0)
00550   {
00551     ROS_WARN_STREAM_THROTTLE(1,"Negative scale detected: " << delaystate.L_ << ". Correcting to 0.1");
00552     delaystate.L_ = 0.1;
00553   }
00554 
00555   Eigen::Quaternion<double> qbuff_q = quaternionFromSmallAngle(correction_.block<3, 1> (6, 0));
00556   delaystate.q_ = delaystate.q_ * qbuff_q;
00557   delaystate.q_.normalize();
00558 
00559   Eigen::Quaternion<double> qbuff_qwv = quaternionFromSmallAngle(correction_.block<3, 1> (16, 0));
00560   delaystate.q_wv_ = delaystate.q_wv_ * qbuff_qwv;
00561   delaystate.q_wv_.normalize();
00562 
00563   Eigen::Quaternion<double> qbuff_qci = quaternionFromSmallAngle(correction_.block<3, 1> (19, 0));
00564   delaystate.q_ci_ = delaystate.q_ci_ * qbuff_qci;
00565   delaystate.q_ci_.normalize();
00566 
00567   delaystate.p_ci_ = delaystate.p_ci_ + correction_.block<3, 1> (22, 0);
00568 
00569         // update qbuff_ and check for fuzzy tracking
00570   if (qvw_inittimer_ > nBuff_)
00571   {
00572     // should be unit quaternion if no error
00573     Eigen::Quaternion<double> errq = delaystate.q_wv_.conjugate() *
00574         Eigen::Quaternion<double>(
00575             getMedian(qbuff_.block<nBuff_, 1> (0, 3)),
00576             getMedian(qbuff_.block<nBuff_, 1> (0, 0)),
00577             getMedian(qbuff_.block<nBuff_, 1> (0, 1)),
00578             getMedian(qbuff_.block<nBuff_, 1> (0, 2))
00579             );
00580 
00581     if (std::max(errq.vec().maxCoeff(), -errq.vec().minCoeff()) / fabs(errq.w()) * 2 > fuzzythres) // fuzzy tracking (small angle approx)
00582     {
00583       ROS_WARN_STREAM_THROTTLE(1,"fuzzy tracking triggered: " << std::max(errq.vec().maxCoeff(), -errq.vec().minCoeff())/fabs(errq.w())*2 << " limit: " << fuzzythres <<"\n");
00584 
00585       //state_.q_ = buff_q;
00586       delaystate.b_w_ = buff_bw;
00587       delaystate.b_a_ = buff_ba;
00588       delaystate.L_ = buff_L;
00589       delaystate.q_wv_ = buff_qwv;
00590       delaystate.q_ci_ = buff_qci;
00591       delaystate.p_ci_ = buff_pic;
00592       correction_.block<16, 1> (9, 0) = Eigen::Matrix<double, 16, 1>::Zero();
00593       qbuff_q.setIdentity();
00594       qbuff_qwv.setIdentity();
00595       qbuff_qci.setIdentity();
00596     }
00597     else // if tracking ok: update mean and 3sigma of past N q_vw's
00598     {
00599       qbuff_.block<1, 4> (qvw_inittimer_ - nBuff_ - 1, 0) = Eigen::Matrix<double, 1, 4>(delaystate.q_wv_.coeffs());
00600       qvw_inittimer_ = (qvw_inittimer_) % nBuff_ + nBuff_ + 1;
00601     }
00602   }
00603   else // at beginning get mean and 3sigma of past N q_vw's
00604   {
00605     qbuff_.block<1, 4> (qvw_inittimer_ - 1, 0) = Eigen::Matrix<double, 1, 4>(delaystate.q_wv_.coeffs());
00606     qvw_inittimer_++;
00607   }
00608 
00609   // idx fiddeling to ensure correct update until now from the past
00610   idx_time_ = idx_state_;
00611   idx_state_ = idx_delaystate + 1;
00612   idx_P_ = idx_delaystate + 1;
00613 
00614   // propagate state matrix until now
00615   while (idx_state_ != idx_time_)
00616     propagateState(StateBuffer_[idx_state_].time_ - StateBuffer_[(unsigned char)(idx_state_ - 1)].time_);
00617 
00618   checkForNumeric(&correction_[0], HLI_EKF_STATE_SIZE, "update");
00619 
00620   // publish correction for external propagation
00621   msgCorrect_.header.stamp = ros::Time::now();
00622   msgCorrect_.header.seq = seq_m;
00623   msgCorrect_.angular_velocity.x = 0;
00624   msgCorrect_.angular_velocity.y = 0;
00625   msgCorrect_.angular_velocity.z = 0;
00626   msgCorrect_.linear_acceleration.x = 0;
00627   msgCorrect_.linear_acceleration.y = 0;
00628   msgCorrect_.linear_acceleration.z = 0;
00629 
00630   const unsigned char idx = (unsigned char)(idx_state_ - 1);
00631   msgCorrect_.state[0] = StateBuffer_[idx].p_[0] - hl_state_buf_.state[0];
00632   msgCorrect_.state[1] = StateBuffer_[idx].p_[1] - hl_state_buf_.state[1];
00633   msgCorrect_.state[2] = StateBuffer_[idx].p_[2] - hl_state_buf_.state[2];
00634   msgCorrect_.state[3] = StateBuffer_[idx].v_[0] - hl_state_buf_.state[3];
00635   msgCorrect_.state[4] = StateBuffer_[idx].v_[1] - hl_state_buf_.state[4];
00636   msgCorrect_.state[5] = StateBuffer_[idx].v_[2] - hl_state_buf_.state[5];
00637 
00638   Eigen::Quaterniond hl_q(hl_state_buf_.state[6], hl_state_buf_.state[7], hl_state_buf_.state[8], hl_state_buf_.state[9]);
00639   qbuff_q = hl_q.inverse() * StateBuffer_[idx].q_;
00640   msgCorrect_.state[6] = qbuff_q.w();
00641   msgCorrect_.state[7] = qbuff_q.x();
00642   msgCorrect_.state[8] = qbuff_q.y();
00643   msgCorrect_.state[9] = qbuff_q.z();
00644 
00645   msgCorrect_.state[10] = StateBuffer_[idx].b_w_[0] - hl_state_buf_.state[10];
00646   msgCorrect_.state[11] = StateBuffer_[idx].b_w_[1] - hl_state_buf_.state[11];
00647   msgCorrect_.state[12] = StateBuffer_[idx].b_w_[2] - hl_state_buf_.state[12];
00648   msgCorrect_.state[13] = StateBuffer_[idx].b_a_[0] - hl_state_buf_.state[13];
00649   msgCorrect_.state[14] = StateBuffer_[idx].b_a_[1] - hl_state_buf_.state[14];
00650   msgCorrect_.state[15] = StateBuffer_[idx].b_a_[2] - hl_state_buf_.state[15];
00651 
00652   msgCorrect_.flag = sensor_fusion_comm::ExtEkf::state_correction;
00653   pubCorrect_.publish(msgCorrect_);
00654 
00655   // publish state
00656   msgState_.header = msgCorrect_.header;
00657   StateBuffer_[idx].toStateMsg(msgState_);
00658   pubState_.publish(msgState_);
00659   seq_m++;
00660 
00661   return 1;
00662 }
00663 
00664 void SSF_Core::Config(ssf_core::SSF_CoreConfig& config, uint32_t level)
00665 {
00666   for (std::vector<CallbackType>::iterator it = callbacks_.begin(); it != callbacks_.end(); it++)
00667     (*it)(config, level);
00668 }
00669 
00670 void SSF_Core::DynConfig(ssf_core::SSF_CoreConfig& config, uint32_t level)
00671 {
00672   config_ = config;
00673 }
00674 
00675 double SSF_Core::getMedian(const Eigen::Matrix<double, nBuff_, 1> & data)
00676 {
00677   std::vector<double> mediandistvec;
00678   mediandistvec.reserve(nBuff_);
00679   for (int i = 0; i < nBuff_; ++i)
00680     mediandistvec.push_back(data(i));
00681 
00682   if (mediandistvec.size() > 0)
00683   {
00684     std::vector<double>::iterator first = mediandistvec.begin();
00685     std::vector<double>::iterator last = mediandistvec.end();
00686     std::vector<double>::iterator middle = first + std::floor((last - first) / 2);
00687     std::nth_element(first, middle, last); // can specify comparator as optional 4th arg
00688     return *middle;
00689   }
00690   else
00691     return 0;
00692 }
00693 
00694 }; // end namespace


ssf_core
Author(s): Stephan Weiss, Markus Achtelik
autogenerated on Fri Jan 3 2014 11:21:29