.. _program_listing_file_larflow_Reco_TrackOTFit.cxx:

Program Listing for File TrackOTFit.cxx
=======================================

|exhale_lsh| :ref:`Return to documentation for file <file_larflow_Reco_TrackOTFit.cxx>` (``larflow/Reco/TrackOTFit.cxx``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   #include "TrackOTFit.h"
   
   #include <cmath>
   #include <stdexcept>
   
   namespace larflow {
   namespace reco {
   
     void TrackOTFit::fit( std::vector< std::vector<float> >& initial_track,
                           std::vector< std::vector<float> >& track_pts_w_feat_v )
     {
       
     }
   
     float TrackOTFit::d2_segment_point( const std::vector<float>& seg_start,
                                         const std::vector<float>& seg_end,
                                         const std::vector<float>& testpt )
     {
   
       std::vector<float> a(3,0);
       std::vector<float> b(3,0);
       float a2 = 0.;
       float b2 = 0.;
       float c2 = 0.;
       float ab = 0.;
       for (int i=0; i<3; i++) {
         a[i] = seg_start[i]-testpt[i];
         b[i] = seg_end[i]-seg_start[i];
         a2 += a[i]*a[i];
         b2 += b[i]*b[i];
         ab += a[i]*b[i];
         c2 += ( seg_end[i]-testpt[i] )*( seg_end[i]-testpt[i] );
       }
   
       if (b2<1e-9) {
         throw std::runtime_error( "[TrackOTFit::d2_segment_point] segment too short" );
       }
   
       float lenb = sqrt(b2);
       float s  = -ab/lenb;  // projection onto segment
   
       if ( s>lenb+0.5 || s<-0.5 ) {
         // past the segment end, return distance to end
         return c2;
       }
   
       float d2 = (a2*b2-ab*ab)*b2; // distance from point to line
   
   
       return d2;
       
     }
   
     std::vector<float> TrackOTFit::grad_d2_wrt_segend( const std::vector<float>& seg_start,
                                                        const std::vector<float>& seg_end,
                                                        const std::vector<float>& testpt )
     {
   
       std::vector<float> a(3,0);
       std::vector<float> b(3,0);
       float a2 = 0.;
       float b2 = 0.;
       float ab = 0.;
       float c2 = 0.;    
       for (int i=0; i<3; i++) {
         a[i] = seg_start[i]-testpt[i];
         b[i] = seg_end[i]-seg_start[i];
         a2 += a[i]*a[i];
         b2 += b[i]*b[i];
         ab += a[i]*b[i];
         c2 += ( seg_end[i]-testpt[i] )*( seg_end[i]-testpt[i] );      
       }
   
       if (b2<1e-9) {
         throw std::runtime_error( "[TrackOTFit::grad_d2_wrt_segend] segment too short" );
       }
   
   
       std::vector<float> grad_d2(3,0);
   
       
       float lenb = sqrt(b2);
       float s  = -ab/lenb;  // projection onto segment
   
       if ( s>lenb+0.5 || s<-0.5 ) {
         // past the segment end, return distance to end, so we return grad
         for (int i=0; i<3; i++ ) {
           grad_d2[i] = 2.0*( seg_end[i] - testpt[i] );
         }
         return grad_d2;
       }
   
       
       std::vector<float> db2(3,0); // partials of |b|^2
       std::vector<float> dab(3,0); // partials of a.b
       for (int i=0; i<3; i++ ) {
         db2[i] = 2*(seg_end[i]-seg_start[i]);
         dab[i] = (seg_start[i]-testpt[i]);
       }
   
       float c = a2*b2-ab*ab; // numerator of d2 formula
       for (int i=0; i<3; i++) {
         grad_d2[i] = (b2*(a2*db2[i]-2*ab*dab[i]) - c*db2[i])/(b2*b2);
       }
   
       return grad_d2;
       
     }
   
     void TrackOTFit::getLossAndGradient(  const std::vector< std::vector<float> >& initial_track,
                                           const std::vector< std::vector<float> >& track_pts_w_feat_v,
                                           float& loss,
                                           std::vector<float>& grad )
     {
       float weight = 0;
       getWeightedLossAndGradient( initial_track, track_pts_w_feat_v, loss, weight, grad );
     }
   
     void TrackOTFit::getWeightedLossAndGradient(  const std::vector< std::vector<float> >& initial_track,
                                                   const std::vector< std::vector<float> >& track_pts_w_feat_v,
                                                   float& loss,
                                                   float& tot_weight,
                                                   std::vector<float>& grad )
     {
       grad.resize(3,0);
       for (int i=0; i<3; i++)
         grad[i] = 0.;
       loss = 0.;
   
       tot_weight = 0.;
   
       const std::vector<float>& start = initial_track[0];
       const std::vector<float>& end   = initial_track[1];
       
       int ndatapts = track_pts_w_feat_v.size();
       for ( int ipt=0; ipt<ndatapts; ipt++ ) {
         const std::vector<float>& testpt = track_pts_w_feat_v[ipt];
   
         // calculate the weight
         float w = 1.0;
         for (int i=3;i<(int)testpt.size(); i++)
           w *= testpt[i];
         w = fabs(w);
         
         loss += w*d2_segment_point( start, end, testpt );
         std::vector<float> ptgrad = grad_d2_wrt_segend( start, end, testpt );
   
         for (int i=0; i<3; i++) {
           grad[i] += ptgrad[i]*w;
         }
         tot_weight += w;
       }
   
       if ( tot_weight>0 ) {
         loss /= tot_weight;
         for (int i=0; i<3; i++)
           grad[i] /= tot_weight;
       }
   
     }
     
   }
   }