contrib/brl/bbas/bsta/algo/bsta_von_mises

Go to the documentation of this file.
00001 // This is brl/bbas/bsta/algo/bsta_von_mises_updater.txx
00002 #ifndef bsta_von_mises_updater_txx_
00003 #define bsta_von_mises_updater_txx_
00004 //:
00005 // \file
00006 #include "bsta_von_mises_updater.h"
00007 #include <vcl_iostream.h>
00008 #include <vcl_cassert.h>
00009 #include <vcl_limits.h>
00010 //: The main function
00011 template <class von_mises_dist_>
00012 void bsta_von_mises_updater<von_mises_dist_>::update(obs_vm_dist_& pdist, const vect_t& vsum, math_t alpha) const
00013 {
00014   if(!(data_dimension == 2||data_dimension == 3)){
00015     vcl_cerr << "von mises update only implemented for 2-d and 3-d vectors\n";
00016     return;
00017   }
00018   math_t r_mag = vsum.magnitude();
00019   pdist.set_mean(vsum/r_mag);
00020   if(alpha ==math_t(1)){
00021     pdist.set_kappa(initial_kappa_);
00022     return;
00023   }
00024   //  double minv = static_cast<double>(vcl_numeric_limits<math_t>::min()); 
00025   double minv = static_cast<double>(0.00001); 
00026   double r_bar = static_cast<double>(r_mag*alpha);
00027   assert(r_bar<=1.00001); //round off in vector normalization
00028   if(r_bar>1.0) r_bar = 1.0;
00029   if(data_dimension == 3){
00030   if(r_bar>=0.0&&r_bar<0.2941)
00031     {
00032       double k = 16.2227*(0.348127-0.351118*vcl_sqrt(0.980629-r_bar));
00033       pdist.set_kappa(static_cast<math_t>(k));
00034       return;
00035     }
00036   if(r_bar>=0.2941&&r_bar<0.5398)
00037     {
00038       double k = 9.96768*(0.373669-0.447938*vcl_sqrt(0.686434-r_bar));
00039       pdist.set_kappa(static_cast<math_t>(k));
00040       return;
00041     }
00042   if(r_bar>=0.5398&&r_bar<0.6775)
00043     {
00044       double k = 12.0976*(0.338089-0.406599*vcl_sqrt(0.717942-r_bar));
00045       pdist.set_kappa(static_cast<math_t>(k));
00046       return;
00047     }
00048   if(r_bar>=0.6775)
00049     {
00050       double k = 1.0;
00051       double temp = (1.0-r_bar);
00052       if(temp<minv) 
00053         k = static_cast<double>(vcl_numeric_limits<math_t>::max());
00054       else
00055         k = 1.0/temp;
00056       pdist.set_kappa(static_cast<math_t>(k));
00057       return;
00058     }
00059   }
00060   if(data_dimension == 2){
00061     double t2 = r_bar*r_bar;
00062     double t3 = t2*r_bar;
00063     if(r_bar<0.53)
00064       {
00065         double t5 = t2*t3;
00066         double k = 2.0*r_bar+ t3 + 5.0*t5/6.0;
00067         pdist.set_kappa(static_cast<math_t>(k));
00068         return;
00069       }
00070     if(r_bar>=0.53&&r_bar<0.85)
00071       {
00072         double t1 = 1.0;
00073         double temp = (1.0-r_bar);
00074         if(temp<minv)
00075           t1 = static_cast<double>(vcl_numeric_limits<math_t>::max());
00076         else
00077           t1 = 1.0/temp;
00078         double k = -0.4 + 1.39*r_bar + 0.43*t1;
00079         pdist.set_kappa(static_cast<math_t>(k));
00080         return;
00081       }
00082     if(r_bar>=0.85)
00083       {
00084         double k = 1.0;
00085         double temp = t3 - 4.0*t2 + 3.0*r_bar;
00086         if(temp<minv)
00087           k = static_cast<double>(vcl_numeric_limits<math_t>::max());
00088         else
00089           k = 1.0/temp;
00090         pdist.set_kappa(static_cast<math_t>(k));
00091         return;
00092       }
00093   }
00094 }
00095 
00096 
00097 #define BSTA_VON_MISES_UPDATER_INSTANTIATE(T) \
00098 template class bsta_von_mises_updater<T >
00099 
00100 #endif // bsta_von_mises_updater_txx_