LoTTIEstimator.C

Go to the documentation of this file.
00001 /**
00002    \file  Robots/LoBot/tti/LoTTIEstimator.C
00003    \brief This file defines the non-inline member functions and static
00004    data members of the lobot::TTIEstimator class.
00005 */
00006 
00007 // //////////////////////////////////////////////////////////////////// //
00008 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2000-2005   //
00009 // by the University of Southern California (USC) and the iLab at USC.  //
00010 // See http://iLab.usc.edu for information about this project.          //
00011 // //////////////////////////////////////////////////////////////////// //
00012 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00013 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00014 // in Visual Environments, and Applications'' by Christof Koch and      //
00015 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00016 // pending; application number 09/912,225 filed July 23, 2001; see      //
00017 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00018 // //////////////////////////////////////////////////////////////////// //
00019 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00020 //                                                                      //
00021 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00022 // redistribute it and/or modify it under the terms of the GNU General  //
00023 // Public License as published by the Free Software Foundation; either  //
00024 // version 2 of the License, or (at your option) any later version.     //
00025 //                                                                      //
00026 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00027 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00028 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00029 // PURPOSE.  See the GNU General Public License for more details.       //
00030 //                                                                      //
00031 // You should have received a copy of the GNU General Public License    //
00032 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00033 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00034 // Boston, MA 02111-1307 USA.                                           //
00035 // //////////////////////////////////////////////////////////////////// //
00036 //
00037 // Primary maintainer for this file: mviswana usc edu
00038 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Robots/LoBot/tti/LoTTIEstimator.C $
00039 // $Id: LoTTIEstimator.C 14018 2010-09-23 07:10:34Z mviswana $
00040 //
00041 
00042 //------------------------------ HEADERS --------------------------------
00043 
00044 // lobot headers
00045 #include "Robots/LoBot/tti/LoTTIEstimator.H"
00046 #include "Robots/LoBot/config/LoConfigHelpers.H"
00047 
00048 #include "Robots/LoBot/util/LoGL.H"
00049 #include "Robots/LoBot/util/LoString.H"
00050 #include "Robots/LoBot/util/LoMath.H"
00051 #include "Robots/LoBot/misc/LoExcept.H"
00052 #include "Robots/LoBot/misc/singleton.hh"
00053 #include "Robots/LoBot/util/range.hh"
00054 #include "Robots/LoBot/util/triple.hh"
00055 
00056 // OpenGL headers
00057 #ifdef INVT_HAVE_LIBGLU
00058 #include <GL/glu.h>
00059 #endif
00060 
00061 #ifdef INVT_HAVE_LIBGL
00062 #include <GL/gl.h>
00063 #endif
00064 
00065 // Standard C++ headers
00066 #include <iomanip>
00067 #include <sstream>
00068 #include <numeric>
00069 #include <algorithm>
00070 #include <functional>
00071 #include <iterator>
00072 
00073 //----------------------------- NAMESPACE -------------------------------
00074 
00075 namespace lobot {
00076 
00077 //-------------------------- KNOB TWIDDLING -----------------------------
00078 
00079 namespace {
00080 
00081 // Retrieve settings from tti_estimator section of config file
00082 template<typename T>
00083 static inline T conf(const std::string& key, T default_value)
00084 {
00085    return get_conf<T>("tti_estimator", key, default_value) ;
00086 }
00087 
00088 // Overload for retrieving ranges
00089 template<typename T>
00090 static inline range<T> conf(const std::string& key, const range<T>& defval)
00091 {
00092    return get_conf<T>("tti_estimator", key, defval) ;
00093 }
00094 
00095 // To minimize clutter in the LGMD drawable, we render either the TTI
00096 // belief or the actual and predicted times-to-impact or the actual and
00097 // predicted distances. The user picks what should be rendered. This
00098 // enumeration simply names the different possibilities.
00099 enum RenderMode {
00100    RENDER_OFF,
00101    RENDER_BELIEF,
00102    RENDER_TTI,
00103    RENDER_DISTANCE,
00104 } ;
00105 
00106 /// This local class encapsulates various parameters that can be used to
00107 /// tweak different aspects of the Bayesian time-to-impact estimation.
00108 class TTIParams : public singleton<TTIParams> {
00109    /// The LGMD spike rate is a function of an approaching object's
00110    /// time-to-impact. When the object is far away, the LGMD's spike
00111    /// rate will be fairly low. As it approaches, the LGMD starts
00112    /// firing very rapidly. Shortly before impact, the LGMD firing
00113    /// rate reaches a peak and then drops off sharply until impact.
00114    ///
00115    /// The peak described above "partitions" the LGMD firing rate vs.
00116    /// time-to-impact curve into two distinct "phases." We refer to
00117    /// the first phase, wherein the curve rises to its peak, as
00118    /// LOOMING because the object is looming large in the LGMD's field
00119    /// of view. The second phase, we call BLANKING because feedforward
00120    /// inhibition kicks in after the peak to shutdown the LGMD right
00121    /// before impact.
00122    ///
00123    /// To recognize the transition from LOOMING to BLANKING, we
00124    /// monitor the second derivative of the LGMD signal to find the
00125    /// inflection point where the curve becomes concave down (thus
00126    /// indicating a local maximum near the signal's peak). Since the
00127    /// derivatives of a signal can be quite noisy, it would be best to
00128    /// filter the second derivative.
00129    ///
00130    /// This setting specifies the size of the low-pass filter used to
00131    /// filter the second derivative of the LGMD input signal. For
00132    /// example, if this value is 10, then the previous ten readings
00133    /// will be used to smooth the second derivative of the input
00134    /// signal. To disable this filtering (not recommended), use a
00135    /// filter size of one.
00136    int m_sder_filter_size ;
00137 
00138    /// As mentioned above, the LGMD signal is partitioned into two
00139    /// phases, viz., LOOMING and BLANKING. We recognize transitions
00140    /// from LOOMING to BLANKING using the second derivative of the
00141    /// LGMD input signal and a spike rate threshold for the rising
00142    /// portion of the TTI-LGMD curve.
00143    ///
00144    /// To recognize the transition from BLANKING back to LOOMING, we
00145    /// wait for the LGMD signal to fall below some empirically
00146    /// determined threshold firing rate rather than relying on
00147    /// higher-order derivatives of the input signal.
00148    ///
00149    /// These two settings specify the above-mentioned firing rate
00150    /// thresholds for recognizing transitions from LOOMING to BLANKING
00151    /// and then back to LOOMING.
00152    float m_rising_threshold, m_falling_threshold ;
00153 
00154    /// Once we have a time-to-impact estimate for each locust, we can
00155    /// combine that estimate with the robot's current motion to
00156    /// determine the distances to obstacles in each locust's
00157    /// direction, thus, converting a vision sensor to a range sensor.
00158    ///
00159    /// As mentioned earlier, the time-to-impact estimates are made on
00160    /// the basis of a Bayesian state estimation model that works by
00161    /// continually updating a probability distribution that specifies
00162    /// the likelihood of each TTI value. The TTI with the maximum
00163    /// likelihood is used to compute the corresponding distance.
00164    ///
00165    /// However, we only want to use TTI values when their likelihoods
00166    /// are significant. That is, if the maximum likelihood in the
00167    /// entire probability distribution is really low (e.g., 1%), then
00168    /// it might not be a good thing to use that TTI estimate to try
00169    /// and determine the corresponding distance. Instead, we want to
00170    /// wait until the confidence level of the most likely
00171    /// time-to-impact is a reasonably high value before using it.
00172    ///
00173    /// This setting specifies the minimum confidence we expect to see
00174    /// before a TTI estimate will be used to generate distance
00175    /// readings. It should be a number between zero and one.
00176    float m_confidence_threshold ;
00177 
00178    /// This setting specifies the min and max bounds for the distance
00179    /// "readings" computed by the Bayesian time-to-impact estimator.
00180    /// The units for this setting is mm.
00181    range<float> m_distance_range ;
00182 
00183    /// There are three visualization modes for the time-to-impact
00184    /// estimator. In the first mode, the estimator renders its current
00185    /// belief, i.e., the posterior probability distribution computed
00186    /// in each iteration of the Bayesian state update loop.
00187    ///
00188    /// In the second mode, the estimator shows the history of recent
00189    /// time-to-impact values predicted by the Bayesian state update.
00190    /// To help gauge how well the Bayes filter is doing, the estimator
00191    /// will also render the actual TTI values.
00192    ///
00193    /// In the third mode, the estimator shows the history of recent
00194    /// distance "readings" computed using the predicted
00195    /// times-to-impact and the actual distances to approaching
00196    /// obstacles (as reported by the laser range finder).
00197    ///
00198    /// This setting specifies which of the above render modes to use.
00199    /// The value of this setting should be a string. As described
00200    /// above, the currently supported modes are:
00201    ///        off ==> don't perform any rendering
00202    ///        bel ==> render current belief
00203    ///        tti ==> render time-to-impact histories
00204    ///        dis ==> render distance histories
00205    RenderMode m_render_mode ;
00206 
00207    /// The labels used to show the current TTI and other relevant info
00208    /// can clutter the visualization quite a bit (especially when the
00209    /// locust model drawables are small). This flag can be used to
00210    /// turn these labels on/off. By default, the labels are off.
00211    bool m_render_labels ;
00212 
00213    /// Private constructor because this is a singleton.
00214    TTIParams() ;
00215 
00216    // Boilerplate code to make generic singleton design pattern work
00217    friend class singleton<TTIParams> ;
00218 
00219 public:
00220    /// Accessing the various parameters.
00221    //@{
00222    static int sder_filter_size() {
00223       return instance().m_sder_filter_size ;
00224    }
00225    static float rising_threshold() {
00226       return instance().m_rising_threshold ;
00227    }
00228    static float falling_threshold() {
00229       return instance().m_falling_threshold ;
00230    }
00231    static float confidence_threshold() {
00232       return instance().m_confidence_threshold ;
00233    }
00234    static const range<float>& distance_range() {
00235       return instance().m_distance_range ;
00236    }
00237    static RenderMode render_mode() {return instance().m_render_mode   ;}
00238    static bool render_labels()     {return instance().m_render_labels ;}
00239    //@}
00240 } ;
00241 
00242 // Parameters initialization
00243 TTIParams::TTIParams()
00244    : m_sder_filter_size(clamp(conf("sder_filter_size", 5), 1, 100)),
00245      m_rising_threshold(clamp(conf("rising_threshold", 700.0f),
00246                               200.0f, 1000.0f)),
00247      m_falling_threshold(clamp(conf("falling_threshold", 100.0f),
00248                                10.0f, 250.0f)),
00249      m_confidence_threshold(clamp(conf("confidence_threshold", 0.3f),
00250                                   0.01f, 0.9f)),
00251      m_distance_range(clamp(conf("distance_range", make_range(50.0f, 5000.0f)),
00252                             make_range(10.0f, 10000.0f))),
00253      m_render_mode(RENDER_OFF),
00254      m_render_labels(conf("render_labels", false))
00255 {
00256    const std::string render_mode =
00257       downstring(conf<std::string>("render_mode", "off")) ;
00258    if (render_mode == "bel")
00259       m_render_mode = RENDER_BELIEF ;
00260    else if (render_mode == "tti")
00261       m_render_mode = RENDER_TTI ;
00262    else if (render_mode == "dis")
00263       m_render_mode = RENDER_DISTANCE ;
00264 }
00265 
00266 // Shortcut
00267 typedef TTIParams Params ;
00268 
00269 } // end of local anonymous namespace encapsulating above helpers
00270 
00271 //-------------------------- INITIALIZATION -----------------------------
00272 
00273 SensorModel& TTIEstimator::looming_sensor_model()
00274 {
00275    static SensorModel S("looming") ;
00276    return S ;
00277 }
00278 
00279 SensorModel& TTIEstimator::blanking_sensor_model()
00280 {
00281    static SensorModel S("blanking") ;
00282    return S ;
00283 }
00284 
00285 // When a Bayesian time-to-impact estimator is created, we initialize the
00286 // belief to a uniform probability distribution (i.e., we use an
00287 // uninformed prior).
00288 TTIEstimator::TTIEstimator(const LocustModel* L)
00289    : Drawable(L->name(), L->geometry()),
00290      m_locust(L),
00291      m_tti(-1), m_confidence(0),
00292      m_direction(cos(L->direction()), sin(L->direction())),
00293      m_sder(Params::sder_filter_size()),
00294      m_distance(-1),
00295      m_sensor_model(& looming_sensor_model())
00296 {
00297    m_lgmd[0] = m_lgmd[1] = 0 ;
00298    m_fder[0] = m_fder[1] = 0 ;
00299 
00300    const int N = m_sensor_model->column_size() ;
00301    m_belief.reserve(N) ;
00302    std::fill_n(std::back_inserter(m_belief), N, 1.0f/N) ;
00303 
00304    RenderMode render_mode = Params::render_mode() ;
00305    if (render_mode != RENDER_OFF)
00306    {
00307       if (render_mode == RENDER_TTI || render_mode == RENDER_DISTANCE) {
00308          m_actual.resize(m_geometry.width) ;
00309          m_predicted.resize(m_geometry.width) ;
00310       }
00311       (const_cast<LocustModel*>(L))->add_hook(
00312          RenderHook(render_hook, reinterpret_cast<unsigned long>(this))) ;
00313    }
00314 }
00315 
00316 //--------------------------- STATE MACHINE -----------------------------
00317 
00318 // Check which phase of the LGMD signal the current firing rate indicates
00319 TTIEstimator::LGMDPhase TTIEstimator::lgmd_phase() const
00320 {
00321    if (m_sensor_model == & looming_sensor_model())
00322       return LOOMING ;
00323    if (m_sensor_model == & blanking_sensor_model())
00324       return BLANKING ;
00325    throw misc_error(LOGIC_ERROR) ;
00326 }
00327 
00328 // Switch the sensor model to point to the correct likelihood profile
00329 // (based on LGMD firing rate phase as determined in update method).
00330 void TTIEstimator::sensor_model(const SensorModel* S)
00331 {
00332    viz_lock() ;
00333       m_sensor_model = S ;
00334    viz_unlock() ;
00335 }
00336 
00337 //------------------- BAYES FILTER UPDATE EQUATIONS ---------------------
00338 
00339 void TTIEstimator::copy_lgmd()
00340 {
00341    // The LGMD signal itself
00342    m_lgmd[1] = m_lgmd[0] ;
00343    m_lgmd[0] = m_locust->get_lgmd() ;
00344 
00345    // First derivative of LGMD signal
00346    m_fder[1] = m_fder[0] ;
00347    m_fder[0] = m_lgmd[0] - m_lgmd[1] ;
00348 
00349    // Second derivative of LGMD signal (filtered using weighted moving average)
00350    m_sder.add(m_fder[0] - m_fder[1]) ;
00351 
00352    // Also copy actual time-to-impact or distance so we can later compare
00353    // with the predicted values.
00354    viz_lock() ;
00355       switch (Params::render_mode())
00356       {
00357          case RENDER_TTI:
00358             m_actual.pop_front() ;
00359             m_actual.push_back(m_locust->tti()) ;
00360             break ;
00361          case RENDER_DISTANCE:
00362             m_actual.pop_front() ;
00363             m_actual.push_back(m_locust->distance()) ;
00364             break ;
00365          default:
00366             break ;
00367       }
00368    viz_unlock() ;
00369 }
00370 
00371 // Helper function object to add a uniform distribution to the elements
00372 // of an existing probability distribution.
00373 //
00374 // DEVNOTE: Since both the existing and uniform distributions sum to
00375 // unity, the sum of the combined distribution will be two. Rather than
00376 // perform another normalization step after the addition of the two
00377 // distributions, this function object simply divides each probability it
00378 // computes by two and returns the (already) normalized final result.
00379 // Thus, clients don't have to go through another normalization step
00380 // after applying this function to an existing probability distribution.
00381 class add_uniform_distribution {
00382    float uniform_probability ;
00383 public:
00384    add_uniform_distribution(float uniform_probability) ;
00385    float operator()(float p) const {
00386       return (p + uniform_probability)/2 ;
00387    }
00388 } ;
00389 
00390 add_uniform_distribution::add_uniform_distribution(float p)
00391    : uniform_probability(p)
00392 {}
00393 
00394 // Recursive Bayesian update to get new belief, i.e., posterior
00395 // probability distribution, using current belief as the prior and the
00396 // latest sensor (i.e., LGMD) value.
00397 //
00398 // DEVNOTE: The client behaviour *must* call TTIEstimator::copy_lgmd()
00399 // before invoking this method.
00400 void TTIEstimator::update()
00401 {
00402    // Use the correct sensor model for the current phase of the LGMD
00403    // input signal.
00404    switch (lgmd_phase())
00405    {
00406       case LOOMING:
00407          if (m_lgmd[0] > Params::rising_threshold() && m_sder.value() < 0)
00408             sensor_model(& blanking_sensor_model()) ;
00409          break ;
00410       case BLANKING:
00411          if (m_lgmd[0] < Params::falling_threshold()) // blanking over
00412             sensor_model(& looming_sensor_model()) ;
00413          break ;
00414    }
00415 
00416    // Multiply the individual probabilities of the current belief with
00417    // the corresponding probabilities in the appropriate column vector of
00418    // the sensor model.
00419    Belief tmp(m_belief.size(), 0.0f) ;
00420    std::transform(m_belief.begin(), m_belief.end(),
00421                   m_sensor_model->column_vector(m_lgmd[0]).begin(),
00422                   tmp.begin(), std::multiplies<float>()) ;
00423 
00424    // Normalize the intermediate belief obtained above. If the normalizer
00425    // is out-of-whack, reinit the belief to a uniform probability
00426    // distribution.
00427    float normalizer = std::accumulate(tmp.begin(), tmp.end(), 0.0f) ;
00428    if (normalizer <= 0) // something weird going on!
00429       std::fill(tmp.begin(), tmp.end(), 1.0f/tmp.size()) ;
00430    else
00431       std::transform(tmp.begin(), tmp.end(), tmp.begin(),
00432                      std::bind2nd(std::multiplies<float>(), 1/normalizer)) ;
00433 
00434    // Add a uniform distribution to the above result so as to model
00435    // random noise that diffuses the belief. If we don't do this, it
00436    // often happens that the belief reaches 100% for some state (0% for
00437    // all others) and then gets stuck in that state.
00438    //
00439    // In essence, this step ensures that we don't trust the sensor model
00440    // completely. After all, no model can be perfect...
00441    viz_lock() ;
00442       std::transform(tmp.begin(), tmp.end(), m_belief.begin(),
00443                      add_uniform_distribution(1.0f/tmp.size())) ;
00444 
00445       Belief::const_iterator max = std::max_element(m_belief.begin(),
00446                                                     m_belief.end()) ;
00447       const float min  = m_sensor_model->row_min() ;
00448       const float step = m_sensor_model->row_step();
00449       m_tti = min + (max - m_belief.begin() + 1) * step ;
00450       m_confidence = *max ;
00451    viz_unlock() ;
00452 }
00453 
00454 // Determine distance to obstacle in locust's direction by projecting
00455 // robot's current velocity vector onto locust's direction vector and
00456 // using the TTI estimate with max belief (subject to that estimate's
00457 // confidence being greater than the configured minimum).
00458 void TTIEstimator::compute_distance(const Vector& velocity)
00459 {
00460    float speed    = magnitude(dot(m_direction, velocity) * m_direction) ;
00461    float distance = -1 ;
00462    float time     = -1 ;
00463    if (m_confidence >= Params::confidence_threshold())
00464    {
00465       time = m_tti ;
00466       if (! is_zero(speed))
00467          distance = clamp(speed * time * 1000, Params::distance_range()) ;
00468    }
00469 
00470    viz_lock() ;
00471       m_distance = distance ;
00472       switch (Params::render_mode())
00473       {
00474          case RENDER_TTI:
00475             m_predicted.pop_front() ;
00476             m_predicted.push_back(time) ;
00477             break ;
00478          case RENDER_DISTANCE:
00479             m_predicted.pop_front() ;
00480             m_predicted.push_back(distance) ;
00481             break ;
00482          default:
00483             break ;
00484       }
00485    viz_unlock() ;
00486 }
00487 
00488 //--------------------------- VISUALIZATION -----------------------------
00489 
00490 #ifdef INVT_HAVE_LIBGLU
00491 
00492 // Quick helper to return a label for the current LGMD phase
00493 static std::string phase_label(int lgmd_phase)
00494 {
00495    std::ostringstream str ;
00496    switch (lgmd_phase)
00497    {
00498       case 0: // LOOMING
00499          str << "Loom" ;
00500          break ;
00501       case 1: // BLANKING
00502          str << "Blank" ;
00503          break ;
00504       default: // should never happen!
00505          str << "???" ;
00506          break ;
00507    }
00508    return str.str() ;
00509 }
00510 
00511 // Helper function to convert a TTI value to an appropriate label
00512 static std::string tti_label(float tti)
00513 {
00514    using namespace std ;
00515 
00516    std::ostringstream str ;
00517    if (tti < 0)
00518       str << "???" ;
00519    else
00520       str << fixed << setprecision(1) << tti << 's' ;
00521    return str.str() ;
00522 }
00523 
00524 // Helper function to convert current belief's peak to appropriate label
00525 // showing TTI and confidence level.
00526 static std::string tti_label(float tti, float confidence)
00527 {
00528    using namespace std ;
00529 
00530    std::ostringstream str ;
00531    str << fixed << setprecision(1) << tti << "s ("
00532        << fixed << setprecision(1) << (confidence * 100) << "%)" ;
00533    return str.str() ;
00534 }
00535 
00536 // Quick helper to return a label for current distance estimate based on
00537 // TTI computation.
00538 static std::string distance_label(int distance)
00539 {
00540    std::ostringstream str ;
00541    if (distance < 0)
00542       str << "???" ;
00543    else
00544       str << distance << " mm" ;
00545    return str.str() ;
00546 }
00547 
00548 // Helper function to show error in TTI estimate
00549 static std::string error_label(float actual, float predicted)
00550 {
00551    std::ostringstream str ;
00552    str << "Err: " ;
00553    if (actual < 0 || predicted < 0) // bogus TTI values
00554       str << '?' ;
00555    else
00556       str << round(100 * (predicted - actual)/actual) << '%' ;
00557    return str.str() ;
00558 }
00559 
00560 // This function renders the TTI estimator's current state on the
00561 // lobot::LocustModel drawable so that we can see the Bayesian state
00562 // estimation superimposed on the LGMD spike trains.
00563 void TTIEstimator::render_hook(unsigned long client_data)
00564 {
00565    TTIEstimator* me = reinterpret_cast<TTIEstimator*>(client_data) ;
00566    switch (Params::render_mode())
00567    {
00568       case RENDER_BELIEF:
00569          me->render_belief() ;
00570          break ;
00571       case RENDER_TTI:
00572          me->render_tti() ;
00573          break ;
00574       case RENDER_DISTANCE:
00575          me->render_distance() ;
00576          break ;
00577       default:
00578          break ;
00579    }
00580 }
00581 
00582 // This function renders the probability distribution showing the
00583 // time-to-impact likelihoods for each TTI estimator.
00584 void TTIEstimator::render_belief()
00585 {
00586    // Make local copy of current TTI belief so that behaviour's thread
00587    // isn't held up waiting for visualization to complete.
00588    viz_lock() ;
00589       Belief B = m_belief ;
00590       const SensorModel* S = m_sensor_model ;
00591       LGMDPhase P = lgmd_phase() ;
00592       int D = round(m_distance) ;
00593    viz_unlock() ;
00594 
00595    // Since TTI belief is overlaid on LGMD spikes visualization, we need
00596    // to reset the view volume before rendering.
00597    setup_view_volume(S->row_min(), S->row_max(), 0, 1) ;
00598 
00599    // Render TTI belief
00600    glColor3f(1, 0, 0) ;
00601    glBegin(GL_LINE_STRIP) ;
00602       glVertex2f(S->row_min(), 0) ;
00603       float x = S->row_min() + S->row_step() ;
00604       for (unsigned int i = 0; i < B.size(); ++i, x += S->row_step())
00605          glVertex2f(x, B[i]);
00606       glVertex2f(S->row_max(), 0) ;
00607    glEnd() ;
00608    restore_view_volume() ;
00609 
00610    // Label current LGMD phase being used for Bayes filter plus the
00611    // current belief and distance computation.
00612    if (Params::render_labels())
00613    {
00614       Belief::const_iterator max = std::max_element(B.begin(), B.end()) ;
00615       float tti = S->row_min() + (max - B.begin() + 1) * S->row_step() ;
00616       text_view_volume() ;
00617          glColor3f(0, 1, 1) ;
00618          draw_label(3, 24, phase_label(P).c_str()) ;
00619          draw_label(3, 40, tti_label(tti, *max).c_str()) ;
00620          draw_label(3, 56, distance_label(D).c_str()) ;
00621       restore_view_volume() ;
00622    }
00623 }
00624 
00625 // Helpers for drawing history
00626 typedef std::deque<float> History ;
00627 
00628 static void draw_history(const History& history)
00629 {
00630    const int N = history.size() ;
00631    History::const_iterator y = history.begin() ;
00632    glBegin(GL_LINE_STRIP) ;
00633       for (int x = 0; x < N; ++x, ++y)
00634          glVertex2f(x, *y) ;
00635    glEnd() ;
00636 }
00637 
00638 static void draw_history(const History& actual, const History& predicted)
00639 {
00640    glPushAttrib(GL_COLOR_BUFFER_BIT) ;
00641       glColor3f(0.0f, 0.15f, 0.85f) ;
00642       draw_history(actual) ;
00643 
00644       glColor3f(1, 0, 0) ;
00645       draw_history(predicted) ;
00646    glPopAttrib() ;
00647 }
00648 
00649 // This function renders the actual and predicted time-to-impact values
00650 // so that users can gauge how well the Bayes filter's predictions match
00651 // the actual situation on the ground.
00652 void TTIEstimator::render_tti()
00653 {
00654    // Make local copies so as to not hold up client behaviour's thread
00655    viz_lock() ;
00656       const float max_probability =
00657          *(std::max_element(m_belief.begin(), m_belief.end())) ;
00658       History actual = m_actual ;
00659       History predicted = m_predicted ;
00660    viz_unlock() ;
00661 
00662    // Setup view volume so that x-coordinates match the amount of TTI
00663    // history available and y-coordinates match the TTI range.
00664    setup_view_volume(0, m_geometry.width, 0, 60) ;
00665 
00666    // Draw the actual and predicted TTI histories
00667    draw_history(actual, predicted) ;
00668    restore_view_volume() ;
00669 
00670    // Label current values of actual and predicted times-to-impact
00671    if (Params::render_labels())
00672    {
00673       float A = actual.back() ;
00674       float P = predicted.back() ;
00675       text_view_volume() ;
00676          glColor3f(0, 1, 1) ;
00677          draw_label(3, 24, tti_label(A).c_str()) ;
00678          draw_label(3, 40, tti_label(P, max_probability).c_str()) ;
00679          draw_label(3, 56, error_label(A, P).c_str()) ;
00680       restore_view_volume() ;
00681    }
00682 }
00683 
00684 // This function renders the actual and predicted distances to obstacles
00685 // based on the time-to-impact estimates so that users can gauge how well
00686 // the Bayes filter's predictions match the actual situation on the
00687 // ground.
00688 void TTIEstimator::render_distance()
00689 {
00690    // Make local copies so as to not hold up client behaviour's thread
00691    viz_lock() ;
00692       History actual = m_actual ;
00693       History predicted = m_predicted ;
00694    viz_unlock() ;
00695 
00696    // Setup view volume so that x-coordinates match the amount of
00697    // distance history available and y-coordinates match the distance
00698    // range.
00699    setup_view_volume(0, m_geometry.width, 0, Params::distance_range().max()) ;
00700 
00701    // Draw the actual and predicted distance histories
00702    draw_history(actual, predicted) ;
00703    restore_view_volume() ;
00704 
00705    // Label current values of actual and predicted times-to-impact
00706    if (Params::render_labels())
00707    {
00708       float A = actual.back() ;
00709       float P = predicted.back() ;
00710       text_view_volume() ;
00711          glColor3f(0, 1, 1) ;
00712          draw_label(3, 24, distance_label(round(A)).c_str()) ;
00713          draw_label(3, 40, distance_label(round(P)).c_str()) ;
00714          draw_label(3, 56, error_label(A, P).c_str()) ;
00715       restore_view_volume() ;
00716    }
00717 }
00718 
00719 #endif
00720 
00721 //----------------------------- CLEAN-UP --------------------------------
00722 
00723 TTIEstimator::~TTIEstimator(){}
00724 
00725 //-----------------------------------------------------------------------
00726 
00727 } // end of namespace encapsulating this file's definitions
00728 
00729 /* So things look consistent in everyone's emacs... */
00730 /* Local Variables: */
00731 /* indent-tabs-mode: nil */
00732 /* End: */
Generated on Sun May 8 08:41:31 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3