00001 #include <cmath>
00002 #include <iostream>
00003 #include <vector>
00004 #include <algorithm>
00005 #include "PHGeometry.h"
00006 #include "MuiCommon.hh"
00007 #include "TMuiChannelId.hh"
00008 #include "MuiGeomClasses.hh"
00009 #include <TMuiRoadMapO.h>
00010 #include <TMuiClusterMapO.h>
00011 #include "TMuiRoadOGroup.h"
00012 #include <gsl/gsl_sort_double.h>
00013 #include <gsl/gsl_statistics.h>
00014
00015
00016
00017 TMuiRoadOGroup::TMuiRoadOGroup(const short& arm, const float& z,
00018 const float& MutrWin, const float& MuidWin)
00019 : fZPlane(z),fMutrWin(MutrWin),fMuidWin(MuidWin),
00020 fGroup(-1),fGoldenIndex(0),fChi2DOF(100000.0),
00021 fArm(arm),fTotalRoads(0),f_pRoads(0)
00022 {}
00023
00024
00025 TMuiRoadOGroup::TMuiRoadOGroup(const TMuiRoadOGroup& source)
00026 : fZPlane(source.fZPlane),fMutrWin(source.fMutrWin),
00027 fMuidWin(source.fMuidWin),
00028 fGroup(source.fGroup),fGoldenIndex(source.fGoldenIndex),
00029 fChi2DOF(source.fChi2DOF),fArm(source.fArm),
00030 fTotalRoads(source.fTotalRoads),f_pRoads(source.f_pRoads)
00031 {}
00032
00033
00034 TMuiRoadOGroup::~TMuiRoadOGroup() {}
00035
00036
00037
00038
00039
00040 void
00041 TMuiRoadOGroup::SetGroup(const short& Group)
00042 {
00043 if (Group < 0) {
00044 return;
00045 }
00046 fGroup = Group;
00047 vector<TMuiRoadMapO::pointer>::iterator i;
00048 for (i=f_pRoads.begin(); i != f_pRoads.end(); i++){
00049 (*i)->get()->set_group(fGroup);
00050 }
00051 MarkGolden();
00052 }
00053
00054
00055
00056 size_t
00057 TMuiRoadOGroup::GoldenIndex() const
00058 {
00059 return fGoldenIndex;
00060 }
00061
00062
00063
00064 short
00065 TMuiRoadOGroup::Group() const
00066 {
00067 return fGroup;
00068 }
00069
00070
00071
00072
00073
00074 void
00075 TMuiRoadOGroup::AttachRoad(TMuiRoadMapO::pointer road)
00076 {
00077
00078 f_pRoads.push_back(road);
00079 fTotalRoads = f_pRoads.size();
00080 }
00081
00082
00083
00084 void
00085 TMuiRoadOGroup::MergeGroup(TMuiRoadOGroup* sourceGroup)
00086 {
00087 if(sourceGroup==NULL)
00088 {
00089 return;
00090 }
00091 int roadIndex = 0;
00092 while(roadIndex < sourceGroup->TotalRoads())
00093 {
00094 AttachRoad(sourceGroup->Road(roadIndex));
00095 roadIndex++;
00096 }
00097 }
00098
00099
00100
00101 TMuiRoadMapO::pointer TMuiRoadOGroup::Road(int roadIndex) const
00102 {
00103 if((unsigned int)roadIndex < f_pRoads.size())
00104 {
00105 return f_pRoads[roadIndex];
00106 }
00107 return NULL;
00108 }
00109
00110
00111 void TMuiRoadOGroup::Clear()
00112 {
00113 f_pRoads.clear();
00114 fTotalRoads = f_pRoads.size();
00115 }
00116
00117
00118
00119 void
00120 TMuiRoadOGroup::RemoveRoad(TMuiRoadMapO::pointer road)
00121 {
00122
00123 vector<TMuiRoadMapO::pointer>::iterator i;
00124 for (i=f_pRoads.begin(); i != f_pRoads.end(); i++){
00125 if((*i) == road){
00126 f_pRoads.erase(i);
00127 break;
00128 }
00129 }
00130 fTotalRoads = f_pRoads.size();
00131 }
00132
00133
00134
00135 void
00136 TMuiRoadOGroup::MarkGolden()
00137 {
00138
00139
00140
00141 if(f_pRoads.size()>=1500)
00142 {
00143 cout<<"Road Group reached maximum entries: 1500\n";
00144 return;
00145 }
00146 double st3x, st3y;
00147 double st3xlist[1500];
00148 double st3ylist[1500];
00149 double Rst3xlist[1500];
00150 double Rst3ylist[1500];
00151 TMuiRoadMapO::pointer Rlist[1500];
00152 int xycount=0;
00153
00154 short Golden = 0;
00155 vector<TMuiRoadMapO::pointer>::iterator i;
00156 TMuiRoadMapO::pointer road = NULL;
00157 PHPoint st3p;
00158 for (i=f_pRoads.begin(); i != f_pRoads.end(); i++){
00159 road=(*i);
00160 st3p = ProjectToZ(road->get()->get_fit_par(),fZPlane);
00161 st3xlist[xycount] = st3p.getX();
00162 st3ylist[xycount] = st3p.getY();
00163 Rst3xlist[xycount] = st3p.getX();
00164 Rst3ylist[xycount] = st3p.getY();
00165 Rlist[xycount] = road;
00166 xycount++;
00167 }
00168
00169 gsl_sort(st3xlist,1,xycount);
00170 gsl_sort(st3ylist,1,xycount);
00171 st3x = gsl_stats_median_from_sorted_data(st3xlist,1,xycount);
00172 st3y = gsl_stats_median_from_sorted_data(st3ylist,1,xycount);
00173
00174 double MinDist = 1E10;
00175 double ThisDist = 0.0;
00176 int med_id = 0;
00177 for (int rid = 0; rid < xycount; rid++)
00178 {
00179 ThisDist = sqrt((st3x-Rst3xlist[rid])*(st3x-Rst3xlist[rid])+(st3y-Rst3ylist[rid])*(st3y-Rst3ylist[rid]));
00180 if(ThisDist < MinDist)
00181 {
00182 MinDist = ThisDist;
00183 road = Rlist[rid];
00184 med_id = rid;
00185 }
00186 }
00187
00188 for (i=f_pRoads.begin(); i != f_pRoads.end(); i++){
00189 if( (*i) == road){
00190 Golden = 1;
00191 (*i)->get()->set_golden(Golden);
00192 }else{
00193 (*i)->get()->set_golden(0);
00194 }
00195 }
00196 }
00197
00198
00199
00200
00201
00202 short
00203 TMuiRoadOGroup::TotalRoads() const {return fTotalRoads;}
00204
00205
00206
00207 PHPoint TMuiRoadOGroup::ProjectToZ(TMutFitPar fit_par, double fZPlane)
00208 {
00209 PHPoint z_intersect;
00210 PHGeometry::intersectionLinePlane(
00211 PHLine(fit_par.get_point(),fit_par.get_tangent()),
00212 PHPlane(PHPoint(0,0,fZPlane),PHVector(0,0,1)),
00213 z_intersect
00214 );
00215 return z_intersect;
00216 }
00217
00218
00219 bool
00220 TMuiRoadOGroup::IsGroup(TMuiRoadMapO::pointer road)
00221 {
00222 vector<TMuiRoadMapO::pointer>::iterator i;
00223 short arm = road->get()->get_arm();
00224
00225
00226 if ( arm != fArm ) {
00227 return false;
00228 }
00229
00230
00231 if(f_pRoads.size()>=1500)
00232 {
00233 return false;
00234 }
00235
00236
00237
00238
00239
00240
00241 bool St3IsGroup = false;
00242 PHPoint roadX = ProjectToZ(road->get()->get_fit_par(),fZPlane);
00243 for (i = f_pRoads.begin(); i != f_pRoads.end(); i++) {
00244
00245 if( (*i)->get()->get_key() == road->get()->get_key() ){
00246 return false;
00247 }
00248 PHPoint iX = ProjectToZ((*i)->get()->get_fit_par(),fZPlane);
00249 if ( (fabs(iX.getX()- roadX.getX()) < fMutrWin) && (fabs(iX.getY()- roadX.getY()) < fMutrWin)){
00250 St3IsGroup=true;
00251 break;
00252 }
00253 }
00254 if(St3IsGroup == false)
00255 {
00256 return false;
00257 }
00258
00259 int MatchingVHits = 0;
00260 int MatchingHHits = 0;
00261 int MinMatchHits = 1;
00262
00263 for (i = f_pRoads.begin(); i != f_pRoads.end(); i++)
00264 {
00265
00266 TMuiClusterMapO::const_key_iterator iclust1
00267 =road->get()->get_associated<TMuiClusterO>();
00268 TMuiClusterMapO::const_key_iterator iclust2
00269 =(*i)->get()->get_associated<TMuiClusterO>();
00270
00271 while(TMuiClusterMapO::const_pointer clust1 = iclust1.next())
00272 {
00273 iclust2 = (*i)->get()->get_associated<TMuiClusterO>();
00274 while(TMuiClusterMapO::const_pointer clust2 = iclust2.next())
00275 {
00276 if(clust1->get()->get_key()==clust2->get()->get_key())
00277 {
00278 if(clust1->get()->get_orientation()==0)
00279 {
00280 MatchingHHits++;
00281 }else{
00282 MatchingVHits++;
00283 }
00284 }
00285 }
00286 }
00287
00288
00289 }
00290
00291 if(MatchingVHits>=MinMatchHits && MatchingHHits>=MinMatchHits && St3IsGroup)
00292 {
00293 return true;
00294 }
00295
00296 return false;
00297
00298 }
00299