mht_declaration.h

Go to the documentation of this file.

/**************************************************************************************************
 Software License Agreement (BSD License)

 Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
 All rights reserved.

 Redistribution and use in source and binary forms, with or without modification, are permitted
 provided that the following conditions are met:

  *Redistributions of source code must retain the above copyright notice, this list of
   conditions and the following disclaimer.
  *Redistributions in binary form must reproduce the above copyright notice, this list of
   conditions and the following disclaimer in the documentation and/or other materials provided
   with the distribution.
  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
   endorse or promote products derived from this software without specific prior written permission.
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***************************************************************************************************/
#ifndef _MHT_DECLARATION_H_
#define _MHT_DECLARATION_H_

#include <algorithm>
#include <vector>
#include <string>
#include <iostream>
#include <fstream>

#include <assert.h>

#include <mtt/hypothesisTree.h>

#include <mtt/k_best.h>

#include <boost/shared_ptr.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

#include <colormap/colormap.h>
#include <boost/thread.hpp>

#include <mtt/metrics.h>

using namespace std;

MatrixXd ellipseParametric(Matrix2d& cov,Vector2d& mu,double MahThreshold);

extern string fileName;

template<class T>
bool removeLocal(vector<T>& v, T i)
{
        typename vector<T>::iterator it;
        
        it=find(v.begin(),v.end(),i);
        
        if(it==v.end())
                return false;
        
        v.erase(it);
        return true;
}

class Color: public std_msgs::ColorRGBA
{
        public:
                
                Color(std_msgs::ColorRGBA c);
                
                Color& changeColorBrightness(double correctionFactor);

                string str();
                
                ostream& operator<<(ostream&);
};

ostream& operator<<(ostream& o,vector<vector<int> >& measurements_assignemnt);
        
class Mht
{
        public:
                
                vector<ClusterPtr> _clusters;
                string _aux1;
                
                Mht(void);
                
                ~Mht(void);
                
                double getMaxGating(void);
                
                void clearTargetAssociations(void);
                
                void simplifySingleTargetClusters(vector<MeasurementPtr>& global_measurements);
                                
                void solveCluster(ClusterPtr cluster);
                
                void iterate(vector<MeasurementPtr>& measurements);
                
                hypothesisTreePtr getHypothesisTree(void);
                
                vector<TargetPtr> getTargets(void);
                
                void clear(void);
                
                string getReportName(string extra);
                
                void solveClusterMultiTread(ClusterPtr cluster);
                
                void solveHypothesis(HypothesisPtr parent_hypothesis,vector<MeasurementPtr>& measurements, vector<HypothesisPtr>& list_of_hypotheses);
                
                int _k;
                int _j;
                
        private:
        
                hypothesisTreePtr _hypothesisTree;

                HypothesisTree::leaf_iterator _hleaf;
                HypothesisTree::iterator _iterator;

                vector<TargetPtr> _current_targets;
                
                long iteration;
                double _max_gating;
                int _miss_association_threshold;
                double _minimum_representativity;
                
                uint _history_size;
                bool _remove_unused;
                bool _parent_tagging;
                bool _use_minimum_representativity;
                bool _remove_empty_clusters;
                bool _clean_old_modifying_tree;
                long _cluster_id;
                void recursiveHypothesisRemove(HypothesisTree::iterator it);
                void removeIrrelevant(void);
                void removeTheDead(void);
                
                vector<ClusterPtr>::iterator completeRemoveCluster(long id);
                
                vector<ClusterPtr>::iterator completeRemoveCluster(vector<ClusterPtr>::iterator it_cluster);
                
                MeasurementPtr findMeasurement(vector<MeasurementPtr>& measurements, long id);
                
                void switchColor(HypothesisPtr hypothesis);
                
                vector<HypothesisPtr> findHypotheses(int cl,int it_past = 1);
                
                ClusterPtr findCluster(vector<ClusterPtr>& clusters, int id);
                
                void createClusters(void);
                
                void clusterAssignemnt(vector<MeasurementPtr>& measurements);
                
                void cleanClusters(void);
                
                void removeEmptyClusters(void);
                
                bool consistencyCheck(void);
                
                void breakClusters(vector<MeasurementPtr>& measurements);
                
                HypothesisTree::iterator insertForcedParent(long cluster);
                
                vector<TargetPtr> createCopy(vector<TargetPtr>& original);
                
                void clusterMerger(vector<MeasurementPtr>& measurements);
                
                TargetPtr associate(TargetPtr& target,MeasurementPtr& measurement);
                
                bool normalize(vector<HypothesisPtr>& hypotheses,uint j=10e3);
                
                void getProbability(HypothesisPtr hypothesis);
                
                double getProbability(uint n_det, uint n_occ, uint n_del, uint n_new, uint n_fal, double prod, double previous);
                
                HypothesisPtr getHypothesisPtr(long id,ClusterPtr& cluster);
                
                void updateTargetList(void);
                
                void checkCurrentTargets(vector<MeasurementPtr>& measurements);
};

#endif