mobileye fusion

src/fusion/lidar_radar_fusion_cnn/Tracker/Ctracker.cpp

@@ -0,0 +1,275 @@
+#include "Ctracker.h"
+///
+/// \brief CTracker::CTracker
+/// Tracker. Manage tracks. Create, remove, update.
+/// \param settings
+///
+CTracker::CTracker(TrackerSettings& settings)
+{
+    m_settings = settings;
+}
+///
+/// \brief CTracker::CTracker
+/// Tracker. Manage tracks. Create, remove, update.
+/// \param settings
+///
+CTracker::CTracker(const TrackerSettings& settings)
+    :
+      m_settings(settings),
+      m_nextTrackID(0)
+{
+    ShortPathCalculator* spcalc = nullptr;
+    SPSettings spSettings = { settings.m_distThres, 12 };
+    switch (m_settings.m_matchType)
+    {
+    case tracking::MatchHungrian:
+        spcalc = new SPHungrian(spSettings);
+        break;
+        //    case tracking::MatchBipart:
+        //        spcalc = new SPBipart(spSettings);
+        //        break;
+    }
+    assert(spcalc != nullptr);
+    m_SPCalculator = std::unique_ptr<ShortPathCalculator>(spcalc);
+}
+
+///
+/// \brief CTracker::~CTracker
+///
+CTracker::~CTracker(void)
+{
+}
+///
+/// \brief CTracker::setSettings
+/// Tracker. Manage tracks. Create, remove, update.
+/// \param settings
+///
+void CTracker::setSettings(TrackerSettings& settings)
+{
+    m_settings = settings;
+    m_nextTrackID = 0;
+    ShortPathCalculator* spcalc = nullptr;
+    SPSettings spSettings = { settings.m_distThres, 12 };
+    switch (m_settings.m_matchType)
+    {
+    case tracking::MatchHungrian:
+        spcalc = new SPHungrian(spSettings);
+        break;
+        //    case tracking::MatchBipart:
+        //        spcalc = new SPBipart(spSettings);
+        //        break;
+    }
+    assert(spcalc != nullptr);
+    m_SPCalculator = std::unique_ptr<ShortPathCalculator>(spcalc);
+}
+///
+/// \brief CTracker::Update
+/// \param regions
+/// \param currFrame
+/// \param fps
+///
+void CTracker::Update(
+        const regions_t& regions,
+        cv::UMat currFrame,
+        float fps
+        )
+{
+    UpdateTrackingState(regions, currFrame, fps);
+
+    currFrame.copyTo(m_prevFrame);
+}
+
+///
+/// \brief CTracker::UpdateTrackingState
+/// \param regions
+/// \param currFrame
+/// \param fps
+///
+void CTracker::UpdateTrackingState(
+        const regions_t& regions,
+        cv::UMat currFrame,
+        float fps
+        )
+{
+    const size_t N = m_tracks.size();	// Tracking objects
+    const size_t M = regions.size();	// Detections or regions
+
+    assignments_t assignment(N, -1); // Assignments regions -> tracks
+
+    if (!m_tracks.empty())
+    {
+        // Distance matrix between all tracks to all regions
+        distMatrix_t costMatrix(N * M);
+        const track_t maxPossibleCost = static_cast<track_t>(currFrame.cols * currFrame.rows);
+        track_t maxCost = 0;
+        CreateDistaceMatrix(regions, costMatrix, maxPossibleCost, maxCost, currFrame);
+
+        // Solving assignment problem (shortest paths)
+        m_SPCalculator->Solve(costMatrix, N, M, assignment, maxCost);//row->col(trackid->regionid)
+
+        // clean assignment from pairs with large distance
+        for (size_t i = 0; i < assignment.size(); i++)
+        {
+            if (assignment[i] != -1)
+            {
+                if (costMatrix[i + assignment[i] * N] > m_settings.m_distThres)
+                {
+                    assignment[i] = -1;
+                    m_tracks[i]->SkippedFrames()++;
+                }
+            }
+            else
+            {
+                // If track have no assigned detect, then increment skipped frames counter.
+                m_tracks[i]->SkippedFrames()++;
+            }
+            m_tracks[i]->m_regionID = assignment[i];
+
+        }
+
+        // If track didn't get detects long time, remove it.
+        for (size_t i = 0; i < m_tracks.size();)
+        {
+            if (m_tracks[i]->SkippedFrames() > m_settings.m_maximumAllowedSkippedFrames ||
+                    m_tracks[i]->IsOutOfTheFrame() ||
+                    m_tracks[i]->IsStaticTimeout(cvRound(fps * (m_settings.m_maxStaticTime - m_settings.m_minStaticTime))))
+            {
+                m_tracks.erase(m_tracks.begin() + i);
+                assignment.erase(assignment.begin() + i);
+            }
+            else
+            {
+                ++i;
+            }
+        }
+    }
+
+    // Search for unassigned detects and start new tracks for them.
+    for (size_t i = 0; i < regions.size(); ++i)
+    {
+        if (find(assignment.begin(), assignment.end(), i) == assignment.end())
+        {
+            m_tracks.push_back(std::make_unique<CTrack>(regions[i],
+                                                        m_settings.m_kalmanType,
+                                                        m_settings.m_dt,
+                                                        m_settings.m_accelNoiseMag,
+                                                        m_settings.m_useAcceleration,
+                                                        m_nextTrackID++%500,
+                                                        i,
+                                                        m_settings.m_filterGoal == tracking::FilterRect,
+                                                        m_settings.m_lostTrackType));
+        }
+    }
+
+    // Update Kalman Filters state
+    const ptrdiff_t stop_i = static_cast<ptrdiff_t>(assignment.size());
+#pragma omp parallel for
+    for (ptrdiff_t i = 0; i < stop_i; ++i)
+    {
+        // If track updated less than one time, than filter state is not correct.
+        if (assignment[i] != -1) // If we have assigned detect, then update using its coordinates,
+        {
+            m_tracks[i]->SkippedFrames() = 0;
+            m_tracks[i]->Update(
+                        regions[assignment[i]], true,
+                    m_settings.m_maxTraceLength,
+                    m_prevFrame, currFrame,
+                    m_settings.m_useAbandonedDetection ? cvRound(m_settings.m_minStaticTime * fps) : 0);
+        }
+        else				     // if not continue using predictions
+        {
+            m_tracks[i]->Update(CRegion(), false, m_settings.m_maxTraceLength, m_prevFrame, currFrame, 0);
+        }
+    }
+}
+
+///
+/// \brief CTracker::CreateDistaceMatrix
+/// \param regions
+/// \param costMatrix
+/// \param maxPossibleCost
+/// \param maxCost
+///
+void CTracker::CreateDistaceMatrix(const regions_t& regions, distMatrix_t& costMatrix, track_t maxPossibleCost, track_t& maxCost, cv::UMat currFrame)
+{
+    const size_t N = m_tracks.size();	// Tracking objects
+    maxCost = 0;
+
+    for (size_t i = 0; i < N; ++i)
+    {
+        const auto& track = m_tracks[i];
+
+        // Calc predicted area for track
+        cv::Size_<track_t> minRadius;
+        if (m_settings.m_minAreaRadiusPix < 0)
+        {
+            minRadius.width = m_settings.m_minAreaRadiusK * track->LastRegion().m_rrect.size.width;
+            minRadius.height = m_settings.m_minAreaRadiusK * track->LastRegion().m_rrect.size.height;
+        }
+        else
+        {
+            minRadius.width = m_settings.m_minAreaRadiusPix;
+            minRadius.height = m_settings.m_minAreaRadiusPix;
+        }
+        cv::RotatedRect predictedArea = track->CalcPredictionEllipse(minRadius);
+
+        // Calc distance between track and regions
+        for (size_t j = 0; j < regions.size(); ++j)
+        {
+            const auto& reg = regions[j];
+
+            auto dist = maxPossibleCost;
+            if (m_settings.CheckType(m_tracks[i]->LastRegion().m_type, reg.m_type))
+            {
+                dist = 0;
+                size_t ind = 0;
+                if (m_settings.m_distType[ind] > 0.0f && ind == tracking::DistCenters)
+                {
+#if 1
+                    track_t ellipseDist = track->IsInsideArea(reg.m_rrect.center, predictedArea);
+                    if (ellipseDist > 1)
+                        dist += m_settings.m_distType[ind];
+                    else
+                        dist += ellipseDist * m_settings.m_distType[ind];
+#else
+                    dist += m_settings.m_distType[ind] * track->CalcDistCenter(reg);
+#endif
+                }
+                ++ind;
+
+                if (m_settings.m_distType[ind] > 0.0f && ind == tracking::DistRects)
+                {
+#if 1
+                    track_t ellipseDist = track->IsInsideArea(reg.m_rrect.center, predictedArea);
+                    if (ellipseDist < 1)
+                    {
+                        track_t dw = track->WidthDist(reg);
+                        track_t dh = track->HeightDist(reg);
+                        dist += m_settings.m_distType[ind] * (1 - (1 - ellipseDist) * (dw + dh) * 0.5f);
+                    }
+                    else
+                    {
+                        dist += m_settings.m_distType[ind];
+                    }
+                    //std::cout << "dist = " << dist << ", ed = " << ellipseDist << ", dw = " << dw << ", dh = " << dh << std::endl;
+#else
+                    dist += m_settings.m_distType[ind] * track->CalcDistRect(reg);
+#endif
+                }
+                ++ind;
+
+                if (m_settings.m_distType[ind] > 0.0f && ind == tracking::DistJaccard)
+                    dist += m_settings.m_distType[ind] * track->CalcDistJaccard(reg);
+                ++ind;
+
+                if (m_settings.m_distType[ind] > 0.0f && ind == tracking::DistHist)
+                    dist += m_settings.m_distType[ind] * track->CalcDistHist(reg, currFrame);
+                ++ind;
+                assert(ind == tracking::DistsCount);
+            }
+            costMatrix[i + j * N] = dist;
+            if (dist > maxCost)
+                maxCost = dist;
+        }
+    }
+}

src/fusion/lidar_radar_fusion_cnn/Tracker/Ctracker.h

@@ -0,0 +1,257 @@
+#pragma once
+#include <iostream>
+#include <vector>
+#include <memory>
+#include <array>
+#include <deque>
+#include <numeric>
+#include <map>
+#include <set>
+
+#include "defines.h"
+#include "track.h"
+#include "ShortPathCalculator.h"
+
+// ----------------------------------------------------------------------
+
+///
+/// \brief The TrackerSettings struct
+///
+struct TrackerSettings
+{
+    tracking::KalmanType m_kalmanType = tracking::KalmanLinear;
+    tracking::FilterGoal m_filterGoal = tracking::FilterCenter;
+    tracking::LostTrackType m_lostTrackType = tracking::TrackKCF;
+    tracking::MatchType m_matchType = tracking::MatchHungrian;
+
+	std::array<track_t, tracking::DistsCount> m_distType;
+
+    ///
+    /// \brief m_dt
+    /// Time step for Kalman
+    ///
+    track_t m_dt = 1.0f;
+
+    ///
+    /// \brief m_accelNoiseMag
+    /// Noise magnitude for Kalman
+    ///
+    track_t m_accelNoiseMag = 0.1f;
+
+	///
+	/// \brief m_useAcceleration
+	/// Constant velocity or constant acceleration motion model
+	///
+	bool m_useAcceleration = false;
+
+    ///
+    /// \brief m_distThres
+    /// Distance threshold for Assignment problem: from 0 to 1
+    ///
+    track_t m_distThres = 0.8f;
+
+    ///
+    /// \brief m_minAreaRadius
+    /// Minimal area radius in pixels for objects centers
+    ///
+    track_t m_minAreaRadiusPix = 20.f;
+
+	///
+	/// \brief m_minAreaRadius
+	/// Minimal area radius in ration for object size.. Used if m_minAreaRadiusPix < 0
+	///
+	track_t m_minAreaRadiusK = 0.5f;
+
+    ///
+    /// \brief m_maximumAllowedSkippedFrames
+    /// If the object don't assignment more than this frames then it will be removed
+    ///
+    size_t m_maximumAllowedSkippedFrames = 25;
+
+    ///
+    /// \brief m_maxTraceLength
+    /// The maximum trajectory length
+    ///
+    size_t m_maxTraceLength = 50;
+
+    ///
+    /// \brief m_useAbandonedDetection
+    /// Detection abandoned objects
+    ///
+    bool m_useAbandonedDetection = false;
+
+    ///
+    /// \brief m_minStaticTime
+    /// After this time (in seconds) the object is considered abandoned
+    ///
+    int m_minStaticTime = 5;
+    ///
+    /// \brief m_maxStaticTime
+    /// After this time (in seconds) the abandoned object will be removed
+    ///
+    int m_maxStaticTime = 25;
+
+	///
+	/// \brief m_nearTypes
+	/// Object types that can be matched while tracking
+	///
+	std::map<std::string, std::set<std::string>> m_nearTypes;
+
+	///
+	TrackerSettings()
+	{
+		m_distType[tracking::DistCenters] = 0.0f;
+		m_distType[tracking::DistRects] = 0.0f;
+		m_distType[tracking::DistJaccard] = 0.5f;
+		m_distType[tracking::DistHist] = 0.5f;
+
+		assert(CheckDistance());
+	}
+
+	///
+	bool CheckDistance() const
+	{
+		track_t sum = std::accumulate(m_distType.begin(), m_distType.end(), 0.0f);
+		track_t maxOne = std::max(1.0f, std::fabs(sum));
+		return std::fabs(sum - 1.0f) <= std::numeric_limits<track_t>::epsilon() * maxOne;
+	}
+
+	///
+	bool SetDistances(std::array<track_t, tracking::DistsCount> distType)
+	{
+		bool res = true;
+		auto oldDists = m_distType;
+		m_distType = distType;
+		if (!CheckDistance())
+		{
+			m_distType = oldDists;
+			res = false;
+		}
+		return res;
+	}
+
+	///
+	bool SetDistance(tracking::DistType distType)
+	{
+		std::fill(m_distType.begin(), m_distType.end(), 0.0f);
+		m_distType[distType] = 1.f;
+		return true;
+	}
+
+	///
+	void AddNearTypes(const std::string& type1, const std::string& type2, bool sym)
+	{
+		auto AddOne = [&](const std::string& type1, const std::string& type2)
+		{
+			auto it = m_nearTypes.find(type1);
+			if (it == std::end(m_nearTypes))
+			{
+				m_nearTypes[type1] = std::set<std::string>{ type2 };
+			}
+			else
+			{
+				it->second.insert(type2);
+			}
+		};
+		AddOne(type1, type2);
+		if (sym)
+		{
+			AddOne(type2, type1);
+		}
+	}
+
+	///
+	bool CheckType(const std::string& type1, const std::string& type2) const
+	{
+		bool res = type1.empty() || type2.empty() || (type1 == type2);
+		if (!res)
+		{
+			auto it = m_nearTypes.find(type1);
+			if (it != std::end(m_nearTypes))
+			{
+				res = it->second.find(type2) != std::end(it->second);
+			}
+		}
+		return res;
+	}
+};
+
+///
+/// \brief The CTracker class
+///
+class CTracker
+{
+public:
+    CTracker(TrackerSettings& settings);
+    CTracker(const TrackerSettings& settings);
+    CTracker(const CTracker&) = delete;
+    CTracker(CTracker&&) = delete;
+	CTracker& operator=(const CTracker&) = delete;
+	CTracker& operator=(CTracker&&) = delete;
+	
+	~CTracker(void);
+    void setSettings(TrackerSettings& settings);
+    void Update(const regions_t& regions, cv::UMat currFrame, float fps);
+
+    ///
+    /// \brief CanGrayFrameToTrack
+    /// \return
+    ///
+    bool CanGrayFrameToTrack() const
+    {
+        bool needColor = (m_settings.m_lostTrackType == tracking::LostTrackType::TrackGOTURN);/* ||
+            (m_settings.m_lostTrackType == tracking::LostTrackType::TrackDAT) ||
+            (m_settings.m_lostTrackType == tracking::LostTrackType::TrackSTAPLE) ||
+            (m_settings.m_lostTrackType == tracking::LostTrackType::TrackLDES);*/
+        return !needColor;
+    }
+
+	///
+	/// \brief CanColorFrameToTrack
+	/// \return
+	///
+	bool CanColorFrameToTrack() const
+	{
+		return true;
+	}
+
+    ///
+    /// \brief GetTracksCount
+    /// \return
+    ///
+	size_t GetTracksCount() const
+	{
+		return m_tracks.size();
+	}
+    ///
+    /// \brief GetTracks
+    /// \return
+    ///
+	std::vector<TrackingObject> GetTracks() const
+	{
+		std::vector<TrackingObject> tracks;
+		if (!m_tracks.empty())
+		{
+			tracks.reserve(m_tracks.size());
+			for (const auto& track : m_tracks)
+			{
+                tracks.push_back(track->ConstructObject());
+			}
+		}
+		return tracks;
+	}
+
+private:
+    TrackerSettings m_settings;
+
+	tracks_t m_tracks;
+
+    size_t m_nextTrackID;
+
+    cv::UMat m_prevFrame;
+
+    std::unique_ptr<ShortPathCalculator> m_SPCalculator;
+
+    void CreateDistaceMatrix(const regions_t& regions, distMatrix_t& costMatrix, track_t maxPossibleCost, track_t& maxCost, cv::UMat currFrame);
+    void UpdateTrackingState(const regions_t& regions, cv::UMat currFrame, float fps);
+};

src/fusion/lidar_radar_fusion_cnn/Tracker/HungarianAlg.cpp

@@ -0,0 +1,723 @@
+#include "HungarianAlg.h"
+#include <limits>
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+AssignmentProblemSolver::AssignmentProblemSolver()
+{
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+AssignmentProblemSolver::~AssignmentProblemSolver()
+{
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+track_t AssignmentProblemSolver::Solve(
+	const distMatrix_t& distMatrixIn,
+	size_t nOfRows,
+	size_t nOfColumns,
+	std::vector<int>& assignment,
+	TMethod Method
+	)
+{
+	assignment.resize(nOfRows, -1);
+
+	track_t cost = 0;
+
+	switch (Method)
+	{
+	case optimal:
+		assignmentoptimal(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
+		break;
+
+	case many_forbidden_assignments:
+		assignmentsuboptimal1(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
+		break;
+
+	case without_forbidden_assignments:
+		assignmentsuboptimal2(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
+		break;
+	}
+
+	return cost;
+}
+// --------------------------------------------------------------------------
+// Computes the optimal assignment (minimum overall costs) using Munkres algorithm.
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
+{
+	// Generate distance cv::Matrix 
+	// and check cv::Matrix elements positiveness :)
+
+	// Total elements number
+	size_t nOfElements = nOfRows * nOfColumns;
+	// Memory allocation
+	track_t* distMatrix = (track_t *)malloc(nOfElements * sizeof(track_t));
+
+    if (distMatrix == nullptr)
+    {
+        return;
+    }
+
+	// Pointer to last element
+	track_t* distMatrixEnd = distMatrix + nOfElements;
+
+	for (size_t row = 0; row < nOfElements; row++)
+	{
+        track_t value = distMatrixIn[row];
+        assert(value >= 0);
+        distMatrix[row] = value;
+	}
+
+	// Memory allocation
+	bool* coveredColumns = (bool*)calloc(nOfColumns, sizeof(bool));
+	bool* coveredRows = (bool*)calloc(nOfRows, sizeof(bool));
+	bool* starMatrix = (bool*)calloc(nOfElements, sizeof(bool));
+	bool* primeMatrix = (bool*)calloc(nOfElements, sizeof(bool));
+	bool* newStarMatrix = (bool*)calloc(nOfElements, sizeof(bool)); /* used in step4 */
+
+	/* preliminary steps */
+	if (nOfRows <= nOfColumns)
+	{
+		for (size_t row = 0; row < nOfRows; row++)
+		{
+			/* find the smallest element in the row */
+            track_t* distMatrixTemp = distMatrix + row;
+			track_t  minValue = *distMatrixTemp;
+			distMatrixTemp += nOfRows;
+			while (distMatrixTemp < distMatrixEnd)
+			{
+				track_t value = *distMatrixTemp;
+				if (value < minValue)
+				{
+					minValue = value;
+				}
+				distMatrixTemp += nOfRows;
+			}
+			/* subtract the smallest element from each element of the row */
+			distMatrixTemp = distMatrix + row;
+			while (distMatrixTemp < distMatrixEnd)
+			{
+				*distMatrixTemp -= minValue;
+				distMatrixTemp += nOfRows;
+			}
+		}
+		/* Steps 1 and 2a */
+		for (size_t row = 0; row < nOfRows; row++)
+		{
+			for (size_t col = 0; col < nOfColumns; col++)
+			{
+				if (distMatrix[row + nOfRows*col] == 0)
+				{
+					if (!coveredColumns[col])
+					{
+						starMatrix[row + nOfRows * col] = true;
+						coveredColumns[col] = true;
+						break;
+					}
+				}
+			}
+		}
+	}
+	else /* if(nOfRows > nOfColumns) */
+	{
+		for (size_t col = 0; col < nOfColumns; col++)
+		{
+			/* find the smallest element in the column */
+			track_t* distMatrixTemp = distMatrix + nOfRows*col;
+			track_t* columnEnd = distMatrixTemp + nOfRows;
+			track_t  minValue = *distMatrixTemp++;
+			while (distMatrixTemp < columnEnd)
+			{
+				track_t value = *distMatrixTemp++;
+				if (value < minValue)
+				{
+					minValue = value;
+				}
+			}
+			/* subtract the smallest element from each element of the column */
+			distMatrixTemp = distMatrix + nOfRows*col;
+			while (distMatrixTemp < columnEnd)
+			{
+				*distMatrixTemp++ -= minValue;
+			}
+		}
+		/* Steps 1 and 2a */
+		for (size_t col = 0; col < nOfColumns; col++)
+		{
+			for (size_t row = 0; row < nOfRows; row++)
+			{
+				if (distMatrix[row + nOfRows*col] == 0)
+				{
+					if (!coveredRows[row])
+					{
+						starMatrix[row + nOfRows*col] = true;
+						coveredColumns[col] = true;
+						coveredRows[row] = true;
+						break;
+					}
+				}
+			}
+		}
+
+		for (size_t row = 0; row < nOfRows; row++)
+		{
+			coveredRows[row] = false;
+		}
+	}
+	/* move to step 2b */
+	step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, (nOfRows <= nOfColumns) ? nOfRows : nOfColumns);
+	/* compute cost and remove invalid assignments */
+	computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);
+	/* free allocated memory */
+	free(distMatrix);
+	free(coveredColumns);
+	free(coveredRows);
+	free(starMatrix);
+	free(primeMatrix);
+	free(newStarMatrix);
+	return;
+}
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::buildassignmentvector(assignments_t& assignment, bool *starMatrix, size_t nOfRows, size_t nOfColumns)
+{
+    for (size_t row = 0; row < nOfRows; row++)
+	{
+        for (size_t col = 0; col < nOfColumns; col++)
+		{
+			if (starMatrix[row + nOfRows * col])
+			{
+				assignment[row] = static_cast<int>(col);
+				break;
+			}
+		}
+	}
+}
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows)
+{
+	for (size_t row = 0; row < nOfRows; row++)
+	{
+		const int col = assignment[row];
+		if (col >= 0)
+		{
+			cost += distMatrixIn[row + nOfRows * col];
+		}
+	}
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step2a(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim)
+{
+    bool *starMatrixTemp, *columnEnd;
+    /* cover every column containing a starred zero */
+    for (size_t col = 0; col < nOfColumns; col++)
+    {
+        starMatrixTemp = starMatrix + nOfRows * col;
+        columnEnd = starMatrixTemp + nOfRows;
+        while (starMatrixTemp < columnEnd)
+        {
+            if (*starMatrixTemp++)
+            {
+                coveredColumns[col] = true;
+                break;
+            }
+        }
+    }
+    /* move to step 3 */
+	step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step2b(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim)
+{
+	/* count covered columns */
+    size_t nOfCoveredColumns = 0;
+    for (size_t col = 0; col < nOfColumns; col++)
+	{
+		if (coveredColumns[col])
+		{
+			nOfCoveredColumns++;
+		}
+	}
+	if (nOfCoveredColumns == minDim)
+	{
+		/* algorithm finished */
+		buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
+	}
+	else
+	{
+		/* move to step 3 */
+		step3_5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
+	}
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step3_5(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim)
+{
+	for (;;)
+	{
+		/* step 3 */
+		bool zerosFound = true;
+		while (zerosFound)
+		{
+			zerosFound = false;
+			for (size_t col = 0; col < nOfColumns; col++)
+			{
+				if (!coveredColumns[col])
+				{
+					for (size_t row = 0; row < nOfRows; row++)
+					{
+						if ((!coveredRows[row]) && (distMatrix[row + nOfRows*col] == 0))
+						{
+							/* prime zero */
+							primeMatrix[row + nOfRows*col] = true;
+							/* find starred zero in current row */
+							size_t starCol = 0;
+							for (; starCol < nOfColumns; starCol++)
+							{
+								if (starMatrix[row + nOfRows * starCol])
+								{
+									break;
+								}
+							}
+							if (starCol == nOfColumns) /* no starred zero found */
+							{
+								/* move to step 4 */
+								step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim, row, col);
+								return;
+							}
+							else
+							{
+								coveredRows[row] = true;
+								coveredColumns[starCol] = false;
+								zerosFound = true;
+								break;
+							}
+						}
+					}
+				}
+			}
+		}
+		/* step 5 */
+        track_t h = std::numeric_limits<track_t>::max();
+		for (size_t row = 0; row < nOfRows; row++)
+		{
+			if (!coveredRows[row])
+			{
+				for (size_t col = 0; col < nOfColumns; col++)
+				{
+					if (!coveredColumns[col])
+					{
+                        const track_t value = distMatrix[row + nOfRows*col];
+						if (value < h)
+						{
+							h = value;
+						}
+					}
+				}
+			}
+		}
+		/* add h to each covered row */
+		for (size_t row = 0; row < nOfRows; row++)
+		{
+			if (coveredRows[row])
+			{
+				for (size_t col = 0; col < nOfColumns; col++)
+				{
+					distMatrix[row + nOfRows*col] += h;
+				}
+			}
+		}
+		/* subtract h from each uncovered column */
+		for (size_t col = 0; col < nOfColumns; col++)
+		{
+			if (!coveredColumns[col])
+			{
+				for (size_t row = 0; row < nOfRows; row++)
+				{
+					distMatrix[row + nOfRows*col] -= h;
+				}
+			}
+		}
+	}
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step4(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, size_t col)
+{
+	const size_t nOfElements = nOfRows * nOfColumns;
+	/* generate temporary copy of starMatrix */
+	for (size_t n = 0; n < nOfElements; n++)
+	{
+		newStarMatrix[n] = starMatrix[n];
+	}
+	/* star current zero */
+	newStarMatrix[row + nOfRows*col] = true;
+	/* find starred zero in current column */
+	size_t starCol = col;
+	size_t starRow = 0;
+	for (; starRow < nOfRows; starRow++)
+	{
+		if (starMatrix[starRow + nOfRows * starCol])
+		{
+			break;
+		}
+	}
+	while (starRow < nOfRows)
+	{
+		/* unstar the starred zero */
+		newStarMatrix[starRow + nOfRows*starCol] = false;
+		/* find primed zero in current row */
+		size_t primeRow = starRow;
+		size_t primeCol = 0;
+		for (; primeCol < nOfColumns; primeCol++)
+		{
+			if (primeMatrix[primeRow + nOfRows * primeCol])
+			{
+				break;
+			}
+		}
+		/* star the primed zero */
+		newStarMatrix[primeRow + nOfRows*primeCol] = true;
+		/* find starred zero in current column */
+		starCol = primeCol;
+		for (starRow = 0; starRow < nOfRows; starRow++)
+		{
+			if (starMatrix[starRow + nOfRows * starCol])
+			{
+				break;
+			}
+		}
+	}
+	/* use temporary copy as new starMatrix */
+	/* delete all primes, uncover all rows */
+    for (size_t n = 0; n < nOfElements; n++)
+	{
+		primeMatrix[n] = false;
+		starMatrix[n] = newStarMatrix[n];
+	}
+    for (size_t n = 0; n < nOfRows; n++)
+	{
+		coveredRows[n] = false;
+	}
+	/* move to step 2a */
+	step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
+}
+
+// --------------------------------------------------------------------------
+// Computes a suboptimal solution. Good for cases without forbidden assignments.
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
+{
+	/* make working copy of distance Matrix */
+	const size_t nOfElements = nOfRows * nOfColumns;
+	track_t* distMatrix = (track_t*)malloc(nOfElements * sizeof(track_t));
+	for (size_t n = 0; n < nOfElements; n++)
+	{
+		distMatrix[n] = distMatrixIn[n];
+	}
+
+	/* recursively search for the minimum element and do the assignment */
+	for (;;)
+	{
+		/* find minimum distance observation-to-track pair */
+		track_t minValue = std::numeric_limits<track_t>::max();
+		size_t tmpRow = 0;
+		size_t tmpCol = 0;
+		for (size_t row = 0; row < nOfRows; row++)
+		{
+			for (size_t col = 0; col < nOfColumns; col++)
+			{
+				const track_t value = distMatrix[row + nOfRows*col];
+				if (value != std::numeric_limits<track_t>::max() && (value < minValue))
+				{
+					minValue = value;
+					tmpRow = row;
+					tmpCol = col;
+				}
+			}
+		}
+
+		if (minValue != std::numeric_limits<track_t>::max())
+		{
+			assignment[tmpRow] = static_cast<int>(tmpCol);
+			cost += minValue;
+			for (size_t n = 0; n < nOfRows; n++)
+			{
+				distMatrix[n + nOfRows*tmpCol] = std::numeric_limits<track_t>::max();
+			}
+			for (size_t n = 0; n < nOfColumns; n++)
+			{
+				distMatrix[tmpRow + nOfRows*n] = std::numeric_limits<track_t>::max();
+			}
+		}
+		else
+		{
+			break;
+		}
+	}
+
+	free(distMatrix);
+}
+// --------------------------------------------------------------------------
+// Computes a suboptimal solution. Good for cases with many forbidden assignments.
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
+{
+	/* make working copy of distance Matrix */
+	const size_t nOfElements = nOfRows * nOfColumns;
+	track_t* distMatrix = (track_t *)malloc(nOfElements * sizeof(track_t));
+    for (size_t n = 0; n < nOfElements; n++)
+	{
+		distMatrix[n] = distMatrixIn[n];
+	}
+
+	/* allocate memory */
+	int* nOfValidObservations = (int *)calloc(nOfRows, sizeof(int));
+	int* nOfValidTracks = (int *)calloc(nOfColumns, sizeof(int));
+
+	/* compute number of validations */
+	bool infiniteValueFound = false;
+	bool finiteValueFound = false;
+	for (size_t row = 0; row < nOfRows; row++)
+	{
+		for (size_t col = 0; col < nOfColumns; col++)
+		{
+			if (distMatrix[row + nOfRows*col] != std::numeric_limits<track_t>::max())
+			{
+				nOfValidTracks[col] += 1;
+				nOfValidObservations[row] += 1;
+				finiteValueFound = true;
+			}
+			else
+			{
+				infiniteValueFound = true;
+			}
+		}
+	}
+
+	if (infiniteValueFound)
+	{
+		if (!finiteValueFound)
+		{
+            /* free allocated memory */
+            free(nOfValidObservations);
+            free(nOfValidTracks);
+            free(distMatrix);
+
+			return;
+		}
+		bool repeatSteps = true;
+
+		while (repeatSteps)
+		{
+			repeatSteps = false;
+
+			/* step 1: reject assignments of multiply validated tracks to singly validated observations		 */
+			for (size_t col = 0; col < nOfColumns; col++)
+			{
+				bool singleValidationFound = false;
+				for (size_t row = 0; row < nOfRows; row++)
+				{
+					if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() && (nOfValidObservations[row] == 1))
+					{
+						singleValidationFound = true;
+						break;
+					}
+				}
+				if (singleValidationFound)
+				{
+					for (size_t nestedRow = 0; nestedRow < nOfRows; nestedRow++)
+						if ((nOfValidObservations[nestedRow] > 1) && distMatrix[nestedRow + nOfRows * col] != std::numeric_limits<track_t>::max())
+						{
+							distMatrix[nestedRow + nOfRows * col] = std::numeric_limits<track_t>::max();
+							nOfValidObservations[nestedRow] -= 1;
+							nOfValidTracks[col] -= 1;
+							repeatSteps = true;
+						}
+				}
+			}
+
+			/* step 2: reject assignments of multiply validated observations to singly validated tracks */
+			if (nOfColumns > 1)
+			{
+				for (size_t row = 0; row < nOfRows; row++)
+				{
+					bool singleValidationFound = false;
+                    for (size_t col = 0; col < nOfColumns; col++)
+					{
+						if (distMatrix[row + nOfRows*col] != std::numeric_limits<track_t>::max() && (nOfValidTracks[col] == 1))
+						{
+							singleValidationFound = true;
+							break;
+						}
+					}
+
+					if (singleValidationFound)
+					{
+						for (size_t col = 0; col < nOfColumns; col++)
+						{
+							if ((nOfValidTracks[col] > 1) && distMatrix[row + nOfRows*col] != std::numeric_limits<track_t>::max())
+							{
+								distMatrix[row + nOfRows*col] = std::numeric_limits<track_t>::max();
+								nOfValidObservations[row] -= 1;
+								nOfValidTracks[col] -= 1;
+								repeatSteps = true;
+							}
+						}
+					}
+				}
+			}
+		} /* while(repeatSteps) */
+
+		/* for each multiply validated track that validates only with singly validated  */
+		/* observations, choose the observation with minimum distance */
+        for (size_t row = 0; row < nOfRows; row++)
+		{
+			if (nOfValidObservations[row] > 1)
+			{
+				bool allSinglyValidated = true;
+				track_t minValue = std::numeric_limits<track_t>::max();
+				size_t tmpCol = 0;
+                for (size_t col = 0; col < nOfColumns; col++)
+				{
+					const track_t value = distMatrix[row + nOfRows*col];
+					if (value != std::numeric_limits<track_t>::max())
+					{
+						if (nOfValidTracks[col] > 1)
+						{
+							allSinglyValidated = false;
+							break;
+						}
+						else if ((nOfValidTracks[col] == 1) && (value < minValue))
+						{
+							tmpCol = col;
+							minValue = value;
+						}
+					}
+				}
+
+				if (allSinglyValidated)
+				{
+					assignment[row] = static_cast<int>(tmpCol);
+					cost += minValue;
+					for (size_t n = 0; n < nOfRows; n++)
+					{
+						distMatrix[n + nOfRows*tmpCol] = std::numeric_limits<track_t>::max();
+					}
+					for (size_t n = 0; n < nOfColumns; n++)
+					{
+						distMatrix[row + nOfRows*n] = std::numeric_limits<track_t>::max();
+					}
+				}
+			}
+		}
+
+		// for each multiply validated observation that validates only with singly validated  track, choose the track with minimum distance
+        for (size_t col = 0; col < nOfColumns; col++)
+		{
+			if (nOfValidTracks[col] > 1)
+			{
+				bool allSinglyValidated = true;
+				track_t minValue = std::numeric_limits<track_t>::max();
+				size_t tmpRow = 0;
+				for (size_t row = 0; row < nOfRows; row++)
+				{
+					const track_t value = distMatrix[row + nOfRows*col];
+					if (value != std::numeric_limits<track_t>::max())
+					{
+						if (nOfValidObservations[row] > 1)
+						{
+							allSinglyValidated = false;
+							break;
+						}
+						else if ((nOfValidObservations[row] == 1) && (value < minValue))
+						{
+							tmpRow = row;
+							minValue = value;
+						}
+					}
+				}
+
+				if (allSinglyValidated)
+				{
+					assignment[tmpRow] = static_cast<int>(col);
+					cost += minValue;
+					for (size_t n = 0; n < nOfRows; n++)
+					{
+						distMatrix[n + nOfRows*col] = std::numeric_limits<track_t>::max();
+					}
+					for (size_t n = 0; n < nOfColumns; n++)
+					{
+						distMatrix[tmpRow + nOfRows*n] = std::numeric_limits<track_t>::max();
+					}
+				}
+			}
+		}
+	} /* if(infiniteValueFound) */
+
+
+	/* now, recursively search for the minimum element and do the assignment */
+	for (;;)
+	{
+		/* find minimum distance observation-to-track pair */
+		track_t minValue = std::numeric_limits<track_t>::max();
+		size_t tmpRow = 0;
+		size_t tmpCol = 0;
+		for (size_t row = 0; row < nOfRows; row++)
+		{
+			for (size_t col = 0; col < nOfColumns; col++)
+			{
+				const track_t value = distMatrix[row + nOfRows*col];
+				if (value != std::numeric_limits<track_t>::max() && (value < minValue))
+				{
+					minValue = value;
+					tmpRow = row;
+					tmpCol = col;
+				}
+			}
+		}
+
+		if (minValue != std::numeric_limits<track_t>::max())
+		{
+			assignment[tmpRow] = static_cast<int>(tmpCol);
+			cost += minValue;
+			for (size_t n = 0; n < nOfRows; n++)
+			{
+				distMatrix[n + nOfRows*tmpCol] = std::numeric_limits<track_t>::max();
+			}
+			for (size_t n = 0; n < nOfColumns; n++)
+			{
+				distMatrix[tmpRow + nOfRows*n] = std::numeric_limits<track_t>::max();
+			}
+		}
+		else
+		{
+			break;
+		}
+	}
+
+	/* free allocated memory */
+	free(nOfValidObservations);
+	free(nOfValidTracks);
+	free(distMatrix);
+}

src/fusion/lidar_radar_fusion_cnn/Tracker/HungarianAlg.h

@@ -0,0 +1,39 @@
+#include <vector>
+#include <iostream>
+#include <limits>
+#include <time.h>
+#include "defines.h"
+// http://community.topcoder.com/tc?module=Static&d1=tutorials&d2=hungarianAlgorithm
+
+///
+/// \brief The AssignmentProblemSolver class
+///
+class AssignmentProblemSolver
+{
+private:
+	// Computes the optimal assignment (minimum overall costs) using Munkres algorithm.
+	void assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns);
+	void buildassignmentvector(assignments_t& assignment, bool *starMatrix, size_t nOfRows, size_t nOfColumns);
+	void computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows);
+	void step2a(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
+	void step2b(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
+	void step3_5(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
+	void step4(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, size_t col);
+
+	// Computes a suboptimal solution. Good for cases with many forbidden assignments.
+	void assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns);
+	// Computes a suboptimal solution. Good for cases with many forbidden assignments.
+	void assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns);
+
+public:
+	enum TMethod
+	{
+		optimal,
+		many_forbidden_assignments,
+		without_forbidden_assignments
+	};
+
+	AssignmentProblemSolver();
+	~AssignmentProblemSolver();
+	track_t Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns, assignments_t& assignment, TMethod Method = optimal);
+};

src/fusion/lidar_radar_fusion_cnn/Tracker/Kalman.cpp

@@ -0,0 +1,921 @@
+#include "Kalman.h"
+#include <iostream>
+#include <vector>
+
+//---------------------------------------------------------------------------
+TKalmanFilter::TKalmanFilter(
+        tracking::KalmanType type,
+	    bool useAcceleration,
+        track_t deltaTime, // time increment (lower values makes target more "massive")
+        track_t accelNoiseMag
+        )
+    :
+      m_type(type),
+      m_initialized(false),
+      m_deltaTime(deltaTime),
+      m_deltaTimeMin(deltaTime),
+      m_deltaTimeMax(2 * deltaTime),
+      m_lastDist(0),
+      m_accelNoiseMag(accelNoiseMag),
+	  m_useAcceleration(useAcceleration)
+{
+    m_deltaStep = (m_deltaTimeMax - m_deltaTimeMin) / m_deltaStepsCount;
+}
+
+//---------------------------------------------------------------------------
+void TKalmanFilter::CreateLinear(Point_t xy0, Point_t xyv0)
+{
+    // We don't know acceleration, so, assume it to process noise.
+    // But we can guess, the range of acceleration values thich can be achieved by tracked object.
+    // Process noise. (standard deviation of acceleration: m/s^2)
+    // shows, woh much target can accelerate.
+
+    // 4 state variables, 2 measurements
+    m_linearKalman.init(4, 2, 0, El_t);
+    // Transition cv::Matrix
+    m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(4, 4) <<
+                                        1, 0, m_deltaTime, 0,
+                                        0, 1, 0, m_deltaTime,
+                                        0, 0, 1, 0,
+                                        0, 0, 0, 1);
+
+    // init...
+    m_lastPointResult = xy0;
+    m_linearKalman.statePre.at<track_t>(0) = xy0.x;  // x
+    m_linearKalman.statePre.at<track_t>(1) = xy0.y;  // y
+    m_linearKalman.statePre.at<track_t>(2) = xyv0.x; // vx
+    m_linearKalman.statePre.at<track_t>(3) = xyv0.y; // vy
+
+    m_linearKalman.statePost.at<track_t>(0) = xy0.x;
+    m_linearKalman.statePost.at<track_t>(1) = xy0.y;
+    m_linearKalman.statePost.at<track_t>(2) = xyv0.x;
+    m_linearKalman.statePost.at<track_t>(3) = xyv0.y;
+
+    cv::setIdentity(m_linearKalman.measurementMatrix);
+
+    m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(4, 4) <<
+                                       pow(m_deltaTime,4.0)/4.0	,0						,pow(m_deltaTime,3.0)/2.0		,0,
+                                       0						,pow(m_deltaTime,4.0)/4.0	,0							,pow(m_deltaTime,3.0)/2.0,
+                                       pow(m_deltaTime,3.0)/2.0	,0						,pow(m_deltaTime,2.0)			,0,
+                                       0						,pow(m_deltaTime,3.0)/2.0	,0							,pow(m_deltaTime,2.0));
+
+
+    m_linearKalman.processNoiseCov *= m_accelNoiseMag;
+
+    cv::setIdentity(m_linearKalman.measurementNoiseCov, cv::Scalar::all(0.1));
+
+    cv::setIdentity(m_linearKalman.errorCovPost, cv::Scalar::all(.1));
+
+    m_initialized = true;
+}
+
+//---------------------------------------------------------------------------
+void TKalmanFilter::CreateLinear(cv::Rect_<track_t> rect0, Point_t rectv0)
+{
+    // We don't know acceleration, so, assume it to process noise.
+    // But we can guess, the range of acceleration values thich can be achieved by tracked object.
+    // Process noise. (standard deviation of acceleration: m/s^2)
+    // shows, woh much target can accelerate.
+
+    // 8 state variables (x, y, vx, vy, width, height, vw, vh), 4 measurements (x, y, width, height)
+    m_linearKalman.init(8, 4, 0, El_t);
+    // Transition cv::Matrix
+    m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(8, 8) <<
+                                        1, 0, 0, 0, m_deltaTime, 0,           0,           0,
+                                        0, 1, 0, 0, 0,           m_deltaTime, 0,           0,
+                                        0, 0, 1, 0, 0,           0,           m_deltaTime, 0,
+                                        0, 0, 0, 1, 0,           0,           0,           m_deltaTime,
+                                        0, 0, 0, 0, 1,           0,           0,           0,
+                                        0, 0, 0, 0, 0,           1,           0,           0,
+                                        0, 0, 0, 0, 0,           0,           1,           0,
+                                        0, 0, 0, 0, 0,           0,           0,           1);
+
+    // init...
+    m_linearKalman.statePre.at<track_t>(0) = rect0.x;      // x
+    m_linearKalman.statePre.at<track_t>(1) = rect0.y;      // y
+    m_linearKalman.statePre.at<track_t>(2) = rect0.width;  // width
+    m_linearKalman.statePre.at<track_t>(3) = rect0.height; // height
+    m_linearKalman.statePre.at<track_t>(4) = rectv0.x;     // dx
+    m_linearKalman.statePre.at<track_t>(5) = rectv0.y;     // dy
+    m_linearKalman.statePre.at<track_t>(6) = 0;            // dw
+    m_linearKalman.statePre.at<track_t>(7) = 0;            // dh
+
+    m_linearKalman.statePost.at<track_t>(0) = rect0.x;
+    m_linearKalman.statePost.at<track_t>(1) = rect0.y;
+    m_linearKalman.statePost.at<track_t>(2) = rect0.width;
+    m_linearKalman.statePost.at<track_t>(3) = rect0.height;
+    m_linearKalman.statePost.at<track_t>(4) = rectv0.x;
+    m_linearKalman.statePost.at<track_t>(5) = rectv0.y;
+    m_linearKalman.statePost.at<track_t>(6) = 0;
+    m_linearKalman.statePost.at<track_t>(7) = 0;
+
+    cv::setIdentity(m_linearKalman.measurementMatrix);
+
+    track_t n1 = pow(m_deltaTime, 4.f) / 4.f;
+    track_t n2 = pow(m_deltaTime, 3.f) / 2.f;
+    track_t n3 = pow(m_deltaTime, 2.f);
+    m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(8, 8) <<
+                                       n1, 0,  0,  0,  n2, 0,  0,  0,
+                                       0,  n1, 0,  0,  0,  n2, 0,  0,
+                                       0,  0,  n1, 0,  0,  0,  n2, 0,
+                                       0,  0,  0,  n1, 0,  0,  0,  n2,
+                                       n2, 0,  0,  0,  n3, 0,  0,  0,
+                                       0,  n2, 0,  0,  0,  n3, 0,  0,
+                                       0,  0,  n2, 0,  0,  0,  n3, 0,
+                                       0,  0,  0,  n2, 0,  0,  0,  n3);
+
+    m_linearKalman.processNoiseCov *= m_accelNoiseMag;
+
+    cv::setIdentity(m_linearKalman.measurementNoiseCov, cv::Scalar::all(0.1));
+
+    cv::setIdentity(m_linearKalman.errorCovPost, cv::Scalar::all(.1));
+
+    m_initialized = true;
+}
+
+//---------------------------------------------------------------------------
+void TKalmanFilter::CreateLinearAcceleration(Point_t xy0, Point_t xyv0)
+{
+	// 6 state variables, 2 measurements
+	m_linearKalman.init(6, 2, 0, El_t);
+	// Transition cv::Matrix
+	const track_t dt = m_deltaTime;
+	const track_t dt2 = 0.5f * m_deltaTime * m_deltaTime;
+	m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(6, 6) <<
+		1, 0, dt, 0,  dt2, 0,
+		0, 1, 0,  dt, 0,   dt2,
+		0, 0, 1,  0,  dt,  0,
+		0, 0, 0,  1,  0,   dt,
+	    0, 0, 0,  0,  1,   0,
+	    0, 0, 0,  0,  0,   1);
+
+	// init...
+	m_lastPointResult = xy0;
+	m_linearKalman.statePre.at<track_t>(0) = xy0.x;  // x
+	m_linearKalman.statePre.at<track_t>(1) = xy0.y;  // y
+	m_linearKalman.statePre.at<track_t>(2) = xyv0.x; // vx
+	m_linearKalman.statePre.at<track_t>(3) = xyv0.y; // vy
+	m_linearKalman.statePre.at<track_t>(4) = 0;      // ax
+	m_linearKalman.statePre.at<track_t>(5) = 0;      // ay
+
+	m_linearKalman.statePost.at<track_t>(0) = xy0.x;
+	m_linearKalman.statePost.at<track_t>(1) = xy0.y;
+	m_linearKalman.statePost.at<track_t>(2) = xyv0.x;
+	m_linearKalman.statePost.at<track_t>(3) = xyv0.y;
+	m_linearKalman.statePost.at<track_t>(4) = 0;
+	m_linearKalman.statePost.at<track_t>(5) = 0;
+
+	cv::setIdentity(m_linearKalman.measurementMatrix);
+
+	track_t n1 = pow(m_deltaTime, 4.f) / 4.f;
+	track_t n2 = pow(m_deltaTime, 3.f) / 2.f;
+	track_t n3 = pow(m_deltaTime, 2.f);
+	m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(6, 6) <<
+		n1, 0, n2, 0, n2, 0,
+		0, n1, 0, n2, 0, n2,
+		n2, 0, n3, 0, n3, 0,
+		0, n2, 0, n3, 0, n3,
+		0, 0, n2, 0, n3, 0,
+		0, 0, 0, n2, 0, n3);
+
+	m_linearKalman.processNoiseCov *= m_accelNoiseMag;
+
+	cv::setIdentity(m_linearKalman.measurementNoiseCov, cv::Scalar::all(0.1));
+
+	cv::setIdentity(m_linearKalman.errorCovPost, cv::Scalar::all(.1));
+
+	m_initialized = true;
+}
+
+//---------------------------------------------------------------------------
+void TKalmanFilter::CreateLinearAcceleration(cv::Rect_<track_t> rect0, Point_t rectv0)
+{
+	// 12 state variables (x, y, vx, vy, ax, ay, width, height, vw, vh, aw, ah), 4 measurements (x, y, width, height)
+	m_linearKalman.init(12, 4, 0, El_t);
+	// Transition cv::Matrix
+	const track_t dt = m_deltaTime;
+	const track_t dt2 = 0.5f * m_deltaTime * m_deltaTime;
+	m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(12, 12) <<
+		1, 0, 0, 0, dt, 0,  0,  0,  dt2, 0,   dt2, 0,
+		0, 1, 0, 0, 0,  dt, 0,  0,  0,   dt2, 0,   dt2,
+		0, 0, 1, 0, 0,  0,  dt, 0,  0,   0,   dt2, 0,
+		0, 0, 0, 1, 0,  0,  0,  dt, 0,   0,   0,   dt2,
+		0, 0, 0, 0, 1,  0,  0,  0,  dt,  0,   0,   0,
+		0, 0, 0, 0, 0,  1,  0,  0,  0,   dt,  0,   0,
+		0, 0, 0, 0, 0,  0,  1,  0,  0,   0,   dt,  0,
+		0, 0, 0, 0, 0,  0,  0,  1,  0,   0,   0,   dt,
+		0, 0, 0, 0, 0,  0,  0,  0,  1,   0,   0,   0,
+		0, 0, 0, 0, 0,  0,  0,  0,  0,   1,   0,   0,
+		0, 0, 0, 0, 0,  0,  0,  0,  0,   0,   0,   1);
+
+	// init...
+	m_linearKalman.statePre.at<track_t>(0) = rect0.x;      // x
+	m_linearKalman.statePre.at<track_t>(1) = rect0.y;      // y
+	m_linearKalman.statePre.at<track_t>(2) = rect0.width;  // width
+	m_linearKalman.statePre.at<track_t>(3) = rect0.height; // height
+	m_linearKalman.statePre.at<track_t>(4) = rectv0.x;     // dx
+	m_linearKalman.statePre.at<track_t>(5) = rectv0.y;     // dy
+	m_linearKalman.statePre.at<track_t>(6) = 0;            // dw
+	m_linearKalman.statePre.at<track_t>(7) = 0;            // dh
+	m_linearKalman.statePre.at<track_t>(8) = 0;            // ax
+	m_linearKalman.statePre.at<track_t>(9) = 0;            // ay
+	m_linearKalman.statePre.at<track_t>(10) = 0;           // aw
+	m_linearKalman.statePre.at<track_t>(11) = 0;           // ah
+
+	m_linearKalman.statePost.at<track_t>(0) = rect0.x;
+	m_linearKalman.statePost.at<track_t>(1) = rect0.y;
+	m_linearKalman.statePost.at<track_t>(2) = rect0.width;
+	m_linearKalman.statePost.at<track_t>(3) = rect0.height;
+	m_linearKalman.statePost.at<track_t>(4) = rectv0.x;
+	m_linearKalman.statePost.at<track_t>(5) = rectv0.y;
+	m_linearKalman.statePost.at<track_t>(6) = 0;
+	m_linearKalman.statePost.at<track_t>(7) = 0;
+	m_linearKalman.statePost.at<track_t>(8) = 0;
+	m_linearKalman.statePost.at<track_t>(9) = 0;
+	m_linearKalman.statePost.at<track_t>(10) = 0;
+	m_linearKalman.statePost.at<track_t>(11) = 0;
+
+	cv::setIdentity(m_linearKalman.measurementMatrix);
+
+	track_t n1 = pow(m_deltaTime, 4.f) / 4.f;
+	track_t n2 = pow(m_deltaTime, 3.f) / 2.f;
+	track_t n3 = pow(m_deltaTime, 2.f);
+	m_linearKalman.processNoiseCov = (cv::Mat_<track_t>(12, 12) <<
+		n1, 0, 0, 0, n2, 0, 0, 0, n2, 0, n2,
+		0, n1, 0, 0, 0, n2, 0, 0, 0, n2, 0, n2,
+		0, 0, n1, 0, 0, 0, n2, 0, 0, 0, n2, 0,
+		0, 0, 0, n1, 0, 0, 0, n2, 0, 0, 0, n2,
+		n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, n3, 0,
+		0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, n3,
+		0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0,
+		0, 0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3,
+		n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, 0, 0,
+		0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0, 0,
+		0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3, 0,
+		0, 0, 0, n2, 0, 0, 0, n3, 0, 0, 0, n3);
+
+	m_linearKalman.processNoiseCov *= m_accelNoiseMag;
+
+	cv::setIdentity(m_linearKalman.measurementNoiseCov, cv::Scalar::all(0.1));
+
+	cv::setIdentity(m_linearKalman.errorCovPost, cv::Scalar::all(.1));
+
+	m_initialized = true;
+}
+
+#ifdef USE_OCV_UKF
+//---------------------------------------------------------------------------
+class AcceleratedModel: public cv::tracking::UkfSystemModel
+{
+public:
+    AcceleratedModel(track_t deltaTime, bool rectModel)
+        :
+          cv::tracking::UkfSystemModel(),
+          m_deltaTime(deltaTime),
+          m_rectModel(rectModel)
+    {
+
+    }
+
+    void stateConversionFunction(const cv::Mat& x_k, const cv::Mat& u_k, const cv::Mat& v_k, cv::Mat& x_kplus1)
+    {
+        track_t x0 = x_k.at<track_t>(0, 0);
+        track_t y0 = x_k.at<track_t>(1, 0);
+        track_t vx0 = x_k.at<track_t>(2, 0);
+        track_t vy0 = x_k.at<track_t>(3, 0);
+        track_t ax0 = x_k.at<track_t>(4, 0);
+        track_t ay0 = x_k.at<track_t>(5, 0);
+
+        x_kplus1.at<track_t>(0, 0) = x0 + vx0 * m_deltaTime + ax0 * sqr(m_deltaTime) / 2;
+        x_kplus1.at<track_t>(1, 0) = y0 + vy0 * m_deltaTime + ay0 * sqr(m_deltaTime) / 2;
+        x_kplus1.at<track_t>(2, 0) = vx0 + ax0 * m_deltaTime;
+        x_kplus1.at<track_t>(3, 0) = vy0 + ay0 * m_deltaTime;
+        x_kplus1.at<track_t>(4, 0) = ax0;
+        x_kplus1.at<track_t>(5, 0) = ay0;
+
+        if (m_rectModel)
+        {
+            x_kplus1.at<track_t>(6, 0) = x_k.at<track_t>(6, 0);
+            x_kplus1.at<track_t>(7, 0) = x_k.at<track_t>(7, 0);
+        }
+
+        if (v_k.size() == u_k.size())
+        {
+            x_kplus1 += v_k + u_k;
+        }
+        else
+        {
+            x_kplus1 += v_k;
+        }
+    }
+
+    void measurementFunction(const cv::Mat& x_k, const cv::Mat& n_k, cv::Mat& z_k)
+    {
+        track_t x0 = x_k.at<track_t>(0, 0);
+        track_t y0 = x_k.at<track_t>(1, 0);
+        track_t vx0 = x_k.at<track_t>(2, 0);
+        track_t vy0 = x_k.at<track_t>(3, 0);
+        track_t ax0 = x_k.at<track_t>(4, 0);
+        track_t ay0 = x_k.at<track_t>(5, 0);
+
+        z_k.at<track_t>(0, 0) = x0 + vx0 * m_deltaTime + ax0 * sqr(m_deltaTime) / 2 + n_k.at<track_t>(0, 0);
+        z_k.at<track_t>(1, 0) = y0 + vy0 * m_deltaTime + ay0 * sqr(m_deltaTime) / 2 + n_k.at<track_t>(1, 0);
+
+        if (m_rectModel)
+        {
+            z_k.at<track_t>(2, 0) = x_k.at<track_t>(6, 0);
+            z_k.at<track_t>(3, 0) = x_k.at<track_t>(7, 0);
+        }
+    }
+
+private:
+    track_t m_deltaTime;
+    bool m_rectModel;
+};
+
+//---------------------------------------------------------------------------
+void TKalmanFilter::CreateUnscented(Point_t xy0, Point_t xyv0)
+{
+    int MP = 2;
+    int DP = 6;
+    int CP = 0;
+
+    cv::Mat processNoiseCov = cv::Mat::zeros(DP, DP, Mat_t(1));
+    processNoiseCov.at<track_t>(0, 0) = 1e-14f;
+    processNoiseCov.at<track_t>(1, 1) = 1e-14f;
+    processNoiseCov.at<track_t>(2, 2) = 1e-6f;
+    processNoiseCov.at<track_t>(3, 3) = 1e-6f;
+    processNoiseCov.at<track_t>(4, 4) = 1e-6f;
+    processNoiseCov.at<track_t>(5, 5) = 1e-6f;
+
+    cv::Mat measurementNoiseCov = cv::Mat::zeros(MP, MP, Mat_t(1));
+    measurementNoiseCov.at<track_t>(0, 0) = 1e-6f;
+    measurementNoiseCov.at<track_t>(1, 1) = 1e-6f;
+
+    cv::Mat initState(DP, 1, Mat_t(1));
+    initState.at<track_t>(0, 0) = xy0.x;
+    initState.at<track_t>(1, 0) = xy0.y;
+    initState.at<track_t>(2, 0) = xyv0.x;
+    initState.at<track_t>(3, 0) = xyv0.y;
+    initState.at<track_t>(4, 0) = 0;
+    initState.at<track_t>(5, 0) = 0;
+
+    cv::Mat P = 1e-6f * cv::Mat::eye(DP, DP, Mat_t(1));
+
+    cv::Ptr<AcceleratedModel> model(new AcceleratedModel(m_deltaTime, false));
+    cv::tracking::UnscentedKalmanFilterParams params(DP, MP, CP, 0, 0, model);
+    params.dataType = Mat_t(1);
+    params.stateInit = initState.clone();
+    params.errorCovInit = P.clone();
+    params.measurementNoiseCov = measurementNoiseCov.clone();
+    params.processNoiseCov = processNoiseCov.clone();
+
+    params.alpha = 1.0;
+    params.beta = 2.0;
+    params.k = -2.0;
+
+    m_uncsentedKalman = cv::tracking::createUnscentedKalmanFilter(params);
+
+    m_initialized = true;
+}
+
+//---------------------------------------------------------------------------
+void TKalmanFilter::CreateUnscented(cv::Rect_<track_t> rect0, Point_t rectv0)
+{
+    int MP = 4;
+    int DP = 8;
+    int CP = 0;
+
+    cv::Mat processNoiseCov = cv::Mat::zeros(DP, DP, Mat_t(1));
+    processNoiseCov.at<track_t>(0, 0) = 1e-3f;
+    processNoiseCov.at<track_t>(1, 1) = 1e-3f;
+    processNoiseCov.at<track_t>(2, 2) = 1e-3f;
+    processNoiseCov.at<track_t>(3, 3) = 1e-3f;
+    processNoiseCov.at<track_t>(4, 4) = 1e-3f;
+    processNoiseCov.at<track_t>(5, 5) = 1e-3f;
+    processNoiseCov.at<track_t>(6, 6) = 1e-3f;
+    processNoiseCov.at<track_t>(7, 7) = 1e-3f;
+
+    cv::Mat measurementNoiseCov = cv::Mat::zeros(MP, MP, Mat_t(1));
+    measurementNoiseCov.at<track_t>(0, 0) = 1e-3f;
+    measurementNoiseCov.at<track_t>(1, 1) = 1e-3f;
+    measurementNoiseCov.at<track_t>(2, 2) = 1e-3f;
+    measurementNoiseCov.at<track_t>(3, 3) = 1e-3f;
+
+    cv::Mat initState(DP, 1, Mat_t(1));
+    initState.at<track_t>(0, 0) = rect0.x;
+    initState.at<track_t>(1, 0) = rect0.y;
+    initState.at<track_t>(2, 0) = rectv0.x;
+    initState.at<track_t>(3, 0) = rectv0.y;
+    initState.at<track_t>(4, 0) = 0;
+    initState.at<track_t>(5, 0) = 0;
+    initState.at<track_t>(6, 0) = rect0.width;
+    initState.at<track_t>(7, 0) = rect0.height;
+
+    cv::Mat P = 1e-3f * cv::Mat::eye(DP, DP, Mat_t(1));
+
+    cv::Ptr<AcceleratedModel> model(new AcceleratedModel(m_deltaTime, true));
+    cv::tracking::UnscentedKalmanFilterParams params(DP, MP, CP, 0, 0, model);
+    params.dataType = Mat_t(1);
+    params.stateInit = initState.clone();
+    params.errorCovInit = P.clone();
+    params.measurementNoiseCov = measurementNoiseCov.clone();
+    params.processNoiseCov = processNoiseCov.clone();
+
+    params.alpha = 1;
+    params.beta = 2.0;
+    params.k = -2.0;
+
+    m_uncsentedKalman = cv::tracking::createUnscentedKalmanFilter(params);
+
+    m_initialized = true;
+}
+
+//---------------------------------------------------------------------------
+void TKalmanFilter::CreateAugmentedUnscented(Point_t xy0, Point_t xyv0)
+{
+    int MP = 2;
+    int DP = 6;
+    int CP = 0;
+
+    cv::Mat processNoiseCov = cv::Mat::zeros(DP, DP, Mat_t(1));
+    processNoiseCov.at<track_t>(0, 0) = 1e-14f;
+    processNoiseCov.at<track_t>(1, 1) = 1e-14f;
+    processNoiseCov.at<track_t>(2, 2) = 1e-6f;
+    processNoiseCov.at<track_t>(3, 3) = 1e-6f;
+    processNoiseCov.at<track_t>(4, 4) = 1e-6f;
+    processNoiseCov.at<track_t>(5, 5) = 1e-6f;
+
+    cv::Mat measurementNoiseCov = cv::Mat::zeros(MP, MP, Mat_t(1));
+    measurementNoiseCov.at<track_t>(0, 0) = 1e-6f;
+    measurementNoiseCov.at<track_t>(1, 1) = 1e-6f;
+
+    cv::Mat initState(DP, 1, Mat_t(1));
+    initState.at<track_t>(0, 0) = xy0.x;
+    initState.at<track_t>(1, 0) = xy0.y;
+    initState.at<track_t>(2, 0) = xyv0.x;
+    initState.at<track_t>(3, 0) = xyv0.y;
+    initState.at<track_t>(4, 0) = 0;
+    initState.at<track_t>(5, 0) = 0;
+
+    cv::Mat P = 1e-6f * cv::Mat::eye(DP, DP, Mat_t(1));
+
+    cv::Ptr<AcceleratedModel> model(new AcceleratedModel(m_deltaTime, false));
+    cv::tracking::AugmentedUnscentedKalmanFilterParams params(DP, MP, CP, 0, 0, model);
+    params.dataType = Mat_t(1);
+    params.stateInit = initState.clone();
+    params.errorCovInit = P.clone();
+    params.measurementNoiseCov = measurementNoiseCov.clone();
+    params.processNoiseCov = processNoiseCov.clone();
+
+    params.alpha = 1;
+    params.beta = 2.0;
+    params.k = -2.0;
+
+    m_uncsentedKalman = cv::tracking::createAugmentedUnscentedKalmanFilter(params);
+
+    m_initialized = true;
+}
+
+//---------------------------------------------------------------------------
+void TKalmanFilter::CreateAugmentedUnscented(cv::Rect_<track_t> rect0, Point_t rectv0)
+{
+    int MP = 4;
+    int DP = 8;
+    int CP = 0;
+
+    cv::Mat processNoiseCov = cv::Mat::zeros(DP, DP, Mat_t(1));
+    processNoiseCov.at<track_t>(0, 0) = 1e-3f;
+    processNoiseCov.at<track_t>(1, 1) = 1e-3f;
+    processNoiseCov.at<track_t>(2, 2) = 1e-3f;
+    processNoiseCov.at<track_t>(3, 3) = 1e-3f;
+    processNoiseCov.at<track_t>(4, 4) = 1e-3f;
+    processNoiseCov.at<track_t>(5, 5) = 1e-3f;
+    processNoiseCov.at<track_t>(6, 6) = 1e-3f;
+    processNoiseCov.at<track_t>(7, 7) = 1e-3f;
+
+    cv::Mat measurementNoiseCov = cv::Mat::zeros(MP, MP, Mat_t(1));
+    measurementNoiseCov.at<track_t>(0, 0) = 1e-3f;
+    measurementNoiseCov.at<track_t>(1, 1) = 1e-3f;
+    measurementNoiseCov.at<track_t>(2, 2) = 1e-3f;
+    measurementNoiseCov.at<track_t>(3, 3) = 1e-3f;
+
+    cv::Mat initState(DP, 1, Mat_t(1));
+    initState.at<track_t>(0, 0) = rect0.x;
+    initState.at<track_t>(1, 0) = rect0.y;
+    initState.at<track_t>(2, 0) = rectv0.x;
+    initState.at<track_t>(3, 0) = rectv0.y;
+    initState.at<track_t>(4, 0) = 0;
+    initState.at<track_t>(5, 0) = 0;
+    initState.at<track_t>(6, 0) = rect0.width;
+    initState.at<track_t>(7, 0) = rect0.height;
+
+    cv::Mat P = 1e-3f * cv::Mat::eye(DP, DP, Mat_t(1));
+
+    cv::Ptr<AcceleratedModel> model(new AcceleratedModel(m_deltaTime, true));
+    cv::tracking::AugmentedUnscentedKalmanFilterParams params(DP, MP, CP, 0, 0, model);
+    params.dataType = Mat_t(1);
+    params.stateInit = initState.clone();
+    params.errorCovInit = P.clone();
+    params.measurementNoiseCov = measurementNoiseCov.clone();
+    params.processNoiseCov = processNoiseCov.clone();
+
+    params.alpha = 1;
+    params.beta = 2.0;
+    params.k = -2.0;
+
+    m_uncsentedKalman = cv::tracking::createAugmentedUnscentedKalmanFilter(params);
+
+    m_initialized = true;
+}
+#endif
+
+//---------------------------------------------------------------------------
+Point_t TKalmanFilter::GetPointPrediction()
+{
+    if (m_initialized)
+    {
+        cv::Mat prediction;
+
+        switch (m_type)
+        {
+        case tracking::KalmanLinear:
+            prediction = m_linearKalman.predict();
+            break;
+
+        case tracking::KalmanUnscented:
+        case tracking::KalmanAugmentedUnscented:
+#ifdef USE_OCV_UKF
+            prediction = m_uncsentedKalman->predict();
+#else
+            prediction = m_linearKalman.predict();
+            std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
+#endif
+            break;
+        }
+
+        m_lastPointResult = Point_t(prediction.at<track_t>(0), prediction.at<track_t>(1));
+    }
+    else
+    {
+
+    }
+    return m_lastPointResult;
+}
+
+//---------------------------------------------------------------------------
+Point_t TKalmanFilter::Update(Point_t pt, bool dataCorrect)
+{
+    if (!m_initialized)
+    {
+        if (m_initialPoints.size() < MIN_INIT_VALS)
+        {
+            if (dataCorrect)
+            {
+                m_initialPoints.push_back(pt);
+                m_lastPointResult = pt;
+            }
+        }
+        if (m_initialPoints.size() == MIN_INIT_VALS)
+        {
+            track_t kx = 0;
+            track_t bx = 0;
+            track_t ky = 0;
+            track_t by = 0;
+            get_lin_regress_params(m_initialPoints, 0, MIN_INIT_VALS, kx, bx, ky, by);//predict p,v
+            Point_t xy0(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by);
+            Point_t xyv0(kx, ky);
+
+            switch (m_type)
+            {
+            case tracking::KalmanLinear:
+                if (m_useAcceleration)
+					CreateLinearAcceleration(xy0, xyv0);
+				else
+					CreateLinear(xy0, xyv0);
+                break;
+
+            case tracking::KalmanUnscented:
+#ifdef USE_OCV_UKF
+                CreateUnscented(xy0, xyv0);
+#else
+				if (m_useAcceleration)
+					CreateLinearAcceleration(xy0, xyv0);
+				else
+					CreateLinear(xy0, xyv0);
+                std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
+#endif
+                break;
+
+            case tracking::KalmanAugmentedUnscented:
+#ifdef USE_OCV_UKF
+                CreateAugmentedUnscented(xy0, xyv0);
+#else
+				if (m_useAcceleration)
+					CreateLinearAcceleration(xy0, xyv0);
+				else
+					CreateLinear(xy0, xyv0);
+                std::cerr << "AugmentedUnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
+#endif
+                break;
+            }
+            m_lastDist = 0;
+        }
+    }
+
+    if (m_initialized)
+    {
+        cv::Mat measurement(2, 1, Mat_t(1));
+        if (!dataCorrect)
+        {
+            measurement.at<track_t>(0) = m_lastPointResult.x;  //update using prediction
+            measurement.at<track_t>(1) = m_lastPointResult.y;
+        }
+        else
+        {
+            measurement.at<track_t>(0) = pt.x;  //update using measurements
+            measurement.at<track_t>(1) = pt.y;
+        }
+        // Correction
+        cv::Mat estimated;
+        switch (m_type)
+        {
+        case tracking::KalmanLinear:
+        {
+            estimated = m_linearKalman.correct(measurement);
+
+            // Inertia correction
+			if (!m_useAcceleration)
+			{
+				track_t currDist = sqrtf(sqr(estimated.at<track_t>(0) - pt.x) + sqr(estimated.at<track_t>(1) - pt.y));
+				if (currDist > m_lastDist)
+				{
+					m_deltaTime = std::min(m_deltaTime + m_deltaStep, m_deltaTimeMax);
+				}
+				else
+				{
+					m_deltaTime = std::max(m_deltaTime - m_deltaStep, m_deltaTimeMin);
+				}
+				m_lastDist = currDist;
+
+				m_linearKalman.transitionMatrix.at<track_t>(0, 2) = m_deltaTime;
+				m_linearKalman.transitionMatrix.at<track_t>(1, 3) = m_deltaTime;
+			}
+            break;
+        }
+
+        case tracking::KalmanUnscented:
+        case tracking::KalmanAugmentedUnscented:
+#ifdef USE_OCV_UKF
+            estimated = m_uncsentedKalman->correct(measurement);
+#else
+            estimated = m_linearKalman.correct(measurement);
+            std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
+#endif
+            break;
+        }
+
+        m_lastPointResult.x = estimated.at<track_t>(0);   //update using measurements
+        m_lastPointResult.y = estimated.at<track_t>(1);
+    }
+    else
+    {
+        if (dataCorrect)
+        {
+            m_lastPointResult = pt;
+        }
+    }
+    return m_lastPointResult;
+}
+
+//---------------------------------------------------------------------------
+cv::Rect TKalmanFilter::GetRectPrediction()
+{
+    if (m_initialized)
+    {
+        cv::Mat prediction;
+
+        switch (m_type)
+        {
+        case tracking::KalmanLinear:
+            prediction = m_linearKalman.predict();
+            break;
+
+        case tracking::KalmanUnscented:
+        case tracking::KalmanAugmentedUnscented:
+#ifdef USE_OCV_UKF
+            prediction = m_uncsentedKalman->predict();
+#else
+            prediction = m_linearKalman.predict();
+            std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
+#endif
+            break;
+        }
+
+        m_lastRectResult = cv::Rect_<track_t>(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2), prediction.at<track_t>(3));
+    }
+    else
+    {
+
+    }
+    return cv::Rect(static_cast<int>(m_lastRectResult.x), static_cast<int>(m_lastRectResult.y), static_cast<int>(m_lastRectResult.width), static_cast<int>(m_lastRectResult.height));
+}
+
+//---------------------------------------------------------------------------
+cv::Rect TKalmanFilter::Update(cv::Rect rect, bool dataCorrect)
+{
+    if (!m_initialized)
+    {
+        if (m_initialRects.size() < MIN_INIT_VALS)
+        {
+            if (dataCorrect)
+            {
+                m_initialRects.push_back(rect);
+                m_lastRectResult.x = static_cast<track_t>(rect.x);
+                m_lastRectResult.y = static_cast<track_t>(rect.y);
+                m_lastRectResult.width = static_cast<track_t>(rect.width);
+                m_lastRectResult.height = static_cast<track_t>(rect.height);
+            }
+        }
+        if (m_initialRects.size() == MIN_INIT_VALS)
+        {
+            std::vector<Point_t> initialPoints;
+            Point_t averageSize(0, 0);
+            for (const auto& r : m_initialRects)
+            {
+                initialPoints.emplace_back(static_cast<track_t>(r.x), static_cast<track_t>(r.y));
+                averageSize.x += r.width;
+                averageSize.y += r.height;
+            }
+            averageSize.x /= MIN_INIT_VALS;
+            averageSize.y /= MIN_INIT_VALS;
+
+            track_t kx = 0;
+            track_t bx = 0;
+            track_t ky = 0;
+            track_t by = 0;
+            get_lin_regress_params(initialPoints, 0, MIN_INIT_VALS, kx, bx, ky, by);
+            cv::Rect_<track_t> rect0(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by, averageSize.x, averageSize.y);
+            Point_t rectv0(kx, ky);
+
+            switch (m_type)
+            {
+            case tracking::KalmanLinear:
+				if (m_useAcceleration)
+					CreateLinearAcceleration(rect0, rectv0);
+				else
+					CreateLinear(rect0, rectv0);
+                break;
+
+            case tracking::KalmanUnscented:
+#ifdef USE_OCV_UKF
+                CreateUnscented(rect0, rectv0);
+#else
+				if (m_useAcceleration)
+					CreateLinearAcceleration(rect0, rectv0);
+				else
+					CreateLinear(rect0, rectv0);
+                std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
+#endif
+                break;
+
+            case tracking::KalmanAugmentedUnscented:
+#ifdef USE_OCV_UKF
+                CreateAugmentedUnscented(rect0, rectv0);
+#else
+				if (m_useAcceleration)
+					CreateLinearAcceleration(rect0, rectv0);
+				else
+					CreateLinear(rect0, rectv0);
+                std::cerr << "AugmentedUnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
+#endif
+                break;
+            }
+        }
+    }
+
+    if (m_initialized)
+    {
+        cv::Mat measurement(4, 1, Mat_t(1));
+        if (!dataCorrect)
+        {
+            measurement.at<track_t>(0) = m_lastRectResult.x;  // update using prediction
+            measurement.at<track_t>(1) = m_lastRectResult.y;
+            measurement.at<track_t>(2) = m_lastRectResult.width;
+            measurement.at<track_t>(3) = m_lastRectResult.height;
+        }
+        else
+        {
+            measurement.at<track_t>(0) = static_cast<track_t>(rect.x);  // update using measurements
+            measurement.at<track_t>(1) = static_cast<track_t>(rect.y);
+            measurement.at<track_t>(2) = static_cast<track_t>(rect.width);
+            measurement.at<track_t>(3) = static_cast<track_t>(rect.height);
+        }
+        // Correction
+        cv::Mat estimated;
+        switch (m_type)
+        {
+        case tracking::KalmanLinear:
+        {
+            estimated = m_linearKalman.correct(measurement);
+
+            m_lastRectResult.x = estimated.at<track_t>(0);   //update using measurements
+            m_lastRectResult.y = estimated.at<track_t>(1);
+            m_lastRectResult.width = estimated.at<track_t>(2);
+            m_lastRectResult.height = estimated.at<track_t>(3);
+
+            // Inertia correction
+			if (!m_useAcceleration)
+			{
+				track_t currDist = sqrtf(sqr(estimated.at<track_t>(0) - rect.x) + sqr(estimated.at<track_t>(1) - rect.y) + sqr(estimated.at<track_t>(2) - rect.width) + sqr(estimated.at<track_t>(3) - rect.height));
+				if (currDist > m_lastDist)
+				{
+					m_deltaTime = std::min(m_deltaTime + m_deltaStep, m_deltaTimeMax);
+				}
+				else
+				{
+					m_deltaTime = std::max(m_deltaTime - m_deltaStep, m_deltaTimeMin);
+				}
+				m_lastDist = currDist;
+
+				m_linearKalman.transitionMatrix.at<track_t>(0, 4) = m_deltaTime;
+				m_linearKalman.transitionMatrix.at<track_t>(1, 5) = m_deltaTime;
+				m_linearKalman.transitionMatrix.at<track_t>(2, 6) = m_deltaTime;
+				m_linearKalman.transitionMatrix.at<track_t>(3, 7) = m_deltaTime;
+			}
+            break;
+        }
+
+        case tracking::KalmanUnscented:
+        case tracking::KalmanAugmentedUnscented:
+#ifdef USE_OCV_UKF
+            estimated = m_uncsentedKalman->correct(measurement);
+
+            m_lastRectResult.x = estimated.at<track_t>(0);   //update using measurements
+            m_lastRectResult.y = estimated.at<track_t>(1);
+            m_lastRectResult.width = estimated.at<track_t>(6);
+            m_lastRectResult.height = estimated.at<track_t>(7);
+#else
+            estimated = m_linearKalman.correct(measurement);
+
+            m_lastRectResult.x = estimated.at<track_t>(0);   //update using measurements
+            m_lastRectResult.y = estimated.at<track_t>(1);
+            m_lastRectResult.width = estimated.at<track_t>(2);
+            m_lastRectResult.height = estimated.at<track_t>(3);
+            std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
+#endif
+            break;
+        }
+    }
+    else
+    {
+        if (dataCorrect)
+        {
+            m_lastRectResult.x = static_cast<track_t>(rect.x);
+            m_lastRectResult.y = static_cast<track_t>(rect.y);
+            m_lastRectResult.width = static_cast<track_t>(rect.width);
+            m_lastRectResult.height = static_cast<track_t>(rect.height);
+        }
+    }
+    return cv::Rect(static_cast<int>(m_lastRectResult.x), static_cast<int>(m_lastRectResult.y), static_cast<int>(m_lastRectResult.width), static_cast<int>(m_lastRectResult.height));
+}
+
+//---------------------------------------------------------------------------
+cv::Vec<track_t, 2> TKalmanFilter::GetVelocity() const
+{
+    cv::Vec<track_t, 2> res(0, 0);
+    if (m_initialized)
+    {
+        switch (m_type)
+        {
+        case tracking::KalmanLinear:
+        {
+            if (m_linearKalman.statePre.rows > 3)
+            {
+                int indX = 2;
+                int indY = 3;
+                if (m_linearKalman.statePre.rows > 4)
+                {
+                    indX = 4;
+                    indY = 5;
+                }
+                res[0] = m_linearKalman.statePre.at<track_t>(indX);
+                res[1] = m_linearKalman.statePre.at<track_t>(indY);
+            }
+            break;
+        }
+
+        case tracking::KalmanUnscented:
+        case tracking::KalmanAugmentedUnscented:
+#ifdef USE_OCV_UKF
+            cv::Mat state = m_uncsentedKalman->getState();
+            res[0] = state.at<track_t>(2);
+            res[1] = state.at<track_t>(3);
+#else
+            std::cerr << "UnscentedKalmanFilter was disabled in CMAKE! Set KalmanLinear in constructor." << std::endl;
+#endif
+            break;
+        }
+    }
+    return res;
+}

src/fusion/lidar_radar_fusion_cnn/Tracker/Kalman.h

@@ -0,0 +1,127 @@
+#pragma once
+#include "defines.h"
+#include <memory>
+#include <deque>
+
+#include <opencv2/opencv.hpp>
+
+#ifdef USE_OCV_UKF
+#include <opencv2/tracking.hpp>
+#include <opencv2/tracking/kalman_filters.hpp>
+#endif
+
+///
+/// \brief The TKalmanFilter class
+/// http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/
+///
+class TKalmanFilter
+{
+public:
+    TKalmanFilter(tracking::KalmanType type, bool useAcceleration, track_t deltaTime, track_t accelNoiseMag);
+    ~TKalmanFilter() = default;
+
+    Point_t GetPointPrediction();
+    Point_t Update(Point_t pt, bool dataCorrect);
+
+    cv::Rect GetRectPrediction();
+    cv::Rect Update(cv::Rect rect, bool dataCorrect);
+
+	cv::Vec<track_t, 2> GetVelocity() const;
+
+private:
+    tracking::KalmanType m_type = tracking::KalmanLinear;
+    cv::KalmanFilter m_linearKalman;
+#ifdef USE_OCV_UKF
+    cv::Ptr<cv::tracking::UnscentedKalmanFilter> m_uncsentedKalman;
+#endif
+
+    std::deque<Point_t> m_initialPoints;
+    std::deque<cv::Rect> m_initialRects;
+    static const size_t MIN_INIT_VALS = 4;
+
+    Point_t m_lastPointResult;
+    cv::Rect_<track_t> m_lastRectResult;
+    cv::Rect_<track_t> m_lastRect;
+
+    bool m_initialized = false;
+    track_t m_deltaTime = 0.2f;
+    track_t m_deltaTimeMin = 0.2f;
+    track_t m_deltaTimeMax = 2 * 0.2f;
+    track_t m_lastDist = 0;
+    track_t m_deltaStep = 0;
+    static const int m_deltaStepsCount = 20;
+    track_t m_accelNoiseMag = 0.5f;
+	bool m_useAcceleration = false; // If set true then will be used motion model x(t) = x0 + v0 * t + a * t^2 / 2
+
+	// Constant velocity model
+    void CreateLinear(Point_t xy0, Point_t xyv0);
+    void CreateLinear(cv::Rect_<track_t> rect0, Point_t rectv0);
+	
+	// Constant acceleration model
+	// https://www.mathworks.com/help/driving/ug/linear-kalman-filters.html
+	void CreateLinearAcceleration(Point_t xy0, Point_t xyv0);
+	void CreateLinearAcceleration(cv::Rect_<track_t> rect0, Point_t rectv0);
+
+#ifdef USE_OCV_UKF
+    void CreateUnscented(Point_t xy0, Point_t xyv0);
+    void CreateUnscented(cv::Rect_<track_t> rect0, Point_t rectv0);
+    void CreateAugmentedUnscented(Point_t xy0, Point_t xyv0);
+    void CreateAugmentedUnscented(cv::Rect_<track_t> rect0, Point_t rectv0);
+#endif
+};
+
+//---------------------------------------------------------------------------
+///
+/// \brief sqr
+/// \param val
+/// \return
+///
+template<class T> inline
+T sqr(T val)
+{
+    return val * val;
+}
+
+///
+/// \brief get_lin_regress_params
+/// \param in_data
+/// \param start_pos
+/// \param in_data_size
+/// \param kx
+/// \param bx
+/// \param ky
+/// \param by
+///
+template<typename T, typename CONT>
+void get_lin_regress_params(
+        const CONT& in_data,
+        size_t start_pos,
+        size_t in_data_size,
+        T& kx, T& bx, T& ky, T& by)
+{
+    T m1(0.), m2(0.);
+    T m3_x(0.), m4_x(0.);
+    T m3_y(0.), m4_y(0.);
+
+    const T el_count = static_cast<T>(in_data_size - start_pos);
+    for (size_t i = start_pos; i < in_data_size; ++i)
+    {
+        m1 += i;
+        m2 += sqr(i);
+
+        m3_x += in_data[i].x;
+        m4_x += i * in_data[i].x;
+
+        m3_y += in_data[i].y;
+        m4_y += i * in_data[i].y;
+    }
+    T det_1 = 1 / (el_count * m2 - sqr(m1));
+
+    m1 *= -1;
+
+    kx = det_1 * (m1 * m3_x + el_count * m4_x);
+    bx = det_1 * (m2 * m3_x + m1 * m4_x);
+
+    ky = det_1 * (m1 * m3_y + el_count * m4_y);
+    by = det_1 * (m2 * m3_y + m1 * m4_y);
+}

src/fusion/lidar_radar_fusion_cnn/Tracker/ShortPathCalculator.h

@@ -0,0 +1,65 @@
+#pragma once
+#include "defines.h"
+#include "HungarianAlg.h"
+
+///
+/// \brief The SPSettings struct
+///
+struct SPSettings
+{
+    track_t m_distThres = 0.8f;
+    size_t m_maxHistory = 10;
+};
+
+///
+/// \brief The ShortPathCalculator class
+///
+class ShortPathCalculator
+{
+public:
+    ShortPathCalculator(const SPSettings& settings)
+        : m_settings(settings)
+    {
+    }
+    virtual ~ShortPathCalculator()
+    {
+    }
+
+    virtual void Solve(const distMatrix_t& costMatrix, size_t N, size_t M, assignments_t& assignment, track_t maxCost) = 0;
+protected:
+    SPSettings m_settings;
+};
+
+///
+/// \brief The SPHungrian class
+///
+class SPHungrian : public ShortPathCalculator
+{
+public:
+    SPHungrian(const SPSettings& settings)
+        : ShortPathCalculator(settings)
+    {
+    }
+
+    void Solve(const distMatrix_t& costMatrix, size_t N, size_t M, assignments_t& assignment, track_t /*maxCost*/)
+    {
+        m_solver.Solve(costMatrix, N, M, assignment, AssignmentProblemSolver::optimal);
+    }
+
+private:
+    AssignmentProblemSolver m_solver;
+};
+
+///
+/// \brief The SPBipart class
+///
+//class SPBipart : public ShortPathCalculator
+//{
+//public:
+//    SPBipart(const SPSettings& settings)
+//        : ShortPathCalculator(settings)
+//    {
+//    }
+
+//    void Solve(const distMatrix_t& costMatrix, size_t N, size_t M, assignments_t& assignment, track_t maxCost);
+//};

src/fusion/lidar_radar_fusion_cnn/Tracker/Tracking.h

@@ -0,0 +1,86 @@
+#pragma once
+#include "fusionobjectarray.pb.h"
+#include "fusionobject.pb.h"
+#include "Ctracker.h"
+
+#include <iostream>
+#include <vector>
+bool InitTracker(CTracker& tracker)
+{
+    TrackerSettings settings;
+    settings.SetDistance(tracking::DistCenters);
+    settings.m_kalmanType = tracking::KalmanLinear;
+    settings.m_filterGoal = tracking::FilterCenter;
+    settings.m_lostTrackType = tracking::TrackNone;
+    settings.m_matchType = tracking::MatchHungrian;
+    settings.m_dt = 0.2f;
+    settings.m_accelNoiseMag = 0.5f;
+    settings.m_distThres = 0.8f;
+    settings.m_minAreaRadiusPix = -1.f;//frame.rows / 20.f;
+    settings.m_maximumAllowedSkippedFrames = 30;
+    settings.m_maxTraceLength = 60;
+
+    tracker.setSettings(settings);
+
+    return true;
+}
+
+// ----------------------------------------------------------------------
+void Tracking(iv::fusion::fusionobjectarray& fusionobjvec, CTracker& tracker)
+{
+    //  std::vector<Point_t> pts;
+    regions_t regions;
+    Point_t pointXY;
+    for(int i = 0;i<fusionobjvec.obj_size();i++)
+    {
+        pointXY.x = fusionobjvec.obj(i).centroid().x();
+        pointXY.y = fusionobjvec.obj(i).centroid().y();
+        int w = fabs(fusionobjvec.obj(i).dimensions().x());
+        int h = fabs(fusionobjvec.obj(i).dimensions().z());
+        cv::RotatedRect rrect = cv::RotatedRect(pointXY,cv::Size2f(w>0?w:w+0.0001,h>0?h:h+0.0001),0);
+        CRegion region = CRegion(rrect,fusionobjvec.obj(i).type_name(),fusionobjvec.obj(i).prob());
+        regions.push_back(region);
+        //std::cout<<"old x,y:"<<pointXY.x<<","<<pointXY.y<<std::endl;
+    }
+
+    tracker.Update(regions, cv::UMat(), 30);
+
+    auto tracks = tracker.GetTracks();
+    // std::cout<<fusionobjvec.obj_size()<<","<<tracks.size()<<std::endl;
+    for (size_t i = 0; i < tracks.size(); i++)
+    {
+        const auto& track = tracks[i];
+        int obj_id = track.m_regionID;
+        if(obj_id != -1)
+        {
+            iv::fusion::fusionobject &fusion_object = (iv::fusion::fusionobject &)fusionobjvec.obj(obj_id);
+            fusion_object.set_id(track.m_ID);
+            // std::cout<<"i:"<<fusion_object.id()<<std::endl;
+            // std::cout<<"update x,y:"<<track.m_trace[track.m_trace.size()-1].x<<","<<track.m_trace[track.m_trace.size()-1].y<<std::endl;
+            iv::fusion::PointXYZ centroid;
+            iv::fusion::PointXYZ *centerpoint;
+            iv::fusion::PointXYZ point_forecast;
+            centroid.set_x(track.m_trace[track.m_trace.size()-1].x);
+            centroid.set_y(track.m_trace[track.m_trace.size()-1].y);
+            centerpoint=fusion_object.mutable_centroid();
+            centerpoint->CopyFrom(centroid);
+            for(int j=0;j<track.m_trace.size();j++)
+            {
+                point_forecast.set_x(track.m_trace[j].x);
+                point_forecast.set_y(track.m_trace[j].y);
+                iv::fusion::PointXYZ *p = fusion_object.add_point_forecast();
+                p->CopyFrom(point_forecast);
+            }
+        }
+
+    }
+//    for (size_t i = 0; i < fusionobjvec.obj_size(); i++)
+//    {
+//        iv::fusion::fusionobject fusion_object;
+//        fusion_object = fusionobjvec.obj(i);
+//        std::cout<<"forecast size:"<<fusion_object.point_forecast_size()<<std::endl;
+//    }
+    regions.clear();
+    tracks.clear();
+}
+

src/fusion/lidar_radar_fusion_cnn/Tracker/VOTTracker.hpp

@@ -0,0 +1,21 @@
+#pragma once
+#include <opencv2/opencv.hpp>
+///
+/// \brief The VOTTracker class
+///
+class VOTTracker
+{
+public:
+    VOTTracker()
+    {
+
+    }
+    virtual ~VOTTracker()
+    {
+
+    }
+
+    virtual void Initialize(const cv::Mat &im, cv::Rect region) = 0;
+    virtual cv::RotatedRect Update(const cv::Mat &im, float& confidence) = 0;
+    virtual void Train(const cv::Mat &im, bool first) = 0;
+};

src/fusion/lidar_radar_fusion_cnn/Tracker/defines.h

@@ -0,0 +1,171 @@
+#pragma once
+
+#include <vector>
+#include <string>
+#include <map>
+#include <opencv2/opencv.hpp>
+
+
+// ---------------------------------------------------------------------------
+//
+// ---------------------------------------------------------------------------
+typedef float track_t;
+typedef cv::Point_<track_t> Point_t;
+#define El_t CV_32F
+#define Mat_t CV_32FC
+
+typedef std::vector<int> assignments_t;
+typedef std::vector<track_t> distMatrix_t;
+
+///
+/// \brief config_t
+///
+typedef std::multimap<std::string, std::string> config_t;
+
+///
+/// \brief The CRegion class
+///
+class CRegion
+{
+public:
+    CRegion()
+        : m_type(""), m_confidence(-1)
+    {
+    }
+
+    CRegion(const cv::Rect& rect)
+        : m_brect(rect)
+    {
+        B2RRect();
+    }
+
+    CRegion(const cv::RotatedRect& rrect)
+        : m_rrect(rrect)
+    {
+        R2BRect();
+    }
+
+    CRegion(const cv::Rect& rect, const std::string& type, float confidence)
+        : m_brect(rect), m_type(type), m_confidence(confidence)
+    {
+        B2RRect();
+    }
+
+    CRegion(const cv::RotatedRect& rrect, const std::string& type, float confidence)
+        : m_rrect(rrect), m_type(type), m_confidence(confidence)
+    {
+        R2BRect();
+    }
+    cv::RotatedRect m_rrect;
+    cv::Rect m_brect;
+
+    std::string m_type;
+    float m_confidence = -1;
+	mutable cv::Mat m_hist;
+
+private:
+    ///
+    /// \brief R2BRect
+    /// \return
+    ///
+    cv::Rect R2BRect()
+    {
+        m_brect = m_rrect.boundingRect();
+        return m_brect;
+    }
+    ///
+    /// \brief B2RRect
+    /// \return
+    ///
+    cv::RotatedRect B2RRect()
+    {
+        m_rrect = cv::RotatedRect(m_brect.tl(), cv::Point2f(static_cast<float>(m_brect.x + m_brect.width), static_cast<float>(m_brect.y)), m_brect.br());
+        return m_rrect;
+    }
+};
+
+typedef std::vector<CRegion> regions_t;
+
+///
+///
+///
+namespace tracking
+{
+///
+/// \brief The Detectors enum
+///
+enum Detectors
+{
+    Motion_VIBE,
+    Motion_MOG,
+    Motion_GMG,
+    Motion_CNT,
+    Motion_SuBSENSE,
+    Motion_LOBSTER,
+    Motion_MOG2,
+    Face_HAAR,
+    Pedestrian_HOG,
+    Pedestrian_C4,
+    SSD_MobileNet,
+    Yolo_OCV,
+	Yolo_Darknet,
+	Yolo_TensorRT
+};
+
+///
+/// \brief The DistType enum
+///
+enum DistType
+{
+    DistCenters,   // Euclidean distance between centers, pixels
+    DistRects,     // Euclidean distance between bounding rectangles, pixels
+    DistJaccard,   // Intersection over Union, IoU, [0, 1]
+	DistHist,      // Bhatacharia distance between histograms, [0, 1]
+	DistsCount
+};
+
+///
+/// \brief The FilterGoal enum
+///
+enum FilterGoal
+{
+    FilterCenter,
+    FilterRect
+};
+
+///
+/// \brief The KalmanType enum
+///
+enum KalmanType
+{
+    KalmanLinear,
+    KalmanUnscented,
+    KalmanAugmentedUnscented
+};
+
+///
+/// \brief The MatchType enum
+///
+enum MatchType
+{
+    MatchHungrian/*,
+    //MatchBipart*/
+};
+
+///
+/// \brief The LostTrackType enum
+///
+enum LostTrackType
+{
+    TrackNone,
+    TrackKCF,
+    TrackMIL,
+    TrackMedianFlow,
+    TrackGOTURN,
+    TrackMOSSE,
+    TrackCSRT/*,
+    TrackDAT,
+    TrackSTAPLE,
+    TrackLDES*/
+};
+}

src/fusion/lidar_radar_fusion_cnn/Tracker/track.cpp

@@ -0,0 +1,854 @@
+#include "track.h"
+
+//#include "dat/dat_tracker.hpp"
+
+///
+/// \brief CTrack
+/// \param pt
+/// \param region
+/// \param deltaTime
+/// \param accelNoiseMag
+/// \param trackID
+/// \param filterObjectSize
+/// \param externalTrackerForLost
+///
+CTrack::CTrack(
+        const CRegion& region,
+        tracking::KalmanType kalmanType,
+        track_t deltaTime,
+        track_t accelNoiseMag,
+        bool useAcceleration,
+        size_t trackID,
+        size_t regionID,
+        bool filterObjectSize,
+        tracking::LostTrackType externalTrackerForLost
+        )
+    :
+      m_trackID(trackID),
+      m_regionID(regionID),
+      m_skippedFrames(0),
+      m_lastRegion(region),
+      m_predictionPoint(region.m_rrect.center),
+      m_predictionRect(region.m_rrect),
+      m_kalman(kalmanType, useAcceleration, deltaTime, accelNoiseMag),
+      m_filterObjectSize(filterObjectSize),
+      m_outOfTheFrame(false),
+      m_externalTrackerForLost(externalTrackerForLost)
+{
+    if (filterObjectSize)
+        m_kalman.Update(region.m_brect, true);
+    else
+        m_kalman.Update(m_predictionPoint, true);
+
+    m_trace.push_back(m_predictionPoint, m_predictionPoint);
+}
+
+///
+/// \brief CTrack::CalcDistCenter
+/// \param reg
+/// \return
+///
+track_t CTrack::CalcDistCenter(const CRegion& reg) const
+{
+    Point_t diff = m_predictionPoint - reg.m_rrect.center;
+    return sqrtf(sqr(diff.x) + sqr(diff.y));
+}
+
+///
+/// \brief CTrack::CalcDistRect
+/// \param reg
+/// \return
+///
+track_t CTrack::CalcDistRect(const CRegion& reg) const
+{
+    std::array<track_t, 5> diff;
+    diff[0] = reg.m_rrect.center.x - m_lastRegion.m_rrect.center.x;
+    diff[1] = reg.m_rrect.center.y - m_lastRegion.m_rrect.center.y;
+    diff[2] = static_cast<track_t>(m_lastRegion.m_rrect.size.width - reg.m_rrect.size.width);
+    diff[3] = static_cast<track_t>(m_lastRegion.m_rrect.size.height - reg.m_rrect.size.height);
+    diff[4] = static_cast<track_t>(m_lastRegion.m_rrect.angle - reg.m_rrect.angle);
+
+    track_t dist = 0;
+    for (size_t i = 0; i < diff.size(); ++i)
+    {
+        dist += sqr(diff[i]);
+    }
+    return sqrtf(dist);
+}
+
+///
+/// \brief CTrack::CalcDistJaccard
+/// \param reg
+/// \return
+///
+track_t CTrack::CalcDistJaccard(const CRegion& reg) const
+{
+    track_t intArea = static_cast<track_t>((reg.m_brect & m_lastRegion.m_brect).area());
+    track_t unionArea = static_cast<track_t>(reg.m_brect.area() + m_lastRegion.m_brect.area() - intArea);
+
+    return 1 - intArea / unionArea;
+}
+
+///
+/// \brief CTrack::CalcDistHist
+/// \param reg
+/// \return
+///
+track_t CTrack::CalcDistHist(const CRegion& reg, cv::UMat currFrame) const
+{
+    track_t res = 1;
+
+    if (reg.m_hist.empty())
+    {
+        int bins = 64;
+        std::vector<int> histSize;
+        std::vector<float> ranges;
+        std::vector<int> channels;
+
+        for (int i = 0, stop = currFrame.channels(); i < stop; ++i)
+        {
+            histSize.push_back(bins);
+            ranges.push_back(0);
+            ranges.push_back(255);
+            channels.push_back(i);
+        }
+
+        std::vector<cv::UMat> regROI = { currFrame(reg.m_brect) };
+        cv::calcHist(regROI, channels, cv::Mat(), reg.m_hist, histSize, ranges, false);
+        cv::normalize(reg.m_hist, reg.m_hist, 0, 1, cv::NORM_MINMAX, -1, cv::Mat());
+    }
+    if (!reg.m_hist.empty() && !m_lastRegion.m_hist.empty())
+    {
+#if (((CV_VERSION_MAJOR == 4) && (CV_VERSION_MINOR < 1)) || (CV_VERSION_MAJOR == 3))
+        res = static_cast<track_t>(cv::compareHist(reg.m_hist, m_lastRegion.m_hist, CV_COMP_BHATTACHARYYA));
+        //res = 1.f - static_cast<track_t>(cv::compareHist(reg.m_hist, m_lastRegion.m_hist, CV_COMP_CORREL));
+#else
+        res = static_cast<track_t>(cv::compareHist(reg.m_hist, m_lastRegion.m_hist, cv::HISTCMP_BHATTACHARYYA));
+#endif
+    }
+
+    return res;
+}
+
+///
+/// \brief CTrack::Update
+/// \*param region
+/// \param dataCorrect
+/// \param max_trace_length
+/// \param prevFrame
+/// \param currFrame
+/// \param trajLen
+///
+void CTrack::Update(
+        const CRegion& region,
+        bool dataCorrect,
+        size_t max_trace_length,
+        cv::UMat prevFrame,
+        cv::UMat currFrame,
+        int trajLen
+        )
+{
+    if (m_filterObjectSize) // Kalman filter for object coordinates and size
+        RectUpdate(region, dataCorrect, prevFrame, currFrame);
+    else // Kalman filter only for object center
+        PointUpdate(region.m_rrect.center, region.m_rrect.size, dataCorrect, currFrame.size());
+
+    if (dataCorrect)
+    {
+        //std::cout << m_lastRegion.m_brect << " - " << region.m_brect << std::endl;
+
+        m_lastRegion = region;
+        m_trace.push_back(m_predictionPoint, region.m_rrect.center);
+
+        CheckStatic(trajLen, currFrame, region);
+    }
+    else
+    {
+        m_trace.push_back(m_predictionPoint);
+    }
+
+    if (m_trace.size() > max_trace_length)
+        m_trace.pop_front(m_trace.size() - max_trace_length);
+}
+
+///
+/// \brief CTrack::IsStatic
+/// \return
+///
+bool CTrack::IsStatic() const
+{
+    return m_isStatic;
+}
+
+///
+/// \brief CTrack::IsStaticTimeout
+/// \param framesTime
+/// \return
+///
+bool CTrack::IsStaticTimeout(int framesTime) const
+{
+    return (m_staticFrames > framesTime);
+}
+
+///
+/// \brief CTrack::IsOutOfTheFrame
+/// \return
+///
+bool CTrack::IsOutOfTheFrame() const
+{
+    return m_outOfTheFrame;
+}
+
+///
+cv::RotatedRect CTrack::CalcPredictionEllipse(cv::Size_<track_t> minRadius) const
+{
+    // Move ellipse to velocity
+    auto velocity = m_kalman.GetVelocity();
+    Point_t d(3.f * velocity[0], 3.f * velocity[1]);
+
+    cv::RotatedRect rrect(m_predictionPoint, cv::Size2f(std::max(minRadius.width, float(fabs(d.x))), std::max(minRadius.height, float(fabs(d.y)))), 0);
+    if (fabs(d.x) + fabs(d.y) > 4) // pix
+    {
+        if (fabs(d.x) > 0.0001f)
+        {
+            track_t l = std::min(rrect.size.width, rrect.size.height) / 3;
+
+            track_t p2_l = sqrtf(sqr(d.x) + sqr(d.y));
+            rrect.center.x = l * d.x / p2_l + m_predictionPoint.x;
+            rrect.center.y = l * d.y / p2_l + m_predictionPoint.y;
+
+            rrect.angle = atanf(d.y / d.x);
+        }
+        else
+        {
+            rrect.center.y += d.y / 3;
+            rrect.angle = static_cast<float>(CV_PI / 2.);
+        }
+    }
+    return rrect;
+}
+
+///
+/// \brief CTrack::IsInsideArea
+///        If result <= 1 then center of the object is inside ellipse with prediction and velocity
+/// \param pt
+/// \return
+///
+track_t CTrack::IsInsideArea(const Point_t& pt, const cv::RotatedRect& rrect) const
+{
+    Point_t pt_(pt.x - rrect.center.x, pt.y - rrect.center.y);
+    track_t r = sqrtf(sqr(pt_.x) + sqr(pt_.y));
+    track_t t = (r > 1) ? acosf(pt_.x / r) : 0;
+    track_t t_ = t - rrect.angle;
+    Point_t pt_rotated(r * cosf(t_), r * sinf(t_));
+
+    return sqr(pt_rotated.x) / sqr(rrect.size.width) + sqr(pt_rotated.y) / sqr(rrect.size.height);
+}
+
+///
+/// \brief CTrack::WidthDist
+/// \param reg
+/// \return
+///
+track_t CTrack::WidthDist(const CRegion& reg) const
+{
+    if (m_lastRegion.m_rrect.size.width < reg.m_rrect.size.width)
+        return m_lastRegion.m_rrect.size.width / reg.m_rrect.size.width;
+    else
+        return reg.m_rrect.size.width / m_lastRegion.m_rrect.size.width;
+}
+
+///
+/// \brief CTrack::HeightDist
+/// \param reg
+/// \return
+///
+track_t CTrack::HeightDist(const CRegion& reg) const
+{
+    if (m_lastRegion.m_rrect.size.height < reg.m_rrect.size.height)
+        return m_lastRegion.m_rrect.size.height / reg.m_rrect.size.height;
+    else
+        return reg.m_rrect.size.height / m_lastRegion.m_rrect.size.height;
+}
+
+///
+/// \brief CTrack::CheckStatic
+/// \param trajLen
+/// \return
+///
+bool CTrack::CheckStatic(int trajLen, cv::UMat currFrame, const CRegion& region)
+{
+    if (!trajLen || static_cast<int>(m_trace.size()) < trajLen)
+    {
+        m_isStatic = false;
+        m_staticFrames = 0;
+        m_staticFrame = cv::UMat();
+    }
+    else
+    {
+        track_t kx = 0;
+        track_t bx = 0;
+        track_t ky = 0;
+        track_t by = 0;
+        get_lin_regress_params(m_trace, m_trace.size() - trajLen, m_trace.size(), kx, bx, ky, by);
+        track_t speed = sqrt(sqr(kx * trajLen) + sqr(ky * trajLen));
+        const track_t speedThresh = 10;
+        if (speed < speedThresh)
+        {
+            if (!m_isStatic)
+            {
+                m_staticFrame = currFrame.clone();
+                m_staticRect = region.m_brect;
+#if 0
+#ifndef SILENT_WORK
+                cv::namedWindow("m_staticFrame", cv::WINDOW_NORMAL);
+                cv::Mat img = m_staticFrame.getMat(cv::ACCESS_READ).clone();
+                cv::rectangle(img, m_staticRect, cv::Scalar(255, 0, 255), 1);
+                for (size_t i = m_trace.size() - trajLen; i < m_trace.size() - 1; ++i)
+                {
+                    cv::line(img, m_trace[i], m_trace[i + 1], cv::Scalar(0, 0, 0), 1, cv::LINE_8);
+                }
+                std::string label = "(" + std::to_string(kx) + ", "  + std::to_string(ky) + ") = " + std::to_string(speed);
+                cv::line(img,
+                         cv::Point(cvRound(bx), cvRound(by)),
+                         cv::Point(cvRound(kx * trajLen + bx), cvRound(ky * trajLen + by)),
+                         cv::Scalar(0, 0, 0), 1, cv::LINE_8);
+                cv::putText(img, label, m_staticRect.tl(), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0));
+                cv::imshow("m_staticFrame", img);
+                std::cout << "m_staticRect = " << m_staticRect << std::endl;
+                cv::waitKey(1);
+#endif
+#endif
+            }
+
+            ++m_staticFrames;
+            m_isStatic = true;
+        }
+        else
+        {
+            m_isStatic = false;
+            m_staticFrames = 0;
+            m_staticFrame = cv::UMat();
+        }
+    }
+
+    return m_isStatic;
+}
+
+///
+/// \brief GetLastRect
+/// \return
+///
+cv::RotatedRect CTrack::GetLastRect() const
+{
+    if (m_filterObjectSize)
+    {
+        return m_predictionRect;
+    }
+    else
+    {
+        return cv::RotatedRect(cv::Point2f(m_predictionPoint.x, m_predictionPoint.y), m_predictionRect.size, m_predictionRect.angle);
+    }
+}
+
+///
+/// \brief CTrack::LastRegion
+/// \return
+///
+const CRegion& CTrack::LastRegion() const
+{
+    return m_lastRegion;
+}
+
+///
+/// \brief CTrack::ConstructObject
+/// \return
+///
+TrackingObject CTrack::ConstructObject() const
+{
+    return TrackingObject(GetLastRect(), m_trackID, m_regionID, m_trace, IsStatic(), IsOutOfTheFrame(),
+                          m_lastRegion.m_type, m_lastRegion.m_confidence, m_kalman.GetVelocity());
+}
+
+///
+/// \brief CTrack::SkippedFrames
+/// \return
+///
+size_t CTrack::SkippedFrames() const
+{
+    return m_skippedFrames;
+}
+
+///
+/// \brief CTrack::SkippedFrames
+/// \return
+///
+size_t& CTrack::SkippedFrames()
+{
+    return m_skippedFrames;
+}
+
+///
+/// \brief RectUpdate
+/// \param region
+/// \param dataCorrect
+/// \param prevFrame
+/// \param currFrame
+///
+void CTrack::RectUpdate(
+        const CRegion& region,
+        bool dataCorrect,
+        cv::UMat prevFrame,
+        cv::UMat currFrame
+        )
+{
+    m_kalman.GetRectPrediction();
+
+    bool recalcPrediction = true;
+
+    auto Clamp = [](int& v, int& size, int hi) -> int
+    {
+        int res = 0;
+
+        if (size < 1)
+            size = 0;
+
+        if (v < 0)
+        {
+            res = v;
+            v = 0;
+            return res;
+        }
+        else if (v + size > hi - 1)
+        {
+            res = v;
+            v = hi - 1 - size;
+            if (v < 0)
+            {
+                size += v;
+                v = 0;
+            }
+            res -= v;
+            return res;
+        }
+        return res;
+    };
+
+    auto UpdateRRect = [&](cv::Rect prevRect, cv::Rect newRect)
+    {
+        m_predictionRect.center.x += newRect.x - prevRect.x;
+        m_predictionRect.center.y += newRect.y - prevRect.y;
+        m_predictionRect.size.width *= newRect.width / static_cast<float>(prevRect.width);
+        m_predictionRect.size.height *= newRect.height / static_cast<float>(prevRect.height);
+    };
+
+    switch (m_externalTrackerForLost)
+    {
+    case tracking::TrackNone:
+        break;
+
+    case tracking::TrackKCF:
+    case tracking::TrackMIL:
+    case tracking::TrackMedianFlow:
+    case tracking::TrackGOTURN:
+    case tracking::TrackMOSSE:
+    case tracking::TrackCSRT:
+#ifdef USE_OCV_KCF
+        if (!dataCorrect)
+        {
+            cv::Rect brect = m_predictionRect.boundingRect();
+
+            cv::Size roiSize(std::max(2 * brect.width, currFrame.cols / 4), std::max(2 * brect.height, currFrame.rows / 4));
+            if (roiSize.width > currFrame.cols)
+                roiSize.width = currFrame.cols;
+
+            if (roiSize.height > currFrame.rows)
+                roiSize.height = currFrame.rows;
+
+            cv::Point roiTL(brect.x + brect.width / 2 - roiSize.width / 2, brect.y + brect.height / 2 - roiSize.height / 2);
+            cv::Rect roiRect(roiTL, roiSize);
+            Clamp(roiRect.x, roiRect.width, currFrame.cols);
+            Clamp(roiRect.y, roiRect.height, currFrame.rows);
+
+            bool inited = false;
+            if (!m_tracker || m_tracker.empty())
+            {
+                CreateExternalTracker(currFrame.channels());
+
+                cv::Rect2d lastRect(brect.x - roiRect.x, brect.y - roiRect.y, brect.width, brect.height);
+                if (m_staticFrame.empty())
+                {
+                    int dx = 1;//m_predictionRect.width / 8;
+                    int dy = 1;//m_predictionRect.height / 8;
+                    lastRect = cv::Rect2d(brect.x - roiRect.x - dx, brect.y - roiRect.y - dy, brect.width + 2 * dx, brect.height + 2 * dy);
+                }
+                else
+                {
+                    lastRect = cv::Rect2d(m_staticRect.x - roiRect.x, m_staticRect.y - roiRect.y, m_staticRect.width, m_staticRect.height);
+                }
+
+                if (lastRect.x >= 0 &&
+                        lastRect.y >= 0 &&
+                        lastRect.x + lastRect.width < roiRect.width &&
+                        lastRect.y + lastRect.height < roiRect.height &&
+                        lastRect.area() > 0)
+                {
+                    if (m_staticFrame.empty())
+                        m_tracker->init(cv::UMat(prevFrame, roiRect), lastRect);
+                    else
+                        m_tracker->init(cv::UMat(m_staticFrame, roiRect), lastRect);
+#if 0
+#ifndef SILENT_WORK
+                    cv::Mat tmp = cv::UMat(prevFrame, roiRect).getMat(cv::ACCESS_READ).clone();
+                    cv::rectangle(tmp, lastRect, cv::Scalar(255, 255, 255), 2);
+                    cv::imshow("init", tmp);
+#endif
+#endif
+
+                    inited = true;
+                    m_outOfTheFrame = false;
+                }
+                else
+                {
+                    m_tracker.release();
+                    m_outOfTheFrame = true;
+                }
+            }
+            cv::Rect2d newRect;
+            if (!inited && !m_tracker.empty() && m_tracker->update(cv::UMat(currFrame, roiRect), newRect))
+            {
+#if 0
+#ifndef SILENT_WORK
+                cv::Mat tmp2 = cv::UMat(currFrame, roiRect).getMat(cv::ACCESS_READ).clone();
+                cv::rectangle(tmp2, newRect, cv::Scalar(255, 255, 255), 2);
+                cv::imshow("track", tmp2);
+#endif
+#endif
+
+                cv::Rect prect(cvRound(newRect.x) + roiRect.x, cvRound(newRect.y) + roiRect.y, cvRound(newRect.width), cvRound(newRect.height));
+
+                UpdateRRect(brect, m_kalman.Update(prect, true));
+
+                recalcPrediction = false;
+            }
+        }
+        else
+        {
+            if (m_tracker && !m_tracker.empty())
+                m_tracker.release();
+        }
+#else
+        std::cerr << "KCF tracker was disabled in CMAKE! Set lostTrackType = TrackNone in constructor." << std::endl;
+#endif
+        break;
+
+        //    case tracking::TrackDAT:
+        //    case tracking::TrackSTAPLE:
+        //    case tracking::TrackLDES:
+        //        if (!dataCorrect)
+        //        {
+        //            bool inited = false;
+        //            cv::Rect brect = m_predictionRect.boundingRect();
+        //            if (!m_VOTTracker)
+        //            {
+        //                CreateExternalTracker(currFrame.channels());
+
+        //                cv::Rect2d lastRect(brect.x, brect.y, brect.width, brect.height);
+        //                if (!m_staticFrame.empty())
+        //                    lastRect = cv::Rect2d(m_staticRect.x, m_staticRect.y, m_staticRect.width, m_staticRect.height);
+
+        //                if (lastRect.x >= 0 &&
+        //                        lastRect.y >= 0 &&
+        //                        lastRect.x + lastRect.width < prevFrame.cols &&
+        //                        lastRect.y + lastRect.height < prevFrame.rows &&
+        //                        lastRect.area() > 0)
+        //                {
+        //                    if (m_staticFrame.empty())
+        //                    {
+        //                        cv::Mat mat = prevFrame.getMat(cv::ACCESS_READ);
+        //                        m_VOTTracker->Initialize(mat, lastRect);
+        //                        m_VOTTracker->Train(mat, true);
+        //                    }
+        //                    else
+        //                    {
+        //                        cv::Mat mat = m_staticFrame.getMat(cv::ACCESS_READ);
+        //                        m_VOTTracker->Initialize(mat, lastRect);
+        //                        m_VOTTracker->Train(mat, true);
+        //                    }
+
+        //                    inited = true;
+        //                    m_outOfTheFrame = false;
+        //                }
+        //                else
+        //                {
+        //                    m_VOTTracker = nullptr;
+        //                    m_outOfTheFrame = true;
+        //                }
+        //            }
+        //            if (!inited && m_VOTTracker)
+        //            {
+        //                constexpr float confThresh = 0.3f;
+        //                cv::Mat mat = currFrame.getMat(cv::ACCESS_READ);
+        //                float confidence = 0;
+        //                cv::RotatedRect newRect = m_VOTTracker->Update(mat, confidence);
+        //                if (confidence > confThresh)
+        //                {
+        //                    m_VOTTracker->Train(mat, false);
+
+        //                    if (newRect.angle > 0.5f)
+        //                    {
+        //                        m_predictionRect = newRect;
+        //                        m_kalman.Update(newRect.boundingRect(), true);
+        //                    }
+        //                    else
+        //                    {
+        //                        UpdateRRect(brect, m_kalman.Update(newRect.boundingRect(), true));
+        //                    }
+        //                    recalcPrediction = false;
+        //                }
+        //            }
+        //        }
+        //        else
+        //        {
+        //            if (m_VOTTracker)
+        //                m_VOTTracker = nullptr;
+        //        }
+        //        break;
+    }
+
+    if (recalcPrediction)
+        UpdateRRect(m_predictionRect.boundingRect(), m_kalman.Update(region.m_brect, dataCorrect));
+
+    cv::Rect brect = m_predictionRect.boundingRect();
+    int dx = Clamp(brect.x, brect.width, currFrame.cols);
+    int dy = Clamp(brect.y, brect.height, currFrame.rows);
+#if 0
+    m_predictionRect.center.x += dx;
+    m_predictionRect.center.y += dy;
+#endif
+    m_outOfTheFrame = (dx != 0) || (dy != 0) || (brect.width < 2) || (brect.height < 2);
+
+    m_predictionPoint = m_predictionRect.center;
+
+    //std::cout << "brect = " << brect << ", dx = " << dx << ", dy = " << dy << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
+}
+
+///
+/// \brief CreateExternalTracker
+///
+void CTrack::CreateExternalTracker(int channels)
+{
+    switch (m_externalTrackerForLost)
+    {
+    case tracking::TrackNone:
+        if (m_VOTTracker)
+            m_VOTTracker = nullptr;
+
+#ifdef USE_OCV_KCF
+        if (m_tracker && !m_tracker.empty())
+            m_tracker.release();
+#endif
+        break;
+
+    case tracking::TrackKCF:
+#ifdef USE_OCV_KCF
+        if (!m_tracker || m_tracker.empty())
+        {
+            cv::TrackerKCF::Params params;
+            if (channels == 1)
+            {
+                params.compressed_size = 1;
+                params.desc_pca = cv::TrackerKCF::GRAY;
+                params.desc_npca = cv::TrackerKCF::GRAY;
+            }
+            else
+            {
+                params.compressed_size = 3;
+                params.desc_pca = cv::TrackerKCF::CN;
+                params.desc_npca = cv::TrackerKCF::CN;
+            }
+            params.resize = true;
+            params.detect_thresh = 0.7f;
+#if (((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 3)) || (CV_VERSION_MAJOR > 3))
+            m_tracker = cv::TrackerKCF::create(params);
+#else
+            m_tracker = cv::TrackerKCF::createTracker(params);
+#endif
+        }
+#endif
+        if (m_VOTTracker)
+            m_VOTTracker = nullptr;
+        break;
+
+    case tracking::TrackMIL:
+#ifdef USE_OCV_KCF
+        if (!m_tracker || m_tracker.empty())
+        {
+            cv::TrackerMIL::Params params;
+
+#if (((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 3)) || (CV_VERSION_MAJOR > 3))
+            m_tracker = cv::TrackerMIL::create(params);
+#else
+            m_tracker = cv::TrackerMIL::createTracker(params);
+#endif
+        }
+#endif
+        if (m_VOTTracker)
+            m_VOTTracker = nullptr;
+        break;
+
+    case tracking::TrackMedianFlow:
+#ifdef USE_OCV_KCF
+        if (!m_tracker || m_tracker.empty())
+        {
+            cv::TrackerMedianFlow::Params params;
+
+#if (((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 3)) || (CV_VERSION_MAJOR > 3))
+            m_tracker = cv::TrackerMedianFlow::create(params);
+#else
+            m_tracker = cv::TrackerMedianFlow::createTracker(params);
+#endif
+        }
+#endif
+        if (m_VOTTracker)
+            m_VOTTracker = nullptr;
+        break;
+
+    case tracking::TrackGOTURN:
+#ifdef USE_OCV_KCF
+        if (!m_tracker || m_tracker.empty())
+        {
+            cv::TrackerGOTURN::Params params;
+
+#if (((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 3)) || (CV_VERSION_MAJOR > 3))
+            m_tracker = cv::TrackerGOTURN::create(params);
+#else
+            m_tracker = cv::TrackerGOTURN::createTracker(params);
+#endif
+        }
+#endif
+        if (m_VOTTracker)
+            m_VOTTracker = nullptr;
+        break;
+
+    case tracking::TrackMOSSE:
+#ifdef USE_OCV_KCF
+        if (!m_tracker || m_tracker.empty())
+        {
+#if (((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR > 3)) || (CV_VERSION_MAJOR > 3))
+            m_tracker = cv::TrackerMOSSE::create();
+#else
+            m_tracker = cv::TrackerMOSSE::createTracker();
+#endif
+        }
+#endif
+        if (m_VOTTracker)
+            m_VOTTracker = nullptr;
+        break;
+
+    case tracking::TrackCSRT:
+#ifdef USE_OCV_KCF
+        if (!m_tracker || m_tracker.empty())
+        {
+#if (CV_VERSION_MAJOR >= 4)
+            cv::TrackerCSRT::Params params;
+            params.psr_threshold = 0.04f; // 0.035f;
+            if (channels == 1)
+            {
+                params.use_gray = true;
+                params.use_rgb = false;
+            }
+            else
+            {
+                params.use_gray = false;
+                params.use_rgb = true;
+            }
+            m_tracker = cv::TrackerCSRT::create(params);
+#endif
+        }
+#endif
+        if (m_VOTTracker)
+            m_VOTTracker = nullptr;
+        break;
+
+        //    case tracking::TrackDAT:
+        //#ifdef USE_OCV_KCF
+        //        if (m_tracker && !m_tracker.empty())
+        //            m_tracker.release();
+        //#endif
+        //        if (!m_VOTTracker)
+        //            m_VOTTracker = std::unique_ptr<DAT_TRACKER>(new DAT_TRACKER());
+        //        break;
+
+        //    case tracking::TrackSTAPLE:
+        //#ifdef USE_OCV_KCF
+        //        if (m_tracker && !m_tracker.empty())
+        //            m_tracker.release();
+        //#endif
+        //#ifdef USE_STAPLE_TRACKER
+        //        if (!m_VOTTracker)
+        //            m_VOTTracker = std::unique_ptr<STAPLE_TRACKER>(new STAPLE_TRACKER());
+        //#else
+        //        std::cerr << "Project was compiled without STAPLE tracking!" << std::endl;
+        //#endif
+        //        break;
+        //#if 1
+        //    case tracking::TrackLDES:
+        //#ifdef USE_OCV_KCF
+        //        if (m_tracker && !m_tracker.empty())
+        //            m_tracker.release();
+        //#endif
+        //#ifdef USE_STAPLE_TRACKER
+        //        if (!m_VOTTracker)
+        //            m_VOTTracker = std::unique_ptr<LDESTracker>(new LDESTracker());
+        //#else
+        //        std::cerr << "Project was compiled without STAPLE tracking!" << std::endl;
+        //#endif
+        //        break;
+        //#endif
+    }
+}
+
+///
+/// \brief PointUpdate
+/// \param pt
+/// \param dataCorrect
+///
+void CTrack::PointUpdate(
+        const Point_t& pt,
+        const cv::Size& newObjSize,
+        bool dataCorrect,
+        const cv::Size& frameSize
+        )
+{
+    m_kalman.GetPointPrediction();
+
+    m_predictionPoint = m_kalman.Update(pt, dataCorrect);
+
+    if (dataCorrect)
+    {
+        const int a1 = 1;
+        const int a2 = 9;
+        m_predictionRect.size.width = (a1 * newObjSize.width + a2 * m_predictionRect.size.width) / (a1 + a2);
+        m_predictionRect.size.height = (a1 * newObjSize.height + a2 * m_predictionRect.size.height) / (a1 + a2);
+    }
+
+    auto Clamp = [](track_t& v, int hi) -> bool
+    {
+        if (v < 0)
+        {
+            v = 0;
+            return true;
+        }
+        else if (hi && v > hi - 1)
+        {
+            v = static_cast<track_t>(hi - 1);
+            return true;
+        }
+        return false;
+    };
+    auto p = m_predictionPoint;
+    m_outOfTheFrame = Clamp(p.x, frameSize.width) || Clamp(p.y, frameSize.height) || (m_predictionRect.size.width < 2) || (m_predictionRect.size.height < 2);
+
+    //std::cout << "predictionRect = " << m_predictionRect.boundingRect() << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
+}

src/fusion/lidar_radar_fusion_cnn/Tracker/track.h

@@ -0,0 +1,316 @@
+#pragma once
+#include <iostream>
+#include <vector>
+#include <deque>
+#include <memory>
+#include <array>
+
+#ifdef USE_OCV_KCF
+#include <opencv2/tracking.hpp>
+#endif
+
+#include "defines.h"
+#include "Kalman.h"
+#include "VOTTracker.hpp"
+
+// --------------------------------------------------------------------------
+///
+/// \brief The TrajectoryPoint struct
+///
+struct TrajectoryPoint
+{
+    ///
+    /// \brief TrajectoryPoint
+    ///
+    TrajectoryPoint()
+        : m_hasRaw(false)
+    {
+    }
+
+    ///
+    /// \brief TrajectoryPoint
+    /// \param prediction
+    ///
+    TrajectoryPoint(const Point_t& prediction)
+        :
+          m_hasRaw(false),
+          m_prediction(prediction)
+    {
+    }
+
+    ///
+    /// \brief TrajectoryPoint
+    /// \param prediction
+    /// \param raw
+    ///
+    TrajectoryPoint(const Point_t& prediction, const Point_t& raw)
+        :
+          m_hasRaw(true),
+          m_prediction(prediction),
+          m_raw(raw)
+    {
+    }
+
+    bool m_hasRaw = false;
+    Point_t m_prediction;
+    Point_t m_raw;
+};
+
+// --------------------------------------------------------------------------
+///
+/// \brief The Trace class
+///
+class Trace
+{
+public:
+    ///
+    /// \brief operator []
+    /// \param i
+    /// \return
+    ///
+    const Point_t& operator[](size_t i) const
+    {
+        return m_trace[i].m_prediction;
+    }
+
+    ///
+    /// \brief operator []
+    /// \param i
+    /// \return
+    ///
+    Point_t& operator[](size_t i)
+    {
+        return m_trace[i].m_prediction;
+    }
+
+    ///
+    /// \brief at
+    /// \param i
+    /// \return
+    ///
+    const TrajectoryPoint& at(size_t i) const
+    {
+        return m_trace[i];
+    }
+
+    ///
+    /// \brief size
+    /// \return
+    ///
+    size_t size() const
+    {
+        return m_trace.size();
+    }
+
+    ///
+    /// \brief push_back
+    /// \param prediction
+    ///
+    void push_back(const Point_t& prediction)
+    {
+        m_trace.emplace_back(prediction);
+    }
+    void push_back(const Point_t& prediction, const Point_t& raw)
+    {
+        m_trace.emplace_back(prediction, raw);
+    }
+
+    ///
+    /// \brief pop_front
+    /// \param count
+    ///
+    void pop_front(size_t count)
+    {
+        if (count < size())
+            m_trace.erase(m_trace.begin(), m_trace.begin() + count);
+        else
+            m_trace.clear();
+    }
+
+    ///
+    /// \brief GetRawCount
+    /// \param lastPeriod
+    /// \return
+    ///
+    size_t GetRawCount(size_t lastPeriod) const
+    {
+        size_t res = 0;
+
+        size_t i = 0;
+        if (lastPeriod < m_trace.size())
+            i = m_trace.size() - lastPeriod;
+
+        for (; i < m_trace.size(); ++i)
+        {
+            if (m_trace[i].m_hasRaw)
+                ++res;
+        }
+
+        return res;
+    }
+
+private:
+    std::deque<TrajectoryPoint> m_trace;
+};
+
+// --------------------------------------------------------------------------
+///
+/// \brief The TrackingObject class
+///
+struct TrackingObject
+{
+    cv::RotatedRect m_rrect;           // Coordinates
+    Trace m_trace;                     // Trajectory
+    size_t m_ID = 0;                   // Objects ID
+    size_t m_regionID = 0;             // region id <M
+    bool m_isStatic = false;           // Object is abandoned
+    bool m_outOfTheFrame = false;      // Is object out of freme
+    std::string m_type;                // Objects type name or empty value
+    float m_confidence = -1;           // From Detector with score (YOLO or SSD)
+    cv::Vec<track_t, 2> m_velocity;    // pixels/sec
+
+    ///
+    TrackingObject(const cv::RotatedRect& rrect, size_t ID, size_t regionID, const Trace& trace,
+                   bool isStatic, bool outOfTheFrame, const std::string& type, float confidence, cv::Vec<track_t, 2> velocity)
+        :
+          m_rrect(rrect), m_ID(ID), m_regionID(regionID), m_isStatic(isStatic), m_outOfTheFrame(outOfTheFrame), m_type(type), m_confidence(confidence), m_velocity(velocity)
+    {
+        for (size_t i = 0; i < trace.size(); ++i)
+        {
+            auto tp = trace.at(i);
+            if (tp.m_hasRaw)
+                m_trace.push_back(tp.m_prediction, tp.m_raw);
+            else
+                m_trace.push_back(tp.m_prediction);
+        }
+    }
+
+    ///
+    bool IsRobust(int minTraceSize, float minRawRatio, cv::Size2f sizeRatio) const
+    {
+        bool res = m_trace.size() > static_cast<size_t>(minTraceSize);
+        res &= m_trace.GetRawCount(m_trace.size() - 1) / static_cast<float>(m_trace.size()) > minRawRatio;
+        if (sizeRatio.width + sizeRatio.height > 0)
+        {
+            float sr = m_rrect.size.width / m_rrect.size.height;
+            if (sizeRatio.width > 0)
+                res &= (sr > sizeRatio.width);
+
+            if (sizeRatio.height > 0)
+                res &= (sr < sizeRatio.height);
+        }
+        if (m_outOfTheFrame)
+            res = false;
+
+        return res;
+    }
+};
+
+// --------------------------------------------------------------------------
+///
+/// \brief The CTrack class
+///
+class CTrack
+{
+public:
+    CTrack(const CRegion& region,
+           tracking::KalmanType kalmanType,
+           track_t deltaTime,
+           track_t accelNoiseMag,
+           bool useAcceleration,
+           size_t trackID,
+           size_t regionID,
+           bool filterObjectSize,
+           tracking::LostTrackType externalTrackerForLost);
+
+    ///
+    /// \brief CalcDist
+    /// Euclidean distance in pixels between objects centres on two N and N+1 frames
+    /// \param reg
+    /// \return
+    ///
+    track_t CalcDistCenter(const CRegion& reg) const;
+    ///
+    /// \brief CalcDist
+    /// Euclidean distance in pixels between object contours on two N and N+1 frames
+    /// \param reg
+    /// \return
+    ///
+    track_t CalcDistRect(const CRegion& reg) const;
+    ///
+    /// \brief CalcDistJaccard
+    /// Jaccard distance from 0 to 1 between object bounding rectangles on two N and N+1 frames
+    /// \param reg
+    /// \return
+    ///
+    track_t CalcDistJaccard(const CRegion& reg) const;
+    ///
+    /// \brief CalcDistJaccard
+    /// Distance from 0 to 1 between objects histogramms on two N and N+1 frames
+    /// \param reg
+    /// \param currFrame
+    /// \return
+    ///
+    track_t CalcDistHist(const CRegion& reg, cv::UMat currFrame) const;
+
+    cv::RotatedRect CalcPredictionEllipse(cv::Size_<track_t> minRadius) const;
+    ///
+    /// \brief IsInsideArea
+    /// Test point inside in prediction area: prediction area + object velocity
+    /// \param pt
+    /// \param minVal
+    /// \return
+    ///
+    track_t IsInsideArea(const Point_t& pt, const cv::RotatedRect& rrect) const;
+    track_t WidthDist(const CRegion& reg) const;
+    track_t HeightDist(const CRegion& reg) const;
+
+    void Update(const CRegion& region, bool dataCorrect, size_t max_trace_length, cv::UMat prevFrame, cv::UMat currFrame, int trajLen);
+
+    bool IsStatic() const;
+    bool IsStaticTimeout(int framesTime) const;
+    bool IsOutOfTheFrame() const;
+
+    cv::RotatedRect GetLastRect() const;
+
+    const Point_t& AveragePoint() const;
+    Point_t& AveragePoint();
+    const CRegion& LastRegion() const;
+    size_t SkippedFrames() const;
+    size_t& SkippedFrames();
+
+    TrackingObject ConstructObject() const;
+public:
+    size_t m_regionID = 0;
+private:
+    Trace m_trace;
+    size_t m_trackID = 0;
+    size_t m_skippedFrames = 0;
+    CRegion m_lastRegion;
+
+    Point_t m_predictionPoint;
+    cv::RotatedRect m_predictionRect;
+    TKalmanFilter m_kalman;
+    bool m_filterObjectSize = false;
+    bool m_outOfTheFrame = false;
+
+    tracking::LostTrackType m_externalTrackerForLost;
+#ifdef USE_OCV_KCF
+    cv::Ptr<cv::Tracker> m_tracker;
+#endif
+    std::unique_ptr<VOTTracker> m_VOTTracker;
+
+    void RectUpdate(const CRegion& region, bool dataCorrect, cv::UMat prevFrame, cv::UMat currFrame);
+
+    void CreateExternalTracker(int channels);
+
+    void PointUpdate(const Point_t& pt, const cv::Size& newObjSize, bool dataCorrect, const cv::Size& frameSize);
+
+    bool CheckStatic(int trajLen, cv::UMat currFrame, const CRegion& region);
+    bool m_isStatic = false;
+    int m_staticFrames = 0;
+    cv::UMat m_staticFrame;
+    cv::Rect m_staticRect;
+};
+
+typedef std::vector<std::unique_ptr<CTrack>> tracks_t;

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -0,0 +1,418 @@
+#ifndef FUSION_HPP
+#define FUSION_HPP
+#include "fusion_probabilities.h"
+#include "fusionobjectarray.pb.h"
+#include <iostream>
+#include "opencv2/opencv.hpp"
+#include "objectarray.pb.h"
+#include "mobileye.pb.h"
+
+#include "Eigen/Eigen"
+
+namespace iv {
+namespace fusion {
+
+//std::vector<Match_index> match_idxs;
+
+static float time_step = 0.3;
+static std::vector<std::string> labels = {"unknown","ped","bike","car","bus_or_truck"};
+
+float ComputeDis(cv::Point2f po1, cv::Point2f po2)
+{
+    return (sqrt(pow((po1.x-po2.x),2) + pow((po1.y-po2.y),2)));
+}
+
+void Get_AssociationMat(iv::lidar::objectarray& lidar_object_arry,iv::radar::radarobjectarray& radar_object_array,
+                        std::vector<match_index>& match_idx,std::vector<int>&radar_idx)
+{
+//    std::cout<<" get mat  begin  "<<std::endl;
+    int nlidar = lidar_object_arry.obj_size();
+    int nradar = radar_object_array.obj_size();
+    match_idx.clear();
+    radar_idx.clear();
+    for(int i = 0; i<nlidar; i++)
+    {
+        match_index match;
+        match.nlidar = i;
+        std::vector<int> fuindex;
+        for(int j =0; j<nradar; j++)
+        {
+            if(radar_object_array.obj(j).bvalid() == false) continue;
+            if(iv::fusion::FusionProbabilities::ComputRadarLidarmatch(radar_object_array.obj(j),lidar_object_arry.obj(i)))
+            {
+                fuindex.push_back(j);
+            }
+        }
+        if(fuindex.size() >0)
+        {
+            std::vector<float> dis;
+            cv::Point2f po1;
+            po1.x = lidar_object_arry.obj(i).centroid().x();
+            po1.y = lidar_object_arry.obj(i).centroid().y();
+            for(int index =0; index< dis.size(); index++)
+            {
+                cv::Point2f po2;
+                po2.x = radar_object_array.obj(dis[index]).x();
+                po2.y = radar_object_array.obj(dis[index]).y();
+                dis.push_back(ComputeDis(po1,po2));
+            }
+//            std::cout<<"  x    y     :   "<<po1.x<<"   "<<po1.y<<std::endl;
+            auto smallest = std::min_element(std::begin(dis), std::end(dis));
+            int index_ = std::distance(std::begin(dis), smallest);
+            dis.clear();
+            match.nradar = index_;
+        }else {
+            match.nradar = -1000;
+        }
+        match_idx.push_back(match);
+
+    }
+//std::cout<<"  match_size   :   "<<match_idx.size()<<"    "<<match_idx[0].nradar<<std::endl;
+
+    for(int k = 0; k<radar_object_array.obj_size(); k++)
+    {
+        std::vector<int> indexs;
+        for(int radar_idx =0; radar_idx<match_idx.size(); radar_idx++)
+        {
+            indexs.push_back(match_idx[radar_idx].nradar);
+        }
+        if(radar_object_array.obj(k).bvalid() == false) continue;
+        if(abs(radar_object_array.obj(k).x())< 2 && radar_object_array.obj(k).y()< 100 ){
+            if(!(std::find(indexs.begin(),indexs.end(),k) !=indexs.end()))
+            {
+                radar_idx.push_back(k);
+                //            std::cout<<"    x    y   "<<radar_object_array.obj(k).x()<<"   "<<radar_object_array.obj(k).y()<<std::endl;
+            }
+        }}
+//            std::cout<<"   radar_size   :   "<<"    "<<radar_idx.size()<<std::endl;
+//    std::cout<<"   get  mat end  "<<std::endl;
+}
+
+void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
+{
+//    std::cout<< "    get   fusion  begin   "<<std::endl;
+    lidar_radar_fusion_object_array.Clear();
+    std::vector<match_index> match_idx;
+    std::vector<int> radar_idx;
+    Get_AssociationMat(lidar_object_arr,radar_object_array,match_idx,radar_idx);
+    for(int i =0; i< match_idx.size(); i++)
+    {
+        iv::fusion::fusionobject fusion_object;
+        fusion_object.set_id(lidar_object_arr.obj(match_idx[i].nlidar).id());
+        fusion_object.set_type_name(lidar_object_arr.obj(match_idx[i].nlidar).type_name());
+        fusion_object.set_prob(lidar_object_arr.obj(match_idx[i].nlidar).score());
+        if(match_idx.at(i).nradar == -1000)
+        {
+//            std::cout<<"   fusion    is    ok  "<<std::endl;
+            fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+            iv::fusion::VelXY vel_relative;
+            iv::fusion::VelXY *vel_relative_;
+            vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).position().x());
+            vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).position().y());
+            vel_relative_ = fusion_object.mutable_vel_relative();
+            vel_relative_->CopyFrom(vel_relative);
+
+            iv::fusion::PointXYZ centroid;
+            iv::fusion::PointXYZ *centroid_;
+            centroid.set_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x());
+            centroid.set_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().y());
+            centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
+            centroid_ = fusion_object.mutable_centroid();
+            centroid_->CopyFrom(centroid);
+        }else {
+//             std::cout<<"   fusion    is    ok  "<<std::endl;
+            fusion_object.set_yaw(radar_object_array.obj(match_idx.at(i).nradar).angle());
+            fusion_object.set_lifetime(radar_object_array.obj(match_idx.at(i).nradar).has_bridge_object());
+
+            iv::fusion::VelXY vel_relative;
+            iv::fusion::VelXY *vel_relative_;
+            vel_relative.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
+            vel_relative.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
+            vel_relative_ = fusion_object.mutable_vel_relative();
+            vel_relative_->CopyFrom(vel_relative);
+
+            iv::fusion::VelXY vel_abs;
+            iv::fusion::VelXY *vel_abs_;
+            vel_abs.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
+            vel_abs.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
+            vel_abs_ = fusion_object.mutable_vel_abs();
+            vel_abs_->CopyFrom(vel_abs);
+
+            iv::fusion::PointXYZ centroid;
+            iv::fusion::PointXYZ *centroid_;
+            centroid.set_x(radar_object_array.obj(match_idx[i].nradar).x());
+            centroid.set_y(radar_object_array.obj(match_idx[i].nradar).y());
+            centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
+            centroid_ = fusion_object.mutable_centroid();
+            centroid_->CopyFrom(centroid);
+        }
+
+        iv::fusion::PointXYZ min_point;
+        iv::fusion::PointXYZ *min_point_;
+        min_point.set_x(lidar_object_arr.obj(match_idx[i].nlidar).position().x()
+                        - lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/2.0);
+        min_point.set_y(lidar_object_arr.obj(match_idx[i].nlidar).position().y()
+                        - lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()/2.0);
+        min_point_ = fusion_object.mutable_min_point();
+        min_point_->CopyFrom(min_point);
+
+        iv::fusion::Dimension dimension;
+        iv::fusion::Dimension *dimension_;
+        dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
+        dimension.set_y(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y());
+        dimension.set_z(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().z());
+        dimension_ = fusion_object.mutable_dimensions();
+        dimension_->CopyFrom(dimension);
+
+        if((lidar_object_arr.obj(match_idx[i].nlidar).centroid().x()>0)&&
+                (lidar_object_arr.obj(match_idx[i].nlidar).centroid().y()>0))
+        {
+            int xp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/0.2)/2.0);
+            if(xp == 0)xp=1;
+            int yp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()/0.2)/2.0);
+            if(yp == 0)yp=1;
+            int ix,iy;
+            for(ix = 0; ix<(xp*2); ix++)
+            {
+                for(iy = 0; iy<(yp*2); iy++)
+                {
+                    iv::fusion::NomalXYZ nomal_centroid;
+                    iv::fusion::NomalXYZ *nomal_centroid_;
+                    float nomal_x = ix*0.2 - xp*0.2;
+                    float nomal_y = iy*0.2 - yp*0.2;
+                    float nomal_z = lidar_object_arr.obj(match_idx[i].nlidar).centroid().z();
+                    float s = nomal_x*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) -
+                            nomal_y*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+                    float t = nomal_x*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) +
+                            nomal_y*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+                    nomal_centroid.set_nomal_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() + s);
+                    nomal_centroid.set_nomal_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() + t);
+                    nomal_centroid_ = fusion_object.add_nomal_centroid();
+                    nomal_centroid_->CopyFrom(nomal_centroid);
+                }
+            }
+        }
+        for(int k = 0; k<10; k++)
+        {
+            //            std::cout<<"fusion    begin"<<std::endl;
+            iv::fusion::PointXYZ point_forcaste;
+            iv::fusion::PointXYZ *point_forcaste_;
+            float forcast_x = lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() +
+                    lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()*time_step*(k+1);
+            float forcast_y = lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() +
+                    lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()*time_step*(k+1);
+            point_forcaste.set_x(forcast_x);
+            point_forcaste.set_y(forcast_y);
+            point_forcaste.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
+            point_forcaste_ = fusion_object.add_point_forecast();
+            point_forcaste_->CopyFrom(point_forcaste);
+
+            iv::fusion::NomalForecast forcast_normal;
+            iv::fusion::NomalForecast *forcast_normal_;
+            forcast_normal.set_forecast_index(i);
+            //            std::cout<<"fusion     end"<<std::endl;
+            if((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()>0)&&
+                    (lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()>0))
+            {
+                int xp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/0.2)/2.0);
+                if(xp == 0)xp=1;
+                int yp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()/0.2)/2.0);
+                if(yp == 0)yp=1;
+                int ix,iy;
+                for(ix = 0; ix<(xp*2); ix++)
+                {
+                    for(iy = 0; iy<(yp*2); iy++)
+                    {
+                        iv::fusion::NomalXYZ nomal_forcast;
+                        iv::fusion::NomalXYZ *nomal_forcast_;
+                        float nomal_x = ix*0.2 - xp*0.2;
+                        float nomal_y = iy*0.2 - yp*0.2;
+                        float nomal_z = lidar_object_arr.obj(match_idx[i].nlidar).centroid().z();
+                        float s = nomal_x*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) -
+                                nomal_y*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+                        float t = nomal_x*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) +
+                                nomal_y*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+                        nomal_forcast.set_nomal_x(forcast_x + s);
+                        nomal_forcast.set_nomal_y(forcast_y + t);
+                        nomal_forcast_ = forcast_normal.add_forecast_nomal();
+                        nomal_forcast_->CopyFrom(nomal_forcast);
+                    }
+                }
+            }
+            forcast_normal_=fusion_object.add_forecast_nomals();
+            forcast_normal_->CopyFrom(forcast_normal);
+        }
+
+        iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
+        pobj->CopyFrom(fusion_object);
+    }
+    for(int j = 0; j< radar_idx.size() ; j++){
+        iv::fusion::fusionobject fusion_obj;
+        fusion_obj.set_yaw(radar_object_array.obj(radar_idx[j]).angle());
+        iv::fusion::VelXY vel_relative;
+        iv::fusion::VelXY *vel_relative_;
+        vel_relative.set_x(radar_object_array.obj(radar_idx[j]).vx());
+        vel_relative.set_y(radar_object_array.obj(radar_idx[j]).vy());
+        vel_relative_ = fusion_obj.mutable_vel_relative();
+        vel_relative_->CopyFrom(vel_relative);
+
+        iv::fusion::PointXYZ centroid;
+        iv::fusion::PointXYZ *centroid_;
+        centroid.set_x(radar_object_array.obj(radar_idx[j]).x());
+        centroid.set_y(radar_object_array.obj(radar_idx[j]).y());
+        centroid.set_z(1.0);
+        centroid_ = fusion_obj.mutable_centroid();
+        centroid_->CopyFrom(centroid);
+
+        iv::fusion::Dimension dimension;
+        iv::fusion::Dimension *dimension_;
+        dimension.set_x(0.5);
+        dimension.set_y(0.5);
+        dimension.set_z(1.0);
+        dimension_ = fusion_obj.mutable_dimensions();
+        dimension_->CopyFrom(dimension);
+
+        int xp = (int)((0.5/0.2)/2.0);
+        if(xp == 0)xp=1;
+        int yp = (int)((0.5/0.2)/2.0);
+        if(yp == 0)yp=1;
+        int ix,iy;
+        for(ix = 0; ix<(xp*2); ix++)
+        {
+            for(iy = 0; iy<(yp*2); iy++)
+            {
+                iv::fusion::NomalXYZ nomal_centroid;
+                iv::fusion::NomalXYZ *nomal_centroid_;
+                float nomal_x = ix*0.2 - xp*0.2;
+                float nomal_y = iy*0.2 - yp*0.2;
+                float nomal_z = 1.0;
+                float s = nomal_x*cos(radar_object_array.obj(radar_idx[j]).angle())
+                        - nomal_y*sin(radar_object_array.obj(radar_idx[j]).angle());
+                float t = nomal_x*sin(radar_object_array.obj(radar_idx[j]).angle())
+                        + nomal_y*cos(radar_object_array.obj(radar_idx[j]).angle());
+                nomal_centroid.set_nomal_x(radar_object_array.obj(radar_idx[j]).x() + s);
+                nomal_centroid.set_nomal_y(radar_object_array.obj(radar_idx[j]).y() + t);
+                nomal_centroid_ = fusion_obj.add_nomal_centroid();
+                nomal_centroid_->CopyFrom(nomal_centroid);
+            }
+        }
+
+        for(int k = 0; k<10; k++)
+        {
+            //                std::cout<<"fusion    begin"<<std::endl;
+            iv::fusion::PointXYZ point_forcaste;
+            iv::fusion::PointXYZ *point_forcaste_;
+            float forcast_x = radar_object_array.obj(radar_idx[j]).x()
+                    + radar_object_array.obj(radar_idx[j]).vx()*time_step*(k+1);
+            float forcast_y = radar_object_array.obj(radar_idx[j]).y()
+                    + radar_object_array.obj(radar_idx[j]).vy()*time_step*(k+1);
+            point_forcaste.set_x(forcast_x);
+            point_forcaste.set_y(forcast_y);
+            point_forcaste.set_z(1.0);
+            point_forcaste_ = fusion_obj.add_point_forecast();
+            point_forcaste_->CopyFrom(point_forcaste);
+
+            iv::fusion::NomalForecast forcast_normal;
+            iv::fusion::NomalForecast *forcast_normal_;
+            forcast_normal.set_forecast_index(k);
+
+            int xp = (int)((0.5/0.2)/2.0);
+            if(xp == 0)xp=1;
+            int yp = (int)((0.5/0.2)/2.0);
+            if(yp == 0)yp=1;
+            int ix,iy;
+            for(ix = 0; ix<(xp*2); ix++)
+            {
+                for(iy = 0; iy<(yp*2); iy++)
+                {
+                    iv::fusion::NomalXYZ nomal_forcast;
+                    iv::fusion::NomalXYZ *nomal_forcast_;
+                    float nomal_x = ix*0.2 - xp*0.2;
+                    float nomal_y = iy*0.2 - yp*0.2;
+                    float nomal_z = 1.0;
+                    float s = nomal_x*cos(radar_object_array.obj(radar_idx[j]).angle())
+                            - nomal_y*sin(radar_object_array.obj(radar_idx[j]).angle());
+                    float t = nomal_x*sin(radar_object_array.obj(radar_idx[j]).angle())
+                            + nomal_y*cos(radar_object_array.obj(radar_idx[j]).angle());
+                    nomal_forcast.set_nomal_x(forcast_x + s);
+                    nomal_forcast.set_nomal_y(forcast_y + t);
+                    nomal_forcast_ = forcast_normal.add_forecast_nomal();
+                    nomal_forcast_->CopyFrom(nomal_forcast);
+                }
+            }
+            forcast_normal_=fusion_obj.add_forecast_nomals();
+            forcast_normal_->CopyFrom(forcast_normal);
+        }
+
+        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
+        obj->CopyFrom(fusion_obj);
+    }
+//    std::cout<<"   fusion   end    "<<std::endl;
+}
+
+
+void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::mobileye::mobileye& xobs_info)
+{
+    int time_step = 0.3;
+    lidar_radar_fusion_object_array.Clear();
+    for(int j = 0; j< xobs_info.xobj_size() ; j++){
+        iv::fusion::fusionobject fusion_obj;
+        fusion_obj.set_yaw(xobs_info.xobj(j).obsang());
+        iv::fusion::VelXY vel_relative;
+        iv::fusion::VelXY *vel_relative_;
+        vel_relative.set_x(xobs_info.xobj(j).obs_rel_vel_x() * tan(xobs_info.xobj(j).obsang()));
+        vel_relative.set_y(xobs_info.xobj(j).obs_rel_vel_x());
+        vel_relative_ = fusion_obj.mutable_vel_relative();
+        vel_relative_->CopyFrom(vel_relative);
+
+        iv::fusion::PointXYZ centroid;
+        iv::fusion::PointXYZ *centroid_;
+        centroid.set_x(-(xobs_info.xobj(j).pos_y()));
+        centroid.set_y(xobs_info.xobj(j).pos_x());
+        centroid.set_z(1.0);
+        centroid_ = fusion_obj.mutable_centroid();
+        centroid_->CopyFrom(centroid);
+
+        iv::fusion::Dimension dimension;
+        iv::fusion::Dimension *dimension_;
+        dimension.set_x(xobs_info.xobj(j).obswidth());
+        dimension.set_y(2.0);
+        dimension.set_z(1.0);
+        dimension_ = fusion_obj.mutable_dimensions();
+        dimension_->CopyFrom(dimension);
+
+        int xp = (int)((xobs_info.xobj(j).obswidth()/0.2)/2.0);
+        if(xp == 0)xp=1;
+        int yp = (int)((2.0/0.2)/2.0);
+        if(yp == 0)yp=1;
+        int ix,iy;
+        for(ix = 0; ix<(xp*2); ix++)
+        {
+            for(iy = 0; iy<(yp*2); iy++)
+            {
+                iv::fusion::NomalXYZ nomal_centroid;
+                iv::fusion::NomalXYZ *nomal_centroid_;
+                float nomal_x = ix*0.2 - xp*0.2;
+                float nomal_y = iy*0.2 - yp*0.2;
+                float nomal_z = 1.0;
+                float s = nomal_x*cos(-xobs_info.xobj(j).obsang())
+                        - nomal_y*sin(-xobs_info.xobj(j).obsang());
+                float t = nomal_x*sin(-xobs_info.xobj(j).obsang())
+                        + nomal_y*cos(-xobs_info.xobj(j).obsang());
+                nomal_centroid.set_nomal_x(xobs_info.xobj(j).pos_y() + s);
+                nomal_centroid.set_nomal_y(xobs_info.xobj(j).pos_x() + t);
+                nomal_centroid_ = fusion_obj.add_nomal_centroid();
+                nomal_centroid_->CopyFrom(nomal_centroid);
+            }
+        }
+
+        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
+        obj->CopyFrom(fusion_obj);
+    }
+
+}
+
+}
+}
+
+#endif // FUSION_HPP

src/fusion/lidar_radar_fusion_cnn/fusion_probabilities.cpp

@@ -0,0 +1,25 @@
+#include "fusion_probabilities.h"
+
+//毫米波雷达object点是否和激光雷达object的俯视box匹配
+int iv::fusion::FusionProbabilities::ComputRadarLidarmatch(const  iv::radar::radarobject& radarPoint, const  iv::lidar::lidarobject& lidarobject)
+{
+    Eigen::Matrix<float,3,1> radar_in_radar, radar_in_lidar;
+    radar_in_radar << radarPoint.x(), radarPoint.y(),-0.95;
+    radar_in_lidar = iv::fusion::Transformation::RadarToLidar(radar_in_radar);
+//    std::cout<<" x    y   "<<lidarobject.centroid().x()<<"    "<<lidarobject.centroid().y()<<std::endl;
+    if(!((radarPoint.x()>=(lidarobject.centroid().x() - lidarobject.dimensions().x()/2.0 -2))&&
+         (radarPoint.y()>= (lidarobject.centroid().y()-lidarobject.dimensions().y()/2.0 -2 ))
+         &&(radarPoint.x()<=(lidarobject.centroid().x() + lidarobject.dimensions().x()/2.0 +2))&&
+         (radarPoint.y()<=(lidarobject.centroid().y() + lidarobject.dimensions().y()/2.0 +2))))
+    {
+        return 0;
+    } else {
+        return 1;
+    }
+}
+
+
+
+
+
+

src/fusion/lidar_radar_fusion_cnn/fusion_probabilities.h

@@ -0,0 +1,35 @@
+#ifndef FUSION_PROBABILITIES_H
+#define FUSION_PROBABILITIES_H
+#include "transformation.h"
+#include "objectarray.pb.h"
+
+//struct correct_box
+//{
+//    Point max_point;
+//    Point min_point;
+//};
+
+
+namespace iv {
+namespace fusion {
+
+
+class FusionProbabilities{
+public:
+    FusionProbabilities(){};
+    ~FusionProbabilities(){};
+
+
+    static int ComputeRadarInBox(Point RadarInCamera, Point top_left, Point bot_right);
+    
+
+    static int ComputRadarLidarmatch(const  iv::radar::radarobject& radarPoint, const  iv::lidar::lidarobject& lidarobject);
+    };
+
+
+   }
+}
+
+
+#endif // FUSION_PROBABILITIES_H
+

src/fusion/lidar_radar_fusion_cnn/imageBuffer.h

@@ -0,0 +1,84 @@
+#ifndef IMAGEBUFFER_H
+#define IMAGEBUFFER_H
+
+#include <opencv2/opencv.hpp>
+#include <mutex>
+#include <condition_variable>
+#include <queue>
+
+
+template<typename T>
+class ConsumerProducerQueue
+{
+
+public:
+    ConsumerProducerQueue(int mxsz,bool dropFrame) :
+            maxSize(mxsz),dropFrame(dropFrame)
+    { }
+
+    bool add(T request)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        if(dropFrame && isFull())
+        {
+            lock.unlock();
+			return false;
+        }
+        else {
+            cond.wait(lock, [this]() { return !isFull(); });
+            cpq.push(request);
+            //lock.unlock();
+            cond.notify_all();
+            return true;
+        }
+    }
+
+    void consume(T &request)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        cond.wait(lock, [this]()
+        { return !isEmpty(); });
+        request = cpq.front();
+        cpq.pop();
+        //lock.unlock();
+        cond.notify_all();
+
+    }
+
+    bool isFull() const
+    {
+        return cpq.size() >= maxSize;
+    }
+
+    bool isEmpty() const
+    {
+        return cpq.size() == 0;
+    }
+
+    int length() const
+    {
+        return cpq.size();
+    }
+
+    void clear()
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        while (!isEmpty())
+        {
+            cpq.pop();
+        }
+        lock.unlock();
+        cond.notify_all();
+    }
+
+private:
+    std::condition_variable cond;
+    std::mutex mutex;
+    std::queue<T> cpq;
+    int maxSize;
+    bool dropFrame;
+};
+
+
+
+#endif

src/fusion/lidar_radar_fusion_cnn/lidar_radar_fusion_cnn.pro

@@ -0,0 +1,72 @@
+QT -= gui
+
+#CONFIG += c++14 console
+CONFIG += c++14
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+INCLUDEPATH +=Tracker
+
+HEADERS += \
+    ../../include/msgtype/fusionobject.pb.h \
+    ../../include/msgtype/fusionobjectarray.pb.h \
+    ../../include/msgtype/object.pb.h \
+    ../../include/msgtype/objectarray.pb.h \
+    ../../include/msgtype/radarobject.pb.h \
+    ../../include/msgtype/radarobjectarray.pb.h \
+    fusion.hpp \
+    fusion_probabilities.h \
+    transformation.h \
+    Tracker/Ctracker.h \
+    Tracker/defines.h \
+    Tracker/HungarianAlg.h \
+    Tracker/Kalman.h \
+    Tracker/ShortPathCalculator.h \
+    Tracker/track.h \
+    Tracker/VOTTracker.hpp \
+    Tracker/Tracking.h \
+    ../../include/msgtype/mobileye.pb.h \
+    ../../include/msgtype/mobileye_lane.pb.h \
+    ../../include/msgtype/mobileye_obs.pb.h \
+    ../../include/msgtype/mobileye_tsr.pb.h
+
+
+SOURCES += \
+        ../../include/msgtype/fusionobject.pb.cc \
+        ../../include/msgtype/fusionobjectarray.pb.cc \
+    ../../include/msgtype/object.pb.cc \
+    ../../include/msgtype/objectarray.pb.cc \
+        ../../include/msgtype/radarobject.pb.cc \
+        ../../include/msgtype/radarobjectarray.pb.cc \
+        fusion_probabilities.cpp \
+        main.cpp \
+        transformation.cpp \
+    Tracker/Ctracker.cpp \
+    Tracker/HungarianAlg.cpp \
+    Tracker/Kalman.cpp \
+    Tracker/track.cpp \
+    ../../include/msgtype/mobileye.pb.cc \
+    ../../include/msgtype/mobileye_lane.pb.cc \
+    ../../include/msgtype/mobileye_obs.pb.cc \
+    ../../include/msgtype/mobileye_tsr.pb.cc
+
+
+INCLUDEPATH += /usr/include/eigen3
+INCLUDEPATH += $$PWD/../../include/msgtype
+
+LIBS += -L/usr/local/lib -lprotobuf
+
+INCLUDEPATH += $$PWD/../../../include/
+
+LIBS += -L$$PWD/../../../bin -lxmlparam -lmodulecomm -livlog -livfault
+
+
+INCLUDEPATH += /usr/include/opencv4/
+LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so
+
+

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -0,0 +1,191 @@
+#include <QCoreApplication>
+#include <QDateTime>
+#include <iostream>
+#include "modulecomm.h"
+#include "radarobjectarray.pb.h"
+#include "objectarray.pb.h"
+#include "fusionobjectarray.pb.h"
+#include "fusionobject.pb.h"
+#include <QThread>
+#include <QString>
+#include <QMutex>
+#include "eigen3/Eigen/Dense"
+//#include "data_manager.h"
+#include "fusion.hpp"
+#include "Tracking.h"
+#include "transformation.h"
+#include "mobileye.pb.h"
+using namespace iv;
+using namespace fusion;
+
+void *gfu = iv::modulecomm::RegisterSend("li_ra_fusion",10000000,1);
+static QMutex gMutex;
+
+typedef  iv::radar::radarobjectarray RadarDataType;
+typedef  iv::lidar::objectarray LidarDataType;
+typedef  std::chrono::system_clock::time_point TimeType;
+
+iv::radar::radarobjectarray radarobjvec;
+iv::lidar::objectarray lidar_obj;
+iv::mobileye::mobileye mobileye_info;
+iv::mobileye::obs obs_info;
+iv::mobileye::lane lane_info;
+iv::mobileye::tsr tsr_info;
+
+
+QTime gTime;
+using namespace std;
+int gntemp = 0;
+iv::fusion::fusionobjectarray li_ra_fusion;
+void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& radarobjvec, iv::fusion::fusionobjectarray& li_ra_fusion);
+TrackerSettings settings;
+CTracker tracker(settings);
+bool m_isTrackerInitialized = false;
+
+
+void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    iv::radar::radarobjectarray radarobj;
+    if(nSize<1)return;
+    if(false == radarobj.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" Listenesrfront fail."<<std::endl;
+        return;
+    }
+    else{
+        //std::<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
+    }
+//        std::cout<<"radar is ok :  "<<radarobj.obj_size()<<std::endl;
+    gMutex.lock();
+    radarobjvec.CopyFrom(radarobj);
+    gMutex.unlock();
+}
+
+void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::lidar::objectarray lidarobj;
+    if(nSize<1)return;
+    if(false == lidarobj.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
+        return;
+    }
+//    std::cout<<"  lidar  is  ok   "<<lidarobj.obj_size()<<std::endl;
+    gMutex.lock();
+    datafusion(lidarobj,radarobjvec,li_ra_fusion);
+    gMutex.unlock();
+}
+
+void Listenmobileye(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::mobileye::mobileye mobileye;
+
+    if(nSize<1)return;
+    if(false == mobileye.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
+        return;
+    }
+    std::cout<< " obj size   "<<mobileye.xobj_size()<<std::endl;
+//    for (int m_index = 0; m_index< mobileye.xobj_size(); m_index++)
+//    {
+//        std::cout<<" p_x  p_y  v_x  v_y   "<<mobileye.xobj(m_index).pos_y()<< "  "<<mobileye.xobj(m_index).pos_x()
+//                <<"    "<<mobileye.xobj(m_index).obs_rel_vel_x() * tan(mobileye.xobj(m_index).obsang())
+//               <<"   "<<mobileye.xobj(m_index).obs_rel_vel_x()<<"                                   \n"
+//               << mobileye.xobj(m_index).obsang()<<std::endl;
+//    }
+
+    gMutex.lock();
+    mobileye_info.CopyFrom(mobileye);
+    gMutex.unlock();
+}
+
+int ccccc =0;
+void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& radarobjvec, iv::fusion::fusionobjectarray& li_ra_fusion)
+{
+//    gMutex.lock();
+//    Transformation::RadarPross(radarobjvec);
+//    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
+
+    AddMobileye(li_ra_fusion,mobileye_info);
+//    for(int i=0;i<li_ra_fusion.obj_size();i++)
+//    {
+//        std::cout<<"x    y      vx    vy   w "<<li_ra_fusion.obj(i).centroid().x()<<"          "
+//                <<li_ra_fusion.obj(i).centroid().y()<<"         "
+//                <<li_ra_fusion.obj(i).vel_relative().x()<<"     "
+//                <<li_ra_fusion.obj(i).vel_relative().y()<<"     "
+//                <<li_ra_fusion.obj(i).dimensions().x()<<std::endl;
+//    }
+
+
+//    int nsize =0;
+//    for(int nradar = 0; nradar<radarobjvec.obj_size(); nradar++)
+//    {
+//        if(radarobjvec.obj(nradar).bvalid() == false) continue;
+//        if(sqrt(abs(radarobjvec.obj(nradar).x()))< 4 && radarobjvec.obj(nradar).y()< 20 ) nsize = nsize +1;
+//    }
+
+//        std::cout<<"   fusion_obj_size     radar_obj_size   :   "<<li_ra_fusion.obj_size()<<"     "<<nsize<<std::endl;
+
+
+//    gMutex.lock();
+//    std::cout<<" track   begin"<<std::endl;
+    //---------------------------------------------  init tracker  -------------------------------------------------
+    if (!m_isTrackerInitialized)
+    {
+        m_isTrackerInitialized = InitTracker(tracker);
+        if (!m_isTrackerInitialized)
+        {
+            std::cerr << "Tracker initialize error!!!" << std::endl;
+        }
+    }
+    Tracking(li_ra_fusion, tracker);
+//    std::<<"track    end"<<std::endl;
+
+    //--------------------------------------------  end tracking  --------------------------------------------------
+//    gMutex.unlock();
+    string out;
+
+    if(li_ra_fusion.obj_size() == 0)
+    {
+        std::cout<<"   fake   obj"<<std::endl;
+
+        iv::fusion::fusionobject fake_obj;
+        iv::fusion::fusionobject *fake_obj_;
+        iv::fusion::PointXYZ fake_cen;
+        iv::fusion::PointXYZ *fake_cen_;
+        fake_cen.set_x(10000);
+        fake_cen.set_y(10000);
+        fake_cen.set_z(10000);
+        fake_cen_ = fake_obj.mutable_centroid();
+        fake_cen_ ->CopyFrom(fake_cen);
+
+        fake_obj_ = li_ra_fusion.add_obj();
+        fake_obj_->CopyFrom(fake_obj);
+        out = li_ra_fusion.SerializeAsString();
+    }else
+    {
+        out = li_ra_fusion.SerializeAsString();
+//        for (int k = 0; k<li_ra_fusion.obj_size();k++){
+//            std::cout<<" size   x    y    vy :   "<<li_ra_fusion.obj_size()<<"   "
+//                    <<li_ra_fusion.obj(k).centroid().x()<< "   "<<li_ra_fusion.obj(k).centroid().y()<<"    "
+//                   <<li_ra_fusion.obj(k).vel_relative().y()<<"   "<<li_ra_fusion.obj(k).nomal_centroid_size() <<std::endl;
+//        }
+    }
+    iv::modulecomm::ModuleSendMsg(gfu,out.data(),out.length());
+//    gMutex.unlock();
+}
+
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+    tracker.setSettings(settings);
+
+    void *gpa;
+    gpa = iv::modulecomm::RegisterRecv("radar",Listenesrfront);
+    gpa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndetect);
+    gpa = iv::modulecomm::RegisterRecv("mobileye",Listenmobileye);
+    return a.exec();
+}

src/fusion/lidar_radar_fusion_cnn/perceptionoutput.cpp

@@ -0,0 +1,8 @@
+#include "perceptionoutput.h"
+
+iv::Perception::PerceptionOutput::PerceptionOutput()
+{
+
+}
+
+

src/fusion/lidar_radar_fusion_cnn/transformation.cpp

@@ -0,0 +1,72 @@
+#include "transformation.h"
+
+using namespace Eigen;
+using namespace std;
+
+//计算毫米波目标距离
+float iv::fusion::Transformation::ComputelonDistance(double x, double y)
+{
+    return (sqrt(pow(x,2) + pow(y,2)));
+}
+
+//计算毫米波目标速度
+float iv::fusion::Transformation::ComputeRadarSpeed(double vx, double vy)
+{
+    return (sqrt(pow(vx,2) + pow(vy,2)));
+}
+
+//radar to lidar
+Eigen::Matrix<float,3,1> iv::fusion::Transformation::RadarToLidar(Eigen::Matrix<float,3,1> radar_int_radar)
+{
+    Eigen::Matrix<float,3,3> radar_to_lidar_R;
+    radar_to_lidar_R << -0.07164891, -0.99734797, -0.01278487,
+                        0.99733994,  -0.07180872, 0.01251175,
+                        -0.01339663, -0.0118544,  0.99983999;
+    Eigen::Matrix<float,3,1> radar_in_lidar;
+    Eigen::Matrix<float,3,1> radar_to_lidar_T;
+    radar_to_lidar_T << -0.35455075, -0.84757324, 0.26293475;
+    radar_in_lidar = radar_to_lidar_R*radar_int_radar + radar_to_lidar_T;
+    return radar_in_lidar;
+}
+
+//lidar to radar
+Eigen::Matrix<float,3,1> iv::fusion::Transformation::LidarToRadar(Eigen::Matrix<float,3,1> lidar_in_lidar)
+{
+    Eigen::Matrix<float,3,3> lidar_to_radar_R;
+    Eigen::Matrix<float,3,1> lidar_in_radar;
+    Eigen::Matrix<float,3,1> lidar_to_radar_T;
+    lidar_in_lidar = lidar_to_radar_R*lidar_in_lidar + lidar_to_radar_T;
+    return lidar_in_radar;
+}
+
+
+
+//毫米波数据去重
+//将速度相近、目标距离相近、纵向坐标相近的目标状态置为false
+void iv::fusion::Transformation::RadarPross(iv::radar::radarobjectarray& radar_object)
+{
+    for(int r = 0; r<(radar_object.obj_size()-1);r++)
+    {
+        if(radar_object.obj(r).bvalid() == false) continue;
+        for (int s=(r+1); s<(radar_object.obj_size());s++)
+        {
+            if(radar_object.obj(s).bvalid() == false) continue;
+            float lonDistance1 = ComputelonDistance(radar_object.obj(r).x(),radar_object.obj(r).y());
+            float lonDistance2 = ComputelonDistance(radar_object.obj(s).x(),radar_object.obj(s).y());
+            float RadarSpeed1 = ComputeRadarSpeed(radar_object.obj(r).vx(),radar_object.obj(r).vy());
+            float RadarSpeeds = ComputeRadarSpeed(radar_object.obj(s).vx(), radar_object.obj(s).vy());
+
+            if(
+                   (fabs(lonDistance1- lonDistance2) <= 1.00) &&
+                   (fabs(radar_object.obj(r).x() - radar_object.obj(s).x()) <=1.00)&&
+                   (fabs(RadarSpeed1-RadarSpeeds)<=1.00)
+             )
+            {
+                iv::radar::radarobject & radar_objct_new = (iv::radar::radarobject&)radar_object.obj(r);
+                radar_objct_new.set_bvalid(false);
+            }
+
+        }
+
+    }
+}

src/fusion/lidar_radar_fusion_cnn/transformation.h

@@ -0,0 +1,58 @@
+#ifndef TRANSFORMATION_H
+#define TRANSFORMATION_H
+#include "eigen3/Eigen/Dense"
+//#include "objectarray.pb.h"
+#include "radarobjectarray.pb.h"
+#include "fusionobjectarray.pb.h"
+#include <opencv2/opencv.hpp>
+struct Point{
+    double x;
+    double y;
+    double z;
+    double i;   //强度信息
+    double r;
+    double g;
+    double b;
+};
+
+struct Vel_Struct{
+    double vx;
+    double vy;
+};
+
+struct match_index
+{
+    int nlidar;
+    int nradar;
+};
+
+namespace iv {
+namespace fusion {
+
+
+class Transformation{
+public:
+    Transformation(){};
+    ~Transformation(){};
+
+    static Point RadarToCamera(Point radarInfo);
+
+//    static Point CameraToWorld(Point CameraInfo);
+
+    static void RadarPross(iv::radar::radarobjectarray& radar_object);
+
+    static float ComputelonDistance(double x, double y);
+
+    static float ComputeRadarSpeed(double vx, double vy);
+
+    static Vel_Struct lidarVelTra(float yaw, float LineVel);
+
+    static Eigen::Matrix<float,3,1> RadarToLidar(Eigen::Matrix<float,3,1> radar_in_radar);
+    static Eigen::Matrix<float,3,1> LidarToRadar(Eigen::Matrix<float,3,1> lidar_in_lidar);
+
+
+
+    };
+}
+}
+#endif // TRANSFORMATION_H