add pro lidar_radar_fusion............---tianxiaosen

src/fusion/lidar_radar_fusion/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/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/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/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/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/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/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/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/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/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/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/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/fusion.hpp

@@ -0,0 +1,321 @@
+#ifndef FUSION_HPP
+#define FUSION_HPP
+#include "fusion_probabilities.h"
+#include "fusionobjectarray.pb.h"
+#include <iostream>
+#include "opencv2/opencv.hpp"
+#include "perceptionoutput.h"
+
+namespace iv {
+namespace fusion {
+
+
+static float time_step = 0.3;
+static std::vector<std::string> labels = {"unknown","ped","bike","car","bus_or_truck"};
+
+typedef  std::vector<iv::Perception::PerceptionOutput> LidarObject;
+float ComputeDis(cv::Point2f po1, cv::Point2f po2)
+{
+    return (sqrt(pow((po1.x-po2.x),2) + pow((po1.y-po2.y),2)));
+}
+
+void RLfusion(LidarObject& lidar_object_vector,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
+{
+    lidar_radar_fusion_object_array.Clear();
+//    std::cout<<"~~~~~~~~~~~~"<<lidar_object_array.obj_size()<<std::endl;
+
+    for(int i = 0; i<lidar_object_vector.size(); i++)
+    {
+        std::vector<int> fuindex;
+        for(int j =0; j<radar_object_array.obj_size(); j++)
+        {
+            if(radar_object_array.obj(j).bvalid() == false) continue;
+            if(!(iv::fusion::FusionProbabilities::ComputRadarLidarmatch(radar_object_array.obj(j),lidar_object_vector.at(i))))
+            {
+                fuindex.push_back(j);
+            }
+        }
+        if(fuindex.size() == 0)
+        {
+            iv::fusion::fusionobject fusion_object;
+            fusion_object.set_id(lidar_object_vector.at(i).tracker_id);
+            fusion_object.set_type_name(labels[lidar_object_vector.at(i).label]);
+            fusion_object.set_prob(lidar_object_vector.at(i).robustness);
+            fusion_object.set_yaw(lidar_object_vector.at(i).yaw);
+            fusion_object.set_lifetime(lidar_object_vector.at(i).visible_life);
+
+            iv::fusion::VelXY vel_relative;
+            iv::fusion::VelXY *vel_relative_;
+            vel_relative.set_x(lidar_object_vector.at(i).velocity.x);
+            vel_relative.set_y(lidar_object_vector.at(i).velocity.y);
+            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(lidar_object_vector.at(i).velocity_abs.x);
+            vel_abs.set_y(lidar_object_vector.at(i).velocity_abs.y);
+            vel_abs_ = fusion_object.mutable_vel_abs();
+            vel_abs_->CopyFrom(vel_abs);
+
+            iv::fusion::AccXY acc_relative;
+            iv::fusion::AccXY *acc_relative_;
+            acc_relative.set_x(lidar_object_vector.at(i).acceleration_abs.x);
+            acc_relative.set_y(lidar_object_vector.at(i).acceleration_abs.y);
+            acc_relative_ = fusion_object.mutable_acc_relative();
+            acc_relative_->CopyFrom(acc_relative);
+
+            iv::fusion::PointXYZ min_point;
+            iv::fusion::PointXYZ *min_point_;
+            min_point.set_x(lidar_object_vector.at(i).nearest_point.x);
+            min_point.set_y(lidar_object_vector.at(i).nearest_point.y);
+            min_point_ = fusion_object.mutable_min_point();
+            min_point_->CopyFrom(min_point);
+
+            iv::fusion::PointXYZ centroid;
+            iv::fusion::PointXYZ *centroid_;
+            centroid.set_x(lidar_object_vector.at(i).location.x);
+            centroid.set_y(lidar_object_vector.at(i).location.y);
+            centroid.set_z(lidar_object_vector.at(i).location.z);
+            centroid_ = fusion_object.mutable_centroid();
+            centroid_->CopyFrom(centroid);
+
+            iv::fusion::Dimension dimension;
+            iv::fusion::Dimension *dimension_;
+            dimension.set_x(lidar_object_vector.at(i).size.x);
+            dimension.set_y(lidar_object_vector.at(i).size.y);
+            dimension.set_z(lidar_object_vector.at(i).size.z);
+            dimension_ = fusion_object.mutable_dimensions();
+            dimension_->CopyFrom(dimension);
+
+            if((lidar_object_vector.at(i).size.x>0)&&(lidar_object_vector.at(i).size.y>0))
+            {
+                int xp = (int)((lidar_object_vector.at(i).size.x/0.2)/2.0);
+                if(xp == 0)xp=1;
+                int yp = (int)((lidar_object_vector.at(i).size.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_vector.at(i).location.z;
+                        float s = nomal_x*cos(lidar_object_vector.at(i).yaw) - nomal_y*sin(lidar_object_vector.at(i).yaw);
+                        float t = nomal_x*sin(lidar_object_vector.at(i).yaw) + nomal_y*cos(lidar_object_vector.at(i).yaw);
+                        nomal_centroid.set_nomal_x(lidar_object_vector.at(i).location.x + s);
+                        nomal_centroid.set_nomal_y(lidar_object_vector.at(i).location.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_vector.at(i).location.x + lidar_object_vector.at(i).velocity.x*time_step*(k+1);
+                float forcast_y = lidar_object_vector.at(i).location.y + lidar_object_vector.at(i).velocity.y*time_step*(k+1);
+                point_forcaste.set_x(forcast_x);
+                point_forcaste.set_y(forcast_y);
+                point_forcaste.set_z(lidar_object_vector.at(i).location.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_vector.at(i).size.x>0)&&(lidar_object_vector.at(i).size.y>0))
+                {
+                    int xp = (int)((lidar_object_vector.at(i).size.x/0.2)/2.0);
+                    if(xp == 0)xp=1;
+                    int yp = (int)((lidar_object_vector.at(i).size.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_vector.at(i).location.z;
+                            float s = nomal_x*cos(lidar_object_vector.at(i).yaw) - nomal_y*sin(lidar_object_vector.at(i).yaw);
+                            float t = nomal_x*sin(lidar_object_vector.at(i).yaw) + nomal_y*cos(lidar_object_vector.at(i).yaw);
+                            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);
+         }else
+         {
+            std::vector<float> dis;
+            cv::Point2f po1;
+            po1.x = lidar_object_vector.at(i).location.x;
+            po1.y = lidar_object_vector.at(i).location.y;
+            for(std::vector<int>::iterator d = fuindex.begin(); d !=fuindex.end(); d++)
+            {
+                cv::Point2f po2;
+                po2.x = radar_object_array.obj(*d).x();
+                po2.y = radar_object_array.obj(*d).y();
+                dis.push_back(ComputeDis(po1,po2));
+            }
+            auto smallest = std::min_element(std::begin(dis), std::end(dis));
+            int index = std::distance(std::begin(dis), smallest);
+            dis.clear();
+
+            iv::fusion::fusionobject fusion_object;
+            fusion_object.set_id(lidar_object_vector.at(i).tracker_id);
+            fusion_object.set_type_name(labels[lidar_object_vector.at(i).label]);
+            fusion_object.set_prob(lidar_object_vector.at(i).robustness);
+            fusion_object.set_yaw(lidar_object_vector.at(i).yaw);
+            fusion_object.set_lifetime(lidar_object_vector.at(i).visible_life);
+
+            iv::fusion::VelXY vel_relative;
+            iv::fusion::VelXY *vel_relative_;
+            vel_relative.set_x(radar_object_array.obj(fuindex[index]).vx());
+            vel_relative.set_y(radar_object_array.obj(fuindex[index]).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(fuindex[index]).vx());
+            vel_abs.set_y(radar_object_array.obj(fuindex[index]).vy());
+            vel_abs_ = fusion_object.mutable_vel_abs();
+            vel_abs_->CopyFrom(vel_abs);
+
+
+            iv::fusion::AccXY acc_relative;
+            iv::fusion::AccXY *acc_relative_;
+            acc_relative.set_x(radar_object_array.obj(fuindex[index]).range_accel()*
+                               cos(radar_object_array.obj(fuindex[index]).angle()));
+            acc_relative.set_y(radar_object_array.obj(fuindex[index]).range_accel()*
+                               sin(radar_object_array.obj(fuindex[index]).angle()));
+            acc_relative_ = fusion_object.mutable_acc_relative();
+            acc_relative_->CopyFrom(acc_relative);
+
+            iv::fusion::PointXYZ centroid;
+            iv::fusion::PointXYZ *centroid_;
+            centroid.set_x(radar_object_array.obj(fuindex[index]).x());
+            centroid.set_y(radar_object_array.obj(fuindex[index]).y());
+            centroid.set_z(lidar_object_vector.at(i).location.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_vector.at(i).nearest_point.x);
+            min_point.set_y(lidar_object_vector.at(i).nearest_point.y);
+            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_vector.at(i).size.x);
+            dimension.set_y(lidar_object_vector.at(i).size.y);
+            dimension.set_z(lidar_object_vector.at(i).size.z);
+            dimension_ = fusion_object.mutable_dimensions();
+            dimension_->CopyFrom(dimension);
+
+
+            if((lidar_object_vector.at(i).size.x>0)&&(lidar_object_vector.at(i).size.y>0))
+            {
+                int xp = (int)((lidar_object_vector.at(i).size.x/0.2)/2.0);
+                if(xp == 0)xp=1;
+                int yp = (int)((lidar_object_vector.at(i).size.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_vector.at(i).location.z;
+                        float s = nomal_x*cos(lidar_object_vector.at(i).yaw) - nomal_y*sin(lidar_object_vector.at(i).yaw);
+                        float t = nomal_x*sin(lidar_object_vector.at(i).yaw) + nomal_y*cos(lidar_object_vector.at(i).yaw);
+                        nomal_centroid.set_nomal_x(radar_object_array.obj(fuindex[index]).x() + s);
+                        nomal_centroid.set_nomal_y(radar_object_array.obj(fuindex[index]).y() + t);
+                        nomal_centroid_ = fusion_object.add_nomal_centroid();
+                        nomal_centroid_->CopyFrom(nomal_centroid);
+                    }
+                }
+
+            }
+            for(int k = 0; k<10; k++)
+            {
+                iv::fusion::PointXYZ point_forcaste;
+                iv::fusion::PointXYZ *point_forcaste_;
+                float forcast_x = radar_object_array.obj(fuindex[index]).x() + radar_object_array.obj(fuindex[index]).vx()*time_step*(k+1);
+                float forcast_y = radar_object_array.obj(fuindex[index]).y() + radar_object_array.obj(fuindex[index]).vy()*time_step*(k+1);
+                point_forcaste.set_x(forcast_x);
+                point_forcaste.set_y(forcast_y);
+                point_forcaste.set_z(lidar_object_vector.at(i).location.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);
+                if((lidar_object_vector.at(i).size.x>0)&&(lidar_object_vector.at(i).size.y>0))
+                {
+                    int xp = (int)((lidar_object_vector.at(i).size.x/0.2)/2.0);
+                    if(xp == 0)xp=1;
+                    int yp = (int)((lidar_object_vector.at(i).size.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_vector.at(i).location.z;
+                            float s = nomal_x*cos(lidar_object_vector.at(i).yaw) - nomal_y*sin(lidar_object_vector.at(i).yaw);
+                            float t = nomal_x*sin(lidar_object_vector.at(i).yaw) + nomal_y*cos(lidar_object_vector.at(i).yaw);
+                            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);
+        }
+        fuindex.clear();
+    }
+}
+
+}
+}
+
+
+#endif // FUSION_HPP

src/fusion/lidar_radar_fusion/fusion_probabilities.cpp

@@ -0,0 +1,22 @@
+#include "fusion_probabilities.h"
+
+//毫米波雷达object点是否和激光雷达object的俯视box匹配
+int iv::fusion::FusionProbabilities::ComputRadarLidarmatch(const  iv::radar::radarobject& radarPoint, const  iv::Perception::PerceptionOutput& 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);
+    if(!((radar_in_lidar[0]>=(lidarobject.nearest_point.x))&&(radar_in_lidar[1]>= (lidarobject.nearest_point.y))&&
+         (radar_in_lidar[0]<=(lidarobject.location.x + lidarobject.size.x*0.5))&&(radar_in_lidar[1]<=(lidarobject.location.y + lidarobject.size.y * 0.5))))
+    {
+        return 0;
+    } else {
+        return 1;
+    }
+}
+
+
+
+
+
+

src/fusion/lidar_radar_fusion/fusion_probabilities.h

@@ -0,0 +1,34 @@
+#ifndef FUSION_PROBABILITIES_H
+#define FUSION_PROBABILITIES_H
+#include "transformation.h"
+#include "perceptionoutput.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::Perception::PerceptionOutput& lidarobject);
+    };
+
+
+   }
+}
+
+
+#endif // FUSION_PROBABILITIES_H
+

src/fusion/lidar_radar_fusion/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/lidar_radar_fusion.pro

@@ -0,0 +1,63 @@
+QT -= gui
+
+CONFIG += c++14 console
+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/radarobject.pb.h \
+    ../../include/msgtype/radarobjectarray.pb.h \
+    ../../include/msgtype/rawpic.pb.h \
+    fusion.hpp \
+    fusion_probabilities.h \
+    perceptionoutput.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 \
+
+
+SOURCES += \
+        ../../include/msgtype/fusionobject.pb.cc \
+        ../../include/msgtype/fusionobjectarray.pb.cc \
+        ../../include/msgtype/radarobject.pb.cc \
+        ../../include/msgtype/radarobjectarray.pb.cc \
+        ../../include/msgtype/rawpic.pb.cc \
+        fusion_probabilities.cpp \
+        main.cpp \
+    perceptionoutput.cpp \
+        transformation.cpp \
+    Tracker/Ctracker.cpp \
+    Tracker/HungarianAlg.cpp \
+    Tracker/Kalman.cpp \
+    Tracker/track.cpp \
+
+
+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/main.cpp

@@ -0,0 +1,146 @@
+#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 "perceptionoutput.h"
+#include "transformation.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;
+typedef  std::vector<iv::Perception::PerceptionOutput> LidarObject;
+
+iv::radar::radarobjectarray radarobjvec;
+LidarObject lidar_obj;
+
+
+QTime gTime;
+using namespace std;
+int gntemp = 0;
+iv::fusion::fusionobjectarray li_ra_fusion;
+void datafusion();
+TrackerSettings settings;
+CTracker tracker(settings);
+bool m_isTrackerInitialized = false;
+//iv::DataManager data_manager;
+
+void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+//    std::cout<<"radar is ok"<<std::endl;
+    iv::radar::radarobjectarray radarobj;
+    if(nSize<1)return;
+    if(false == radarobj.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" Listenesrfront fail."<<std::endl;
+        return;
+    }
+    else{
+        //std::cout<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
+    }
+    gMutex.lock();
+    radarobjvec.CopyFrom(radarobj);
+    gMutex.unlock();
+}
+
+void ListenPer(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+//    qDebug("size is %d",nSize);
+
+    std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per(new std::vector<iv::Perception::PerceptionOutput>);
+
+
+    iv::Perception::PerceptionOutput * pdata = (iv::Perception::PerceptionOutput *)strdata;
+    int nCount = nSize/sizeof(iv::Perception::PerceptionOutput);
+    int i;
+    for(i=0;i<nCount;i++)
+    {
+        iv::Perception::PerceptionOutput temp;
+        memcpy(&temp,pdata,sizeof(iv::Perception::PerceptionOutput));
+        lidar_per->push_back(temp);
+        pdata++;
+    }
+    gMutex.lock();
+    lidar_obj.swap(*lidar_per);
+//    std::cout<<"lidar   objs      :"<<lidar_obj.size()<<std::endl;
+    datafusion();
+    gMutex.unlock();
+}
+
+int ccccc =0;
+void datafusion()
+{
+//    gMutex.lock();
+//    Transformation::RadarPross(radarobjvec);
+    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
+    std::cout<<li_ra_fusion.obj_size()<<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::cout<<"track    end"<<std::endl;
+
+    //--------------------------------------------  end tracking  ---------------------------------------------------
+//    gMutex.unlock();
+    string out;
+
+    if(li_ra_fusion.obj_size() == 0)
+    {
+        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();
+    }
+    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_esr_f",Listenesrfront);
+//    gpa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndetect);
+    gpa = iv::modulecomm::RegisterRecv("lidar_per",ListenPer);
+    return a.exec();
+}

src/fusion/lidar_radar_fusion/perceptionoutput.cpp

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

src/fusion/lidar_radar_fusion/perceptionoutput.h

@@ -0,0 +1,55 @@
+#ifndef PERCEPTIONOUTPUT_H
+#define PERCEPTIONOUTPUT_H
+
+namespace iv {
+namespace Perception {
+class Point3f
+{
+public:
+  float x;
+  float y;
+  float z;
+};
+class Point2f
+{
+public:
+    float x;
+    float y;
+};
+
+class PerceptionOutput
+{
+public:
+    PerceptionOutput();
+    Point3f location; /**<position of bbox center, measured in lidar or vehicle coordinate system, according to user's setting*/
+    Point3f direction; /**<direction of bbox, vector format, measured by the length edge direction, in lidar or vehicle coordinate system, according to user's setting*/
+    float yaw; /**<direction of bbox with roll pitch raw depictions, for simplicity, just yaw is usefull, coincident with "direction"*/
+    Point3f size; /**<size of bbox, length, width, height of box, length is always >= width*/
+    Point3f nearest_point; /**<position of nearest corner of bbox, measured in lidar or vehicle coordinate system, according to user's setting*/
+
+    int tracker_id; /**<tracker id for objects, same object in sequtial frames share the same id*/
+    float track_prob; /**<tracking association probability, confidence level (0~1), the higher, the better.*/
+
+    Point2f velocity; /**<relative speed of obstacles, measured in local vehicle coordinate system*/
+    Point2f acceleration; /**<relative acceleration of obstacles, measured in local vehicle coordinate system*/
+
+    Point2f velocity_abs; /**<speed of obstacles, measured in global coordinate system*/
+    Point2f acceleration_abs; /**<acceleration of obstacles, measured in global coordinate system*/
+    float angle_velocity; /**< angle velocity in radian*/
+
+    float life; /**<total current tracker life time, including visible and invisible ones, with unit seconds*/
+    float visible_life; /**<current tracker life time only considering visible tracks, with unit seconds*/
+    float robustness; /**< robustness analyzed by a historical sequential tracker frames, the smaller, the better.*/
+
+    int label; /**<type of obstacles, like pedestrain, bike, car, truck*/  /* 0 unknown 1 ped 2 bike 3 car 4 bus/truck */
+    float label_confidence; /**< confidence of classification*/
+
+    bool is_background; /**< if is background, the flag will be set true. */
+};
+
+}
+
+}
+
+
+#endif // PERCEPTIONOUTPUT_H

src/fusion/lidar_radar_fusion/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/transformation.h

@@ -0,0 +1,53 @@
+#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;
+};
+
+
+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

src/tool/wt/detection_wt.xml

@@ -0,0 +1,15 @@
+<xml>	
+	<node name="detection_wt.xml">
+		<param name="lidarmsg" value="lidar_cnn_dectect" />
+		<param name="gpsmsg" value="hcp2_gpsimu" />
+		<param name="wt_p1_lon" value="117.027215500000" />
+		<param name="wt_p1_lat" value="39.122433800000" />
+		<param name="wt_p2_lon" value="117.027213200000" />
+		<param name="wt_p2_lat" value="39.121866600000" />
+		<param name="wt_p3_lon" value="117.027132100000" />
+		<param name="wt_p3_lat" value="39.121822200000" />
+		<param name="wt_p4_lon" value="117.027133700000" />
+		<param name="wt_p4_lat" value="39.122383200000" />
+                
+	</node>
+</xml>

src/tool/wt/gnss_coordinate_convert.cpp

@@ -0,0 +1,153 @@
+#include <gnss_coordinate_convert.h>
+
+void gps2xy(double J4, double K4, double *x, double *y)
+{
+    int L4 = (int)((K4 - 1.5) / 3) + 1;
+    double M4 = K4 - (L4 * 3);
+    double N4 = sin(J4 * 3.1415926536 / 180);
+    double O4 = cos(J4 * 3.1415926536 / 180);
+    double P4 = tan(J4 * 3.1415926536 / 180);
+    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
+    double R4 = sqrt(0.006738525414683) * O4;
+    double S4 = sqrt(1 + R4 * R4);
+    double T4 = 6399698.901783 / S4;
+    double U4 = (T4 / S4) / S4;
+    double V4 = O4 * M4 * 3.1415926536 / 180;
+    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
+    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
+    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
+    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
+
+    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
+    *x = 500000 + T4 * V4 * (Y4 + Z4);
+}
+
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+{
+    int ProjNo = 0; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double a, f, e2, ee, NN, T, C, A, M, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    ZoneWide = 6; ////6度带宽
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ProjNo = (int)(longitude / ZoneWide);
+    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI;
+    latitude0 = 0;
+    longitude1 = longitude * iPI; //经度转换为弧度
+    latitude1 = latitude * iPI; //纬度转换为弧度
+    e2 = 2 * f - f * f;
+    ee = e2 * (1.0 - e2);
+    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+    T = tan(latitude1)*tan(latitude1);
+    C = ee * cos(latitude1)*cos(latitude1);
+    A = (longitude1 - longitude0)*cos(latitude1);
+    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+        *e2 / 1024)*sin(2 * latitude1)
+        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+    X0 = 1000000L * (ProjNo + 1) + 500000L;
+    Y0 = 0;
+    xval = xval + X0; yval = yval + Y0;
+    *X = xval;
+    *Y = yval;
+}
+
+
+
+//=======================zhaobo0904
+#define PI  3.14159265358979
+void ZBGaussProjCal(double lon, double lat, double *X, double *Y)
+{
+    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
+    double a = 6378140.0;
+    double e2 = 0.006694384999588;
+    double ep2 = e2/(1-e2);
+
+    // 原点所在经度
+    double lon_origin = 6.0*int(lon/6) + 3.0;
+    lon_origin *= PI / 180.0;
+
+    double k0 = 0.9996;
+
+    // 角度转弧度
+    double lat1 = lat * PI / 180.0;
+    double lon1 = lon * PI / 180.0;
+
+
+    // 经线在该点处的曲率半径,
+    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
+
+
+    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
+    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
+    // 首先计算前四项的系数 a1~a4.
+    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
+    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
+    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
+    double a4 = (35*e2*e2*e2)/3072;
+    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
+
+    // 辅助量
+    double T = tan(lat1)*tan(lat1);
+    double C = ep2*cos(lat1)*cos(lat1);
+    double A = (lon1 - lon_origin)*cos(lat1);
+
+    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
+    *Y = M + N * tan(lat1) * ((A*A)/2 +
+                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
+                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
+
+
+    *Y *= k0;
+    return;
+}
+//==========================================================
+
+
+
+
+
+//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
+{
+    int ProjNo; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ZoneWide = 6; ////6度带宽
+    ProjNo = (int)(X / 1000000L); //查找带号
+    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI; //中央经线
+    X0 = ProjNo * 1000000L + 500000L;
+    Y0 = 0;
+    xval = X - X0; yval = Y - Y0; //带内大地坐标
+    e2 = 2 * f - f * f;
+    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
+    ee = e2 / (1 - e2);
+    M = yval;
+    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
+    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
+                4 * u)
+            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
+    C = ee * cos(fai)*cos(fai);
+    T = tan(fai)*tan(fai);
+    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
+    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
+                                                                                       (fai)*sin(fai)));
+    D = xval / NN;
+    //计算经度(Longitude) 纬度(Latitude)
+    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
+                               *D*D*D*D / 120) / cos(fai);
+    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
+                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
+    //转换为度 DD
+    *longitude = longitude1 / iPI;
+    *latitude = latitude1 / iPI;
+}

src/tool/wt/gnss_coordinate_convert.h

@@ -0,0 +1,27 @@
+#pragma once
+#ifndef _IV_PERCEPTION_GNSS_CONVERT_
+#define _IV_PERCEPTION_GNSS_CONVERT_
+
+#include <math.h>
+//double nmeaConvert2Lat(string lat)
+//{
+//	Console.WriteLine(lat);
+//	double nmea_d = double.Parse(lat.Substring(0, 2));
+//	double nmea_m = double.Parse(lat.Substring(2, 6));
+//	return nmea_d + nmea_m / 60;
+//}
+//
+//double nmeaConvert2Lon(string lon)
+//{
+//	Console.WriteLine(lon);
+//	double nmea_d = double.Parse(lon.Substring(0, 3));
+//	double nmea_m = double.Parse(lon.Substring(3, 7));
+//	return nmea_d + nmea_m / 60;
+//}
+
+void gps2xy(double , double , double *, double *);
+void GaussProjCal(double longitude, double latitude, double *X, double *Y);
+void ZBGaussProjCal(double longitude, double latitude, double *X, double *Y);
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
+
+#endif // !_IV_PERCEPTION_GNSS_CONVERT_

src/tool/wt/gps_pose_transform.cpp

@@ -0,0 +1,89 @@
+#include "gps_pose_transform.h"
+
+extern double garm_x ;
+extern double garm_y ;
+
+iv::gpspos iv::PoseToGPS(iv::gpspos xori,iv::pose xpose)
+{
+    double x_o,y_o;
+    GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
+    double lon,lat;
+    double hdg_o = (90 - xori.heading)*M_PI/180.0;
+    double rel_x = xpose.x * cos(hdg_o) - xpose.y * sin(hdg_o);
+    double rel_y = xpose.x * sin(hdg_o) + xpose.y * cos(hdg_o);
+    GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
+    double hdg_c = hdg_o + xpose.yaw;
+
+    hdg_c = M_PI/2.0 - hdg_c;
+    double heading = hdg_c * 180.0/M_PI;
+    while(heading < 0)heading = heading + 360;
+    while(heading >360)heading = heading - 360;
+    iv::gpspos xgpspos;
+    xgpspos.lon = lon;
+    xgpspos.lat = lat;
+    xgpspos.height = xpose.z + xori.height;
+    xgpspos.heading = heading;
+    xgpspos.pitch = xpose.pitch + xori.pitch;
+    xgpspos.roll  = xpose.roll + xori.roll;
+    xgpspos.ve = xpose.vx * cos(hdg_o) - xpose.vy * sin(hdg_o);
+    xgpspos.vn = xpose.vx * sin(hdg_o) + xpose.vy * cos(hdg_o);
+
+    if((garm_x != 0)||(garm_y != 0)) //arm use to Convert pos to GPS Posision
+    {
+        GaussProjCal(xgpspos.lon,xgpspos.lat,&x_o,&y_o);
+        hdg_o = (90 - xgpspos.heading)*M_PI/180.0;
+        rel_x = garm_x *(-1) * cos(hdg_o) - garm_y *(-1) * sin(hdg_o);
+        rel_y = garm_x *(-1) * sin(hdg_o) + garm_y *(-1) * cos(hdg_o);
+        GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
+        xgpspos.lon = lon;
+        xgpspos.lat = lat;
+    }
+    return xgpspos;
+}
+
+iv::pose iv::CalcPose(iv::gpspos xori, iv::gpspos xcur)
+{
+    double lon,lat;
+    if((garm_x != 0)||(garm_y != 0)) //arm use to Convert pos to GPS Posision
+    {
+        double x_o,y_o;
+        GaussProjCal(xcur.lon,xcur.lat,&x_o,&y_o);
+        double hdg_o = (90 - xcur.heading)*M_PI/180.0;
+        double rel_x = garm_x *(1) * cos(hdg_o) - garm_y *(1) * sin(hdg_o);
+        double rel_y = garm_x *(1) * sin(hdg_o) + garm_y *(1) * cos(hdg_o);
+        GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
+        xcur.lon = lon;
+        xcur.lat = lat;
+    }
+    double x_o,y_o,hdg_o;
+    double x_c,y_c,hdg_c;
+    GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
+    GaussProjCal(xcur.lon,xcur.lat,&x_c,&y_c);
+    double dis = sqrt(pow(x_c-x_o,2)+ pow(y_c-y_o,2));
+    double rel_x0,rel_y0;
+    rel_x0 = x_c -x_o;
+    rel_y0 = y_c -y_o;
+    double rel_x,rel_y;
+
+    hdg_o = (90 - xori.heading)*M_PI/180.0;
+    hdg_c = (90 - xcur.heading)*M_PI/180.0;
+    rel_x = rel_x0 * cos(-hdg_o) - rel_y0 * sin(-hdg_o);
+    rel_y = rel_x0 * sin(-hdg_o) + rel_y0 * cos(-hdg_o);
+
+
+    double rel_hdg;
+    rel_hdg = hdg_c - hdg_o;
+    iv::pose posex;
+    posex.x = -rel_x;
+    posex.y = -rel_y;
+    posex.z = xcur.height - xori.height;
+    posex.pitch = xcur.pitch - xori.pitch;
+    posex.roll = xcur.roll - xori.roll;
+    posex.yaw = rel_hdg;
+
+    posex.vx = xcur.ve * cos(-hdg_o) - xcur.vn * sin(-hdg_o);
+    posex.vy = xcur.ve * sin(-hdg_o) + xcur.vn * cos(-hdg_o);
+
+    return posex;
+
+}

src/tool/wt/gps_pose_transform.h

@@ -0,0 +1,38 @@
+#ifndef GPS_POSE_TRANSFORM_H
+#define GPS_POSE_TRANSFORM_H
+#include "gnss_coordinate_convert.h"
+
+extern double garm_x ;
+extern double garm_y ;
+
+
+namespace iv {
+struct pose
+{
+  double x;
+  double y;
+  double z;
+  double roll;
+  double pitch;
+  double yaw;
+  double vx;
+  double vy;
+};
+
+struct gpspos
+{
+    double lat;
+    double lon;
+    double height;
+    double heading;
+    double pitch;
+    double roll;
+    double ve;
+    double vn;
+};
+
+
+iv::gpspos PoseToGPS(iv::gpspos xori,iv::pose xpose);
+iv::pose CalcPose(iv::gpspos xori,iv::gpspos xcur);
+}
+#endif // GPS_POSE_TRANSFORM_H

src/tool/wt/main.cpp

@@ -0,0 +1,110 @@
+#include <QCoreApplication>
+#include "wt.h"
+#include "modulecomm.h"
+#include "objectarray.pb.h"
+#include "gpsimu.pb.h"
+#include <iostream>
+#include "xmlparam.h"
+#include <QMutex>
+
+std::string gstr_lidarmsgname;   //lidar message name
+std::string gstr_gpsmsgname;   // gps message name
+std::string gstr_arm_x;
+std::string gstr_arm_y;
+double garm_x = 0.0;
+double garm_y = 0.0;
+static QMutex qmutex;
+static QMutex gmutex;
+iv::gpspos gpose;
+iv::Wt_Area_Gps wt_area;
+iv::Wt_Area_Cal wt_area_cal;
+iv::pose pose;
+
+
+void ListenLidarTrack(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    bool wt_state = false;
+//    std::cout<<" lidar   is   ok!"<<std::endl;
+    iv::lidar::objectarray lidarobj;
+    if(nSize<1)return;
+    if(false == lidarobj.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
+        return;
+    }
+
+//    for(int j=0; j<lidarobj.obj_size(); j++)
+//    {
+//        std::cout<<" n   obj  "<<j <<std::endl;
+////        std::cout<<"x   y    z   " <<lidarobj.obj(j).centroid().x()<<"      "<<lidarobj.obj(j).centroid().y()<<"             "<<lidarobj.obj(j).centroid().z()<<std::endl;
+//    }
+    pose.x =0;
+    pose.y =0;
+    pose.z =0;
+
+
+    gmutex.lock();
+    iv::Get_Wt_Area_Cal(wt_area,gpose,wt_area_cal);
+    for(int i =0; i<lidarobj.obj_size(); i++ )
+    {
+        pose.x = lidarobj.obj(i).centroid().y();
+        pose.y =0 -lidarobj.obj(i).centroid().x();
+        pose.z = lidarobj.obj(i).centroid().z();
+
+        std::cout<<"   x  y   z  :"<<pose.x<<"    "<<pose.y<<"          "<<pose.z<<std::endl;
+    }
+    wt_state = iv::compute_point_in_area(wt_area_cal,pose);
+    std::cout<<" wt   state  : "<<wt_state<<std::endl;
+    gmutex.unlock();
+
+}
+
+void ListenGps(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+//    std::cout<<"gps   is    ok!"<<std::endl;
+    iv::gps::gpsimu xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenRaw Parse error."<<std::endl;
+    }
+    qmutex.lock();
+    gpose.lat = xgpsimu.lat();
+    gpose.lon = xgpsimu.lon();
+    gpose.heading = xgpsimu.heading();
+    gpose.height = xgpsimu.height();
+    gpose.roll = xgpsimu.roll();
+    gpose.pitch = xgpsimu.pitch();
+    gpose.ve = xgpsimu.ve();
+    gpose.vn = xgpsimu.vn();
+    qmutex.unlock();
+}
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    QString strpath = QCoreApplication::applicationDirPath();
+
+   if(argc < 2)
+       strpath = strpath + "/detection_wt.xml";
+   else
+       strpath = argv[1];
+   std::cout<<strpath.toStdString()<<std::endl;
+
+   iv::xmlparam::Xmlparam xparam(strpath.toStdString());
+   gstr_lidarmsgname = xparam.GetParam("lidarmsg","lidar_cnn_dectect");
+   gstr_gpsmsgname = xparam.GetParam("gpsmsg","hcp2_gpsimu");
+   wt_area.p1.lat = atof(xparam.GetParam("wt_p1_lat","0.0").data());
+   wt_area.p1.lon = atof(xparam.GetParam("wt_p1_lon","0.0").data());
+   wt_area.p2.lat = atof(xparam.GetParam("wt_p2_lat","0.0").data());
+   wt_area.p2.lon = atof(xparam.GetParam("wt_p2_lon","0.0").data());
+   wt_area.p3.lat = atof(xparam.GetParam("wt_p3_lat","0.0").data());
+   wt_area.p3.lon = atof(xparam.GetParam("wt_p3_lon","0.0").data());
+   wt_area.p4.lat = atof(xparam.GetParam("wt_p4_lat","0.0").data());
+   wt_area.p4.lon = atof(xparam.GetParam("wt_p4_lon","0.0").data());
+
+    void *gpl = iv::modulecomm::RegisterRecv(gstr_lidarmsgname.data(),ListenLidarTrack);
+    void *glc = iv::modulecomm::RegisterRecv(gstr_gpsmsgname.data(),ListenGps);
+
+    return a.exec();
+}

src/tool/wt/wt.cpp

@@ -0,0 +1,42 @@
+#include "wt.h"
+#include <iostream>
+
+void iv::Get_Wt_Area_Cal(iv::Wt_Area_Gps wt_area_gps, iv::gpspos gpose, iv::Wt_Area_Cal& wt_area_cal)
+{
+    wt_area_cal.p1 = iv::CalcPose(wt_area_gps.p1 ,gpose);
+    wt_area_cal.p2 = iv::CalcPose(wt_area_gps.p2 ,gpose);
+    wt_area_cal.p3 = iv::CalcPose(wt_area_gps.p3 ,gpose);
+    wt_area_cal.p4 = iv::CalcPose(wt_area_gps.p4 ,gpose);
+
+//    std::cout<<"p1    x  :  "<<wt_area_cal.p1.x<<"    p1      y:  "<<wt_area_cal.p1.y<<std::endl;
+//    std::cout<<"p2    x  :  "<<wt_area_cal.p2.x<<"    p2      y:  "<<wt_area_cal.p2.y<<std::endl;
+//    std::cout<<"p3    x  :  "<<wt_area_cal.p3.x<<"    p3      y:  "<<wt_area_cal.p3.y<<std::endl;
+//    std::cout<<"p4    x  :  "<<wt_area_cal.p4.x<<"    p4      y:  "<<wt_area_cal.p4.y<<std::endl;
+
+}
+
+bool iv::compute_point_in_area(iv::Wt_Area_Cal& wt_area_cal, iv::pose& pose)
+{
+    bool inside =false;
+    std::vector<iv::pose> wt_area_vector;
+    wt_area_vector.push_back(wt_area_cal.p1);
+    wt_area_vector.push_back(wt_area_cal.p2);
+    wt_area_vector.push_back(wt_area_cal.p3);
+    wt_area_vector.push_back(wt_area_cal.p4);
+    for(unsigned i = 1; i<=4; ++i)
+    {
+        const iv::pose& A = wt_area_vector[i-1];
+        const iv::pose& B = wt_area_vector[i%4];
+        if((B.y <= pose.y && pose.y < A.y) || (A.y <= pose.y && pose.y < B.y))
+        {
+            float t = (pose.x - B.x)*(A.y -B.y) - (A.x - B.y)*(pose.y - B.y);
+            if(A.y < B.y)
+                t = -t;
+            if(t < 0)
+                inside =! inside;
+        }
+    }
+    return inside;
+}
+
+

src/tool/wt/wt.h

@@ -0,0 +1,34 @@
+#ifndef WT_H
+#define WT_H
+#include "gps_pose_transform.h"
+#include "string"
+#include "vector"
+
+
+namespace iv {
+
+struct Wt_Area_Gps
+{
+    struct iv::gpspos p1;
+    struct iv::gpspos p2;
+    struct iv::gpspos p3;
+    struct iv::gpspos p4;
+};
+
+struct Wt_Area_Cal
+{
+    struct iv::pose p1;
+    struct iv::pose p2;
+    struct iv::pose p3;
+    struct iv::pose p4;
+};
+
+void Get_Wt_Area_Cal(iv::Wt_Area_Gps wt_area_gps, iv::gpspos gpose, iv::Wt_Area_Cal& wt_area_cal);
+
+bool compute_point_in_area(Wt_Area_Cal& wt_a, iv::pose& pose);
+
+}
+
+
+
+#endif // WT_H

src/tool/wt/wt.pro

@@ -0,0 +1,54 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has 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
+
+# You can also make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        ../../include/msgtype/gps.pb.cc \
+        ../../include/msgtype/gpsimu.pb.cc \
+        ../../include/msgtype/imu.pb.cc \
+        ../../include/msgtype/object.pb.cc \
+        ../../include/msgtype/objectarray.pb.cc \
+        gnss_coordinate_convert.cpp \
+        gps_pose_transform.cpp \
+        main.cpp \
+        wt.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+
+INCLUDEPATH += /opt/ros/melodic/include
+INCLUDEPATH += /opt/ros/kinetic/include
+INCLUDEPATH += /usr/include/pcl-1.8
+INCLUDEPATH += /usr/include/pcl-1.7
+INCLUDEPATH += /usr/include/eigen3
+INCLUDEPATH += $$PWD/../../include/msgtype/
+
+HEADERS += \
+    ../../include/msgtype/gps.pb.h \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../include/msgtype/imu.pb.h \
+    ../../include/msgtype/object.pb.h \
+    ../../include/msgtype/objectarray.pb.h \
+    gnss_coordinate_convert.h \
+    gps_pose_transform.h \
+    wt.h
+
+LIBS += -lprotobuf
+
+INCLUDEPATH += $$PWD/../../../include/
+LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault  -livexit -livbacktrace