diff --git a/CMakeLists.txt b/CMakeLists.txt
index f9c69b34a4912490c7128a5c9d4f302ddde88c54..b2d263ef798ba78dc9d9e0701ccf25887931f767 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -270,14 +270,6 @@ SET(HDRS_SOLVER
include/core/solver/solver_manager.h
)
-SET(HDRS_DTASSC
- include/core/association/association_node.h
- include/core/association/association_solver.h
- include/core/association/association_tree.h
- include/core/association/matrix.h
- include/core/association/association_nnls.h
- )
-
SET(HDRS_YAML
include/core/yaml/parser_yaml.hpp
include/core/yaml/yaml_conversion.h
@@ -355,12 +347,6 @@ SET(SRCS_SENSOR
src/sensor/sensor_odom_2D.cpp
src/sensor/sensor_odom_3D.cpp
)
-SET(SRCS_DTASSC
- src/association/association_nnls.cpp
- src/association/association_node.cpp
- src/association/association_solver.cpp
- src/association/association_tree.cpp
- )
SET(SRCS_SOLVER
src/solver/solver_manager.cpp
)
@@ -410,7 +396,6 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_BASE}
${SRCS_CAPTURE}
${SRCS_COMMON}
- ${SRCS_DTASSC}
${SRCS_FACTOR}
${SRCS_FEATURE}
${SRCS_FRAME}
@@ -482,8 +467,6 @@ INSTALL(FILES ${HDRS_STATE_BLOCK}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block)
INSTALL(FILES ${HDRS_COMMON}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/common)
-INSTALL(FILES ${HDRS_DTASSC}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/association)
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/capture)
INSTALL(FILES ${HDRS_FACTOR}
diff --git a/include/core/association/association_nnls.h b/include/core/association/association_nnls.h
deleted file mode 100644
index c3030f7493cc3e9117a6433bf8e0521fc6fb8ecf..0000000000000000000000000000000000000000
--- a/include/core/association/association_nnls.h
+++ /dev/null
@@ -1,83 +0,0 @@
-
-#ifndef association_nnls_H
-#define association_nnls_H
-
-//std
-#include <iostream>
-#include <vector>
-
-//pipol tracker
-#include "core/association/association_solver.h"
-
-namespace wolf
-{
-
-//consts
-const double MAX_DIST_DEFAULT = 0.5; //units (meters in pt case)
-
-/** \brief Nearest neighbour linear search
- *
- * Nearest neighbour linear search to solve data association problems, given a table of distances
- *
-*/
-class AssociationNNLS : public AssociationSolver
-{
- protected:
- double max_dist_; //maximum distance to allow association
- std::vector<bool> i_mask_; // mask already allocated detections (rows)
- std::vector<bool> j_mask_; // mask already allocated targets (columns)
-
- public:
- /** \brief Constructor
- *
- * Constructor
- *
- */
- AssociationNNLS();
-
- /** \brief Destructor
- *
- * Destructor
- *
- */
- virtual ~AssociationNNLS();
-
- /** \brief Sets max_dist_
- *
- * Sets max_dist_
- *
- **/
- void setMaxDist(const double _max_dist);
-
- /** \brief Resets problem
- *
- * Resets problem
- *
- */
- void reset();
-
- /** \brief Resizes the problem
- *
- * Resizes the problem
- *
- */
- void resize(const unsigned int _n_det, const unsigned int _n_tar);
-
- /** \brief Solves the problem
- *
- * Solves the association problem following nearest neighbor linear search.
- * Return values are:
- * \param _pairs Returned pairs: vector of pairs (d_i, t_j)
- * \param _associated_mask Resized to nd_. Marks true at i if detection d_i has been associated, otherwise marks false
- *
- * Assumes i/j_mask_ vector class members and scores_ matrix are correctly sized, by a previous call to resize()
- *
- **/
- //void solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<unsigned int> & _unassoc);
- void solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<bool> & _associated_mask);
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/core/association/association_node.h b/include/core/association/association_node.h
deleted file mode 100644
index 50e2f67d07ba42a5fe08d7178ac9746934753bdd..0000000000000000000000000000000000000000
--- a/include/core/association/association_node.h
+++ /dev/null
@@ -1,177 +0,0 @@
-
-#ifndef association_node_H
-#define association_node_H
-
-//std
-#include <iostream>
-#include <vector>
-#include <list>
-#include <algorithm> //find()
-
-//pipol tracker
-#include "core/association/matrix.h"
-
-//constants
-const double PROB_ZERO_ = 1e-3;
-
-/** \brief A node in the association decision tree
- *
- * A node in the association decision tree. A node associates a pair between a detection index and a target index, which is
- * usually diferent from detection Id and target Id. Therefore, Id to index mapping has to be implemented outside of this class.
- *
-*/
-class AssociationNode
-{
- protected:
- bool is_root_;///<true if the node is root
- unsigned int det_idx_; ///< detection node index
- unsigned int tar_idx_; ///< target node index
- double node_prob_; ///< Node Probability. Normalized->Conditional Probability that detection associates to target. Non-normalizeNodeProbs->Product of likelihoods.
- double tree_prob_; ///< Tree Probability. Joint Probability from the root node up to this (product of node probabilities)
- AssociationNode * up_node_ptr_; ///< Pointer to up node
- std::list<AssociationNode> node_list_; ///< List of nodes below of this in the association tree
-
-
- public:
- /** \brief Constructor
- *
- * Constructor with arguments _det_idx and _tar_idx which indicates association of detection and target,
- * with the probability _prob;
- *
- */
- AssociationNode(const unsigned int _det_idx, const unsigned int _tar_idx, const double _prob, AssociationNode * _un_ptr, bool _is_root = false);
-
- /** \brief Destructor
- *
- * Destructor
- *
- */
- virtual ~AssociationNode();
-
- /** \brief True if this is the root node
- *
- * True if this is the root node, which is equivalent to check if up_node_ptr_ == NULL
- *
- **/
- bool isRoot() const;
-
- /** \brief True if this is node is terminus (no more nodes below)
- *
- * True if this is node is terminus (no more nodes below), which is equivalent to check node_list_.empty()
- *
- **/
- bool isTerminus() const;
-
- /** \brief Returns det_idx_
- *
- * Returns det_idx_
- *
- **/
- unsigned int getDetectionIndex() const;
-
- /** \brief Returns tar_idx_
- *
- * Returns tar_idx_
- *
- **/
- unsigned int getTargetIndex() const;
-
- /** \brief Returns node_prob_
- *
- * Returns node_prob_
- *
- **/
- double getNodeProb() const;
-
- /** \brief Sets node_prob_
- *
- * Sets node_prob_
- *
- **/
- void setNodeProb(double _np);
-
- /** \brief Returns tree_prob_
- *
- * Returns tree_prob_
- *
- **/
- double getTreeProb() const;
-
- /** \brief Returns a copy of up_node_ptr_
- *
- * Returns a copy of up_node_ptr_
- *
- **/
- AssociationNode * upNode() const;
-
- /** \brief Computes node probability
- *
- * Computes probability that detection_i associates to target_j, given the scores table _stab.
- * \param _nd TODO document this
- * \param _nt TODO document this
- * \param _di detection index (not id)
- * \param _tj target index (not id)
- * \param _stab score table
- * Returns the probability.
- * Nodes require computing other ij probs to decide if they continue growing or not at function growTree().
- *
- **/
- //double computeNodeProb(const unsigned int _nd, const unsigned int _nt, const unsigned int _di, const unsigned int _tj, const std::vector< std::vector<double> > & _stab) const;
- double computeNodeProb(const unsigned int _nd, const unsigned int _nt, const unsigned int _di, const unsigned int _tj, const Matrixx<double> & _stab) const;
-
- /** \brief Normalizes node probabilities recursively
- *
- * Normalizes node probabilities recursively.
- * All node probs of node_list_ should sum 1
- *
- **/
- void normalizeNodeProbs();
-
- /** \brief Computes tree probabilities recursively
- *
- * Computes tree probabilities recursively, while setting tree_prob_ data member
- * Updates the terminus node list, passed as second parameter, with all nodes that are terminus
- * \param _up_prob probability of upper node
- * \param _tn_list: List of terminus nodes. Filled with terminus nodes, while recurisve computing tree
- *
- **/
- void computeTreeProb(const double & _up_prob, std::list<AssociationNode*> & _tn_list);
-
- /** \brief Grows tree recursively
- *
- * Grows tree recursively according the association probability table provided
- * \param _nd
- * \param _nt
- * \param _det_i: detection index
- * \param _stab: table of association probabilities between detections and targets
- * \param _excluded: vector of target index for which the tree should not continue growing
- *
- **/
- //void growTree(const unsigned int _nd, const unsigned int _nt, const unsigned int _det_i, const std::vector< std::vector<double> > & _stab, std::vector<unsigned int> & _excluded);
- void growTree(const unsigned int _nd, const unsigned int _nt, const unsigned int _det_i, const Matrixx<double> & _stab, std::vector<unsigned int> & _excluded);
-
- /** \brief Destroys tree
- *
- * Recursively destroys tree
- *
- **/
- void destroyTree();
-
- /** \brief Prints node info
- *
- * Prints node info
- *
- **/
- void printNode() const;
-
- /** \brief Prints the tree, by printing node info recursively
- *
- * Prints the tree, by printing node info recursively
- * \param _ntabs Number of tabulators before printing. Useful for recursively print a whole tree
- *
- * TODO: This function should be const, but we run recursively with an iterator over the node_list_ and compiler
- * claims saying it can't return a const iterator.
- **/
- void printTree(const unsigned int _ntabs = 0);
-};
-#endif
diff --git a/include/core/association/association_solver.h b/include/core/association/association_solver.h
deleted file mode 100644
index 7a1ff70ef3876eb75d0b469f581784118aac2f43..0000000000000000000000000000000000000000
--- a/include/core/association/association_solver.h
+++ /dev/null
@@ -1,108 +0,0 @@
-
-#ifndef association_solver_H
-#define association_solver_H
-
-//std
-#include <iostream>
-#include <vector>
-
-//matrix class
-#include "core/association/matrix.h"
-
-namespace wolf
-{
-
-/** \brief A pure virtual solver for the association problem
- *
- * A pure virtual solver for the association problem
- *
-*/
-class AssociationSolver
-{
- protected:
- unsigned int nd_; //num detections
- unsigned int nt_; //num targets, without counting the void target
-// std::vector< std::vector<double> > scores_;//scores table. Size is (nd_) x (nt_+1), to account for the void target
- Matrixx<double> scores_;//scores table. Size is (nd_) x (nt_+1), to account for the void target
-
- public:
- /** \brief Constructor
- *
- * Constructor
- *
- */
- AssociationSolver();
-
- /** \brief Destructor
- *
- * Destructor
- *
- */
- virtual ~AssociationSolver();
-
- /** \brief Returns num of detections nd_
- *
- * Returns num of detections nd_
- *
- **/
- unsigned int numDetections();
-
- /** \brief Returns num of actual targets nt_
- *
- * Returns num of actual targets nt_
- *
- **/
- unsigned int numTargets();
-
- /** \brief Sets values to scores_ table
- *
- * Sets value to score table, at cell ij, corresponding to detection_i and target_j
- *
- **/
- void setScore(const unsigned int _det_i, const unsigned int _tar_j, const double _m_ij);
-
- /** \brief Gets a score
- *
- * Gets score of cell ij, corresponding to detection_i and target_j
- *
- **/
- double getScore(const unsigned int _det_i, const unsigned int _tar_j);
-
- /** \brief Prints the score table
- *
- * Prints the score table
- *
- */
- void printScoreTable() const;
-
- /** \brief Resets the problem
- *
- * Deletes and clears the problem
- *
- */
- virtual void reset() = 0;
-
- /** \brief Resizes the problem
- *
- * Resizes the problem:
- * \param _n_det num of detections
- * \param _n_tar num of targets
- *
- */
- virtual void resize(const unsigned int _n_det, const unsigned int _n_tar) = 0;
-
- /** \brief Solves and sets decision pairs
- *
- * Solves and sets decision pairs
- * Return values are:
- * \param _pairs Returned pairs: vector of pairs (d_i, t_j)
- * \param _associated_mask Resized to nd_. Marks true at i if detection d_i has been associated, otherwise marks false
- *
- **/
- //virtual void solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<unsigned int> & _unassoc) = 0;
- virtual void solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<bool> & _associated_mask) = 0;
-
-};
-
-} //namespace wolf
-#endif
diff --git a/include/core/association/association_tree.h b/include/core/association/association_tree.h
deleted file mode 100644
index e910183b011d485f03368c77578a8d0cdc53f8d5..0000000000000000000000000000000000000000
--- a/include/core/association/association_tree.h
+++ /dev/null
@@ -1,127 +0,0 @@
-
-#ifndef association_tree_H
-#define association_tree_H
-
-//std
-// #include <list>
-// #include <vector>
-//#include <pair>
-//#include <memory>
-
-//pipol tracker
-#include "core/association/matrix.h"
-#include "core/association/association_solver.h"
-#include "core/association/association_node.h"
-#include <map>
-
-namespace wolf
-{
-
-/** \brief The whole decision tree
- */
-class AssociationTree : public AssociationSolver
-{
- protected:
- AssociationNode root_;
- std::list<AssociationNode*> terminus_node_list_;
-
- public:
- /** \brief Constructor
- *
- * Constructor
- *
- */
- AssociationTree();
-
- /** \brief Destructor
- *
- * Destructor
- *
- */
- virtual ~AssociationTree();
-
- /** \brief Reset
- *
- * Deletes all nodes and clears scores and association list
- *
- */
- void reset();
-
- /** \brief Resizes the problem
- *
- * Resizes the problem:
- * \param _n_det num of detections
- * \param _n_tar num of targets
- * Resizes the scores_ matrix which will allocate _n_det rows and _n_tar+1 columns to take into account void target
- *
- */
- void resize(const unsigned int _n_det, const unsigned int _n_tar);
-
- /** \brief Build tree from scores
- *
- * Build tree from scores
- *
- */
- void growTree();
-
- /** \brief Computes tree probabilities
- *
- * Computes tree probabilities
- *
- */
- void computeTree();
-
- /** \brief Normalizes node probabilities
- *
- * Normalizes node probabilities
- *
- */
- void normalizeTree();
-
- /** \brief choose best terminus node
- *
- * Choose best terminus node based on the best tree probability
- * \param _best_node a reference to an iterator to a list of pointers, where returned result is placed.
- * At output, _best_node points the bets node in the terminus_node_list_
- *
- **/
- void chooseBestTerminus(std::list<AssociationNode*>::iterator & _best_node);
-
- /** \brief Gets tree decision
- *
- * Decides best hypothesis according tree computation made by computeTree()
- * Return values are:
- * \param _pairs Returned pairs: vector of pairs (d_i, t_j)
- * \param _associated_mask Resized to nd_. Marks true at i if detection d_i has been associated, otherwise marks false
- *
- **/
- void solve(std::map<unsigned int, unsigned int> & _pairs, std::vector<bool> & _associated_mask);
-
- /** \brief Gets tree decision
- *
- * Decides best hypothesis according tree computation made by computeTree()
- * Return values are:
- * \param _pairs Returned pairs: vector of pairs (d_i, t_j)
- * \param _associated_mask Resized to nd_. Marks true at i if detection d_i has been associated, otherwise marks false
- *
- **/
- void solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<bool> & _associated_mask);
-
- /** \brief Prints the tree
- *
- * Prints the tree
- *
- * TODO: this function should be const. See comments on printTree() at association_node.h
- */
- void printTree();
-
- /** \brief Prints terminus_node_list_
- *
- * Prints terminus_node_list_
- *
- */
- void printTerminusNodes();
-};
-
-} // namespace wolf
-#endif
diff --git a/include/core/association/matrix.h b/include/core/association/matrix.h
deleted file mode 100644
index c76bba51e4de2db56a380e48e7002a16bba78bcc..0000000000000000000000000000000000000000
--- a/include/core/association/matrix.h
+++ /dev/null
@@ -1,82 +0,0 @@
-
-#ifndef matrix_H
-#define matrix_H
-
-//std
-#include <iostream>
-#include <vector>
-#include <assert.h> //assert
-
-template <typename T>
-class Matrixx
-{
- protected:
- unsigned int rows_, cols_;
- std::vector<T> inner_;
-
- public:
- Matrixx() :
- rows_(0),
- cols_(0)
- {
- //
- }
-
- Matrixx(unsigned int _rows, unsigned int _cols) :
- rows_ (_rows),
- cols_ (_cols),
- inner_(rows_*cols_)
- {
- //
- }
-
- ~Matrixx()
- {
- //
- }
-
- void clear()
- {
- inner_.clear();
- }
-
- void resize(unsigned int _rows, unsigned int _cols)
- {
- rows_ = _rows;
- cols_ = _cols;
- inner_.resize (rows_*cols_);
- //std::cout << "Resizing matrix to " << rows_ << " x " << cols_ << std::endl;
- }
-
- unsigned int size() const
- {
- return rows_*cols_;
- }
-
- T& operator()(unsigned int _i, unsigned int _j)
- {
- assert( (_i < rows_) && (_j < cols_) && "Matrix::operator(): Wrong matrix indexes. Program abort.");
- return inner_[cols_*_i + _j];
- }
-
- const T& operator()(unsigned int _i, unsigned int _j) const
- {
- assert( (_i < rows_) && (_j < cols_) && "Matrix::operator(): Wrong matrix indexes. Program abort.");
- return inner_[cols_*_i + _j];
- }
-
- void print() const
- {
- for(unsigned int ii=0; ii<rows_; ii++)
- {
- for(unsigned int jj=0; jj<cols_; jj++)
- {
- std::cout << inner_[cols_*ii + jj] << " ";
- }
- std::cout << std::endl;
- }
- std::cout << std::endl;
- }
-
-};
-#endif
diff --git a/src/association/association_nnls.cpp b/src/association/association_nnls.cpp
deleted file mode 100644
index c1f9d97e7021901928b743270ff565c2fc2c5f3c..0000000000000000000000000000000000000000
--- a/src/association/association_nnls.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-#include "core/association/association_nnls.h"
-
-namespace wolf
-{
-
-AssociationNNLS::AssociationNNLS() :
- max_dist_(MAX_DIST_DEFAULT)
-{
- //
-}
-
-AssociationNNLS::~AssociationNNLS()
-{
- //
-}
-
-void AssociationNNLS::setMaxDist(const double _max_dist)
-{
- max_dist_ = _max_dist;
-}
-
-void AssociationNNLS::reset()
-{
- nd_ = 0;
- nt_ = 0;
- scores_.clear();
- i_mask_.clear();
- j_mask_.clear();
-}
-
-void AssociationNNLS::resize(const unsigned int _n_det, const unsigned int _n_tar)
-{
- nd_ = _n_det; //detections
- nt_ = _n_tar; //targets
- scores_.resize(nd_, nt_);
- i_mask_.resize(nd_, false);
- j_mask_.resize(nt_, false);
-}
-
-//void AssociationNNLS::solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<unsigned int> & _unassoc)
-void AssociationNNLS::solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<bool> & _associated_mask)
-{
- bool min_found = true;
- double min_value;
- unsigned int ii, jj, ii_min, jj_min;
-
- //resize _associated_mask and resets it to false
- _associated_mask.resize(nd_,false);
-
- //find nearest neighbors by successive passing and masking through the scores_ matrix
- while(min_found)
- {
- min_found = false;
- min_value = max_dist_;
-
- for(ii = 0; ii< nd_; ii++)
- {
- if ( i_mask_[ii] == false)
- {
- for(jj = 0; jj< nt_; jj++)
- {
- if ( j_mask_[jj] == false)
- {
- if ( (scores_(ii,jj) < max_dist_) && (scores_(ii,jj) < min_value) )
- {
- min_value = scores_(ii,jj);
- min_found = true;
- ii_min = ii;
- jj_min = jj;
- }
- }
- }
- }
- }
-
- if (min_found)
- {
- i_mask_[ii_min] = true;
- j_mask_[jj_min] = true;
- _associated_mask.at(ii_min) = true;
- _pairs.push_back( std::pair<unsigned int, unsigned int>(ii_min, jj_min) );
- }
- }
-
- //set unassociated detections
-// for(ii = 0; ii< nd_; ii++)
-// {
-// if (i_mask_[ii] == false)
-// {
-// _unassoc.push_back(ii);
-// }
-// }
-
-}
-
-} // namespace wolf
diff --git a/src/association/association_node.cpp b/src/association/association_node.cpp
deleted file mode 100644
index cf8cd45243fab361d5053cfde96f0719cd082c45..0000000000000000000000000000000000000000
--- a/src/association/association_node.cpp
+++ /dev/null
@@ -1,195 +0,0 @@
-
-#include "core/association/association_node.h"
-
-AssociationNode::AssociationNode(const unsigned int _det_idx, const unsigned int _tar_idx, const double _prob, AssociationNode * _un_ptr, bool _is_root) :
- is_root_(_is_root),
- det_idx_(_det_idx),
- tar_idx_(_tar_idx),
- node_prob_(_prob),
- tree_prob_(1.)
-{
- up_node_ptr_ = _un_ptr;
-}
-
-AssociationNode::~AssociationNode()
-{
- //
-}
-
-bool AssociationNode::isRoot() const
-{
-// if ( up_node_ptr_ == NULL ) return true;
-// else return false;
- return is_root_;
-}
-
-bool AssociationNode::isTerminus() const
-{
- if ( node_list_.empty() ) return true;
- else return false;
-}
-
-unsigned int AssociationNode::getDetectionIndex() const
-{
- return det_idx_;
-}
-
-unsigned int AssociationNode::getTargetIndex() const
-{
- return tar_idx_;
-}
-
-double AssociationNode::getNodeProb() const
-{
- return node_prob_;
-}
-
-void AssociationNode::setNodeProb(double _np)
-{
- node_prob_ = _np;
-}
-
-double AssociationNode::getTreeProb() const
-{
- return tree_prob_;
-}
-
-AssociationNode* AssociationNode::upNode() const
-{
- //return &(*up_node_ptr_);
- return up_node_ptr_;
-}
-
-//double AssociationNode::computeNodeProb(const unsigned int _nd, const unsigned int _nt, const unsigned int _di, const unsigned int _tj, const std::vector< std::vector<double> > & _stab) const
-double AssociationNode::computeNodeProb(const unsigned int _nd, const unsigned int _nt, const unsigned int _di, const unsigned int _tj, const Matrixx<double> & _stab) const
-{
- double p_ij = 1.0;
-
- if ( _tj == _nt ) //Case void target -> unassociated detection
- {
- //Prob detection _di does not match to other targets than _tj
- for (unsigned int kk=0; kk<_nt; kk++)
- {
- //p_ij *= ( 1.0 - _stab.at(_di).at(kk) );
- p_ij *= ( 1.0 - _stab(_di,kk) );
- }
- }
- else //General case
- {
- //step 1. Positive matching _di with _tj
-// p_ij *= _stab.at(_di).at(_tj);
- p_ij *= _stab(_di,_tj);
-
- //step2. Prob detection _di does not match to other targets than _tj
- for (unsigned int kk=0; kk<_nt; kk++)
- {
-// if ( kk!=_tj ) p_ij *= 1 - _stab.at(_di).at(kk);
- if ( kk!=_tj ) p_ij *= 1 - _stab(_di,kk);
- }
-
- //step3. Prob target _tj does not match to other detections than _di
- for (unsigned int kk=0; kk<_nd; kk++)
- {
-// if ( kk!=_di ) p_ij *= 1 - _stab.at(kk).at(_tj);
- if ( kk!=_di ) p_ij *= 1 - _stab(kk,_tj);
- }
-
- //step4. Prob detection _di does not remain unassociated
- double p_un = 1.0;
- for (unsigned int kk=0; kk<_nt; kk++)
- {
- //p_un *= ( 1.0 - _stab.at(_di).at(kk) );
- p_un *= ( 1.0 - _stab(_di,kk) );
- }
- p_ij *= ( 1 - p_un );
- }
- return p_ij;
-}
-
-void AssociationNode::normalizeNodeProbs()
-{
- double pSum = 0;
- std::list<AssociationNode>::iterator it;
-
- for(it = node_list_.begin(); it != node_list_.end(); it++)
- pSum += it->getNodeProb();
-
- for(it = node_list_.begin(); it != node_list_.end(); it++)
- {
- it->setNodeProb(it->getNodeProb()/pSum);
- if (!isTerminus())
- it->normalizeNodeProbs();
- }
-}
-
-void AssociationNode::computeTreeProb(const double & _up_prob, std::list<AssociationNode*> & _tn_list)
-{
- std::list<AssociationNode>::iterator it;
-
- //compute joint probability
- tree_prob_ = _up_prob * node_prob_;
-
- //if terminus node, we have to add it to the terminus node list
- if ( isTerminus() )
- {
- _tn_list.push_back( &(*this) );
- }
- else //otherwise carry on recursivity
- {
- for(it = node_list_.begin(); it != node_list_.end(); it++)
- it->computeTreeProb(tree_prob_, _tn_list);
- }
-}
-
-//void AssociationNode::growTree(const unsigned int _nd, const unsigned int _nt, const unsigned int _det_i, const std::vector< std::vector<double> > & _stab, std::vector<unsigned int> & _excluded)
-void AssociationNode::growTree(const unsigned int _nd, const unsigned int _nt, const unsigned int _det_i, const Matrixx<double> & _stab, std::vector<unsigned int> & _excluded)
-{
- unsigned int tar_j; //target index (not target id!)
- double p_ij;//probability that detection i comes from target j
-
- //Recursive growing loop
- for (tar_j=0; tar_j<_nt+1; tar_j++) //for each target, including target void
- {
- //Carry on growing if target is void OR if target is not at excluded vector
- if ( (tar_j == _nt) || ( std::find(_excluded.begin(), _excluded.end(), tar_j) == _excluded.end() ) )
- {
- //compute probability from the score table
- p_ij = computeNodeProb(_nd, _nt, _det_i, tar_j, _stab); //std::cout << __LINE__ << ": p_ij: " << p_ij << std::endl;
-
- //Create node if prob is relevant, and carry on recursivity. Otherwise this current branch stops growing
- if (p_ij > PROB_ZERO_)
- {
- node_list_.push_back(AssociationNode(_det_i, tar_j, p_ij, this));
- if (_det_i+1 < _nd ) //check if there is more detections to carry on recursivity
- {
- _excluded.push_back(tar_j);//push back target to excluded vector
- node_list_.back().growTree(_nd, _nt, _det_i+1, _stab, _excluded); //recursivity
- _excluded.pop_back();//pop back target to excluded vector
- }
- }
- }
- }
-}
-
-void AssociationNode::destroyTree()
-{
- node_list_.clear();
-}
-
-void AssociationNode::printNode() const
-{
- std::cout << "D" << det_idx_ << ",T" << tar_idx_ << ", np=" << node_prob_ << ", tp=" << tree_prob_ << std::endl;
- //std::cout << "D" << det_idx_ << ",T" << tar_idx_ << ", np=" << node_prob_ << ", tp=" << tree_prob_ << ", this=" << this << ", up_ptr=" << up_node_ptr_ << std::endl;
-}
-
-void AssociationNode::printTree(const unsigned int _ntabs)
-{
- std::list<AssociationNode>::iterator it;
-
- for(unsigned int ii=0; ii<_ntabs; ii++) std::cout << "\t";
- printNode();
-
- //for (unsigned int ii=0; ii<node_list_.size(); ii++)
- // node_list_.at(ii).printTree(_ntabs+1);
- for(it = node_list_.begin(); it != node_list_.end(); it++) it->printTree(_ntabs+1);
-}
diff --git a/src/association/association_solver.cpp b/src/association/association_solver.cpp
deleted file mode 100644
index 8b277602114a453a3483db6bd110a9dd7e369430..0000000000000000000000000000000000000000
--- a/src/association/association_solver.cpp
+++ /dev/null
@@ -1,44 +0,0 @@
-
-#include "core/association/association_solver.h"
-
-namespace wolf
-{
-
-AssociationSolver::AssociationSolver() :
- nd_(0),
- nt_(0)
-{
- //
-}
-
-AssociationSolver::~AssociationSolver()
-{
- //
-}
-
-unsigned int AssociationSolver::numDetections()
-{
- return nd_;
-}
-
-unsigned int AssociationSolver::numTargets()
-{
- return nt_;
-}
-
-void AssociationSolver::setScore(const unsigned int _det_i, const unsigned int _tar_j, const double _m_ij)
-{
- scores_(_det_i,_tar_j) = _m_ij;
-}
-
-double AssociationSolver::getScore(const unsigned int _det_i, const unsigned int _tar_j)
-{
- return scores_(_det_i,_tar_j);
-}
-
-void AssociationSolver::printScoreTable() const
-{
- scores_.print();
-}
-
-} //namespace wolf
diff --git a/src/association/association_tree.cpp b/src/association/association_tree.cpp
deleted file mode 100644
index 94d02cab70b693fd194f700893314559c5257dba..0000000000000000000000000000000000000000
--- a/src/association/association_tree.cpp
+++ /dev/null
@@ -1,186 +0,0 @@
-
-#include "core/association/association_tree.h"
-
-namespace wolf
-{
-
-AssociationTree::AssociationTree() :
- AssociationSolver(),
- root_(0,0,1, NULL, true)
-{
- //
-}
-
-AssociationTree::~AssociationTree()
-{
- //
-}
-
-void AssociationTree::reset()
-{
- nd_ = 0;
- nt_ = 0;
- scores_.clear();
- terminus_node_list_.clear();
- root_.destroyTree();
-}
-
-void AssociationTree::resize(const unsigned int _n_det, const unsigned int _n_tar)
-{
- nd_ = _n_det;
- nt_ = _n_tar;
- scores_.resize(nd_,nt_+1); //"+1" to account for void target, which manages unassociated detections
-}
-
-
-void AssociationTree::growTree()
-{
- std::vector<unsigned int> ex_vec;
-
- if ( nd_ != 0 ) //check if detections
- root_.growTree(nd_, nt_, 0,scores_, ex_vec);
-}
-
-void AssociationTree::computeTree()
-{
- if ( nd_ != 0 ) //check if detections
- root_.computeTreeProb(1., terminus_node_list_);
-}
-
-void AssociationTree::normalizeTree()
-{
- root_.normalizeNodeProbs();
-}
-
-void AssociationTree::chooseBestTerminus(std::list<AssociationNode*>::iterator & _best_node)
-{
- std::list<AssociationNode*>::iterator it;
- double bestProb = 0.;
- //double sum = 0;
-
- for (it = terminus_node_list_.begin(); it != terminus_node_list_.end(); it++)
- {
- if ( (*it)->getTreeProb() > bestProb )
- {
- _best_node = it;
- bestProb = (*it)->getTreeProb();
- }
-
- //debugging
- //sum += (*it)->getTreeProb();
- }
-}
-
-void AssociationTree::solve(std::map<unsigned int, unsigned int> & _pairs, std::vector<bool> & _associated_mask)
-{
- std::list<AssociationNode*>::iterator best_node;
-// bool rootReached = false;
- AssociationNode *anPtr;
-
- //grows tree exploring all likely hypothesis
- growTree();
-
- //computes tree probs
- computeTree();
-
- //normalizes tree probs
- normalizeTree();
-
- //if terminus_node_list_ is empty exit withou pairing
- if ( terminus_node_list_.empty() ) return;
-
- //choose best node based on best tree probability
- chooseBestTerminus(best_node);
-
- //resize _associated_mask and resets it to false
- _associated_mask.resize(nd_,false);
-
- //set pairs
- anPtr = *best_node; //init pointer
-// int ii=0;
- while( ! anPtr->isRoot() ) //set pairs
- {
-// if ( anPtr->getTargetIndex() == nt_) //detection with void target -> unassociated detection
-// {
-// _unassoc.push_back(anPtr->getDetectionIndex());
-// }
-// else
-// {
-// _pairs.push_back( std::pair<unsigned int, unsigned int>(anPtr->getDetectionIndex(), anPtr->getTargetIndex()) );
-// }
- if ( anPtr->getTargetIndex() < nt_ ) //association pair
- {
- _associated_mask.at(anPtr->getDetectionIndex()) = true;
- _pairs[anPtr->getDetectionIndex()] = anPtr->getTargetIndex();
- }
- anPtr = anPtr->upNode();
- }
-}
-
-void AssociationTree::solve(std::vector<std::pair<unsigned int, unsigned int> > & _pairs, std::vector<bool> & _associated_mask)
-{
- std::list<AssociationNode*>::iterator best_node;
-// bool rootReached = false;
- AssociationNode *anPtr;
-
- //grows tree exploring all likely hypothesis
- growTree();
-
- //computes tree probs
- computeTree();
-
- //normalizes tree probs
- normalizeTree();
-
- //if terminus_node_list_ is empty exit withou pairing
- if ( terminus_node_list_.empty() ) return;
-
- //choose best node based on best tree probability
- chooseBestTerminus(best_node);
-
- //resize _associated_mask and resets it to false
- _associated_mask.resize(nd_,false);
-
- //set pairs
- anPtr = *best_node; //init pointer
-// int ii=0;
- while( ! anPtr->isRoot() ) //set pairs
- {
-// if ( anPtr->getTargetIndex() == nt_) //detection with void target -> unassociated detection
-// {
-// _unassoc.push_back(anPtr->getDetectionIndex());
-// }
-// else
-// {
-// _pairs.push_back( std::pair<unsigned int, unsigned int>(anPtr->getDetectionIndex(), anPtr->getTargetIndex()) );
-// }
- if ( anPtr->getTargetIndex() < nt_ ) //association pair
- {
- _associated_mask.at(anPtr->getDetectionIndex()) = true;
- _pairs.push_back( std::pair<unsigned int, unsigned int>(anPtr->getDetectionIndex(), anPtr->getTargetIndex()) );
- }
- anPtr = anPtr->upNode();
- }
-}
-
-void AssociationTree::printTree()
-{
- if ( scores_.size() != 0 )
- {
- std::cout << "Nd: " << nd_ << "; Nt: " << nt_ << std::endl;
- root_.printTree();
- }
-}
-
-void AssociationTree::printTerminusNodes()
-{
- std::list<AssociationNode*>::iterator it;
- unsigned int ii;
-
- for (it = terminus_node_list_.begin(), ii=0; it != terminus_node_list_.end(); it++, ii++)
- {
- (*it)->printNode();
- }
-}
-
-} // namespace wolf