add track->_1025

src/detection/detection_lidar_PointPillars_MultiHead_1025/Tracker/Ctracker.cpp

@@ -0,0 +1,294 @@
+#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 = 1e3;//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)
+            {
+#ifdef DEBUG_SHOW
+                std::cout<<costMatrix[i + assignment[i] * N]<<", ";
+#endif
+                if (costMatrix[i + assignment[i] * N] > m_settings.m_distThres)
+                {
+                    assignment[i] = -1;
+                    m_tracks[i]->SkippedFrames()++;
+                }
+            }
+            else
+            {
+#ifdef DEBUG_SHOW
+                std::cout<<-1<<", ";
+#endif
+                // If track have no assigned detect, then increment skipped frames counter.
+                m_tracks[i]->SkippedFrames()++;
+            }
+        }
+#ifdef DEBUG_SHOW
+                std::cout<<std::endl;
+#endif
+        // 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]->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,
+                                                        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]->DetectedFrames()++;
+            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);
+        }
+        m_tracks[i]->m_regionID = assignment[i];
+    }
+}
+
+///
+/// \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_rect.size.width;
+//            minRadius.height = m_settings.m_minAreaRadiusK * track->LastRegion().m_rect.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 (reg.m_type==-1?(m_settings.CheckType(m_tracks[i]->LastRegion().m_type_name, reg.m_type_name)):(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 0
+                    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 0
+                    track_t ellipseDist = track->IsInsideArea(reg.m_rect.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::DistRect3Ds)
+                {
+#if 0
+                    track_t ellipseDist = track->IsInsideArea(reg.m_rect.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->CalcDistRect3D(reg);
+#endif
+                }
+                ++ind;
+                assert(ind == tracking::DistsCount);
+            }
+            costMatrix[i + j * N] = dist;
+            if (dist > maxCost)
+                maxCost = dist;
+        }
+    }
+}

src/detection/detection_lidar_PointPillars_MultiHead_1025/Tracker/Ctracker.h

@@ -0,0 +1,292 @@
+#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::TrackNone;
+    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<int, std::set<int>> m_nearTypes;
+
+    std::map<std::string, std::set<std::string>> m_nearTypeNames;
+
+	///
+	TrackerSettings()
+	{
+        m_distType[tracking::DistCenters] = 0.5f;
+        m_distType[tracking::DistRects] = 0.5f;
+        m_distType[tracking::DistRect3Ds] = 0.f;
+
+		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 int& type1, const int& type2, bool sym)
+	{
+        auto AddOne = [&](const int& type1, const int& type2)
+		{
+			auto it = m_nearTypes.find(type1);
+			if (it == std::end(m_nearTypes))
+			{
+                m_nearTypes[type1] = std::set<int>{ type2 };
+			}
+			else
+			{
+				it->second.insert(type2);
+			}
+		};
+		AddOne(type1, type2);
+		if (sym)
+        {
+			AddOne(type2, type1);
+        }
+	}
+
+	///
+    bool CheckType(const int& type1, const int& type2) const
+	{
+        bool res = type1==-1 || type2==-1 || (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;
+	}
+
+    ///
+    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_nearTypeNames.find(type1);
+            if (it == std::end(m_nearTypeNames))
+            {
+                m_nearTypeNames[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_nearTypeNames.find(type1);
+            if (it != std::end(m_nearTypeNames))
+            {
+                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::TrackNone);
+        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/detection/detection_lidar_PointPillars_MultiHead_1025/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/detection/detection_lidar_PointPillars_MultiHead_1025/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/detection/detection_lidar_PointPillars_MultiHead_1025/Tracker/Kalman.cpp

@@ -0,0 +1,873 @@
+#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(cv::Point3f xy0, cv::Point3f 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::CreateLinear3D(Rect3D rect0, cv::Point3f 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.
+
+    // 14 state variables (x, y, z, width, height, length, yaw, d...), 7 measurements (x, y, z, width, height, length, yaw)
+    m_linearKalman.init(14, 7, 0, El_t);
+    // Transition cv::Matrix
+    m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(14, 14) <<
+                                        1, 0, 0, 0, 0, 0, 0, m_deltaTime, 0,           0,           0,           0,           0,           0,
+                                        0, 1, 0, 0, 0, 0, 0, 0,           m_deltaTime, 0,           0,           0,           0,           0,
+                                        0, 0, 1, 0, 0, 0, 0, 0,           0,           m_deltaTime, 0,           0,           0,           0,
+                                        0, 0, 0, 1, 0, 0, 0, 0,           0,           0,           m_deltaTime, 0,           0,           0,
+                                        0, 0, 0, 0, 1, 0, 0, 0,           0,           0,           0,           m_deltaTime, 0,           0,
+                                        0, 0, 0, 0, 0, 1, 0, 0,           0,           0,           0,           0,           m_deltaTime, 0,
+                                        0, 0, 0, 0, 0, 0, 1, 0,           0,           0,           0,           0,           0,           m_deltaTime,
+                                        0, 0, 0, 0, 0, 0, 0, 1,           0,           0,           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,           0,           1,           0,           0,           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,           0,           1,           0,           0,
+                                        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,           0,           1);
+
+    // init...
+    m_linearKalman.statePre.at<track_t>(0) = rect0.center.x;      // x
+    m_linearKalman.statePre.at<track_t>(1) = rect0.center.y;      // y
+    m_linearKalman.statePre.at<track_t>(2) = rect0.center.z;      // z
+    m_linearKalman.statePre.at<track_t>(3) = rect0.size.width;  // width
+    m_linearKalman.statePre.at<track_t>(4) = rect0.size.height; // height
+    m_linearKalman.statePre.at<track_t>(5) = rect0.size.length; // length
+    m_linearKalman.statePre.at<track_t>(6) = rect0.yaw; // yaw
+    m_linearKalman.statePre.at<track_t>(7) = rectv0.x;     // dx
+    m_linearKalman.statePre.at<track_t>(8) = rectv0.y;     // dy
+    m_linearKalman.statePre.at<track_t>(9) = 0;            // dz
+    m_linearKalman.statePre.at<track_t>(10) = 0;            // dw
+    m_linearKalman.statePre.at<track_t>(11) = 0;            // dh
+    m_linearKalman.statePre.at<track_t>(12) = 0;            // dl
+    m_linearKalman.statePre.at<track_t>(13) = 0;            // dyaw
+
+    m_linearKalman.statePost.at<track_t>(0) = rect0.center.x;
+    m_linearKalman.statePost.at<track_t>(1) = rect0.center.y;
+    m_linearKalman.statePost.at<track_t>(2) = rect0.center.z;
+    m_linearKalman.statePost.at<track_t>(3) = rect0.size.width;
+    m_linearKalman.statePost.at<track_t>(4) = rect0.size.height;
+    m_linearKalman.statePost.at<track_t>(5) = rect0.size.length;
+    m_linearKalman.statePost.at<track_t>(6) = rect0.yaw;
+    m_linearKalman.statePost.at<track_t>(7) = rectv0.x;
+    m_linearKalman.statePost.at<track_t>(8) = rectv0.y;
+    m_linearKalman.statePost.at<track_t>(9) = 0;
+    m_linearKalman.statePost.at<track_t>(10) = 0;
+    m_linearKalman.statePost.at<track_t>(11) = 0;
+    m_linearKalman.statePost.at<track_t>(12) = 0;
+    m_linearKalman.statePost.at<track_t>(13) = 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>(14, 14) <<
+                                       n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,
+                                       0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,
+                                       0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,
+                                       0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,
+                                       0,  0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,
+                                       0,  0,  0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,
+                                       0,  0,  0,  0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2,
+                                       n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,  0,  0,  0,
+                                       0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,  0,  0,
+                                       0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,  0,
+                                       0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,
+                                       0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,
+                                       0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,
+                                       0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  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(cv::Point3f xy0, cv::Point3f 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;
+}
+//---------------------------------------------------------------------------
+void TKalmanFilter::CreateLinearAcceleration3D(Rect3D rect0, cv::Point3f 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.center.x;      // x
+    m_linearKalman.statePre.at<track_t>(1) = rect0.center.y;      // y
+    m_linearKalman.statePre.at<track_t>(2) = rect0.size.width;  // width
+    m_linearKalman.statePre.at<track_t>(3) = rect0.size.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.center.x;
+    m_linearKalman.statePost.at<track_t>(1) = rect0.center.y;
+    m_linearKalman.statePost.at<track_t>(2) = rect0.size.width;
+    m_linearKalman.statePost.at<track_t>(3) = rect0.size.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;
+}
+
+//---------------------------------------------------------------------------
+cv::Point3f TKalmanFilter::GetPointPrediction()
+{
+    if (m_initialized)
+    {
+        cv::Mat prediction;
+
+        switch (m_type)
+        {
+        case tracking::KalmanLinear:
+            prediction = m_linearKalman.predict();
+            break;
+        }
+
+        m_lastPointResult = cv::Point3f(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2));
+    }
+    else
+    {
+
+    }
+    return m_lastPointResult;
+}
+
+//---------------------------------------------------------------------------
+cv::Point3f TKalmanFilter::Update(cv::Point3f 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
+            cv::Point3f xy0(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by, m_lastPointResult.z);
+            cv::Point3f xyv0(kx, ky,0);
+
+            switch (m_type)
+            {
+            case tracking::KalmanLinear:
+                if (m_useAcceleration)
+					CreateLinearAcceleration(xy0, xyv0);
+				else
+					CreateLinear(xy0, xyv0);
+                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;
+        }
+        }
+
+        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;
+        }
+
+        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;
+            }
+        }
+    }
+
+    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;
+        }
+        }
+    }
+    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));
+}
+//---------------------------------------------------------------------------
+Rect3D TKalmanFilter::GetRect3DPrediction()
+{
+    if (m_initialized)
+    {
+        cv::Mat prediction;
+
+        switch (m_type)
+        {
+        case tracking::KalmanLinear:
+            prediction = m_linearKalman.predict();
+            break;
+        }
+
+        m_lastRect3DResult = Rect3D(cv::Point3f(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2)), Size3D(prediction.at<track_t>(3), prediction.at<track_t>(4), prediction.at<track_t>(5)), prediction.at<track_t>(6));
+    }
+    else
+    {
+
+    }
+    return m_lastRect3DResult;
+}
+
+//---------------------------------------------------------------------------
+Rect3D TKalmanFilter::Update(Rect3D rect, bool dataCorrect)
+{
+    if (!m_initialized)
+    {
+        if (m_initialRect3Ds.size() < MIN_INIT_VALS)
+        {
+            if (dataCorrect)
+            {
+                m_initialRect3Ds.push_back(rect);
+                m_lastRect3DResult.center.x = static_cast<track_t>(rect.center.x);
+                m_lastRect3DResult.center.y = static_cast<track_t>(rect.center.y);
+                m_lastRect3DResult.center.z = static_cast<track_t>(rect.center.z);
+                m_lastRect3DResult.size.width = static_cast<track_t>(rect.size.width);
+                m_lastRect3DResult.size.height = static_cast<track_t>(rect.size.height);
+                m_lastRect3DResult.size.length = static_cast<track_t>(rect.size.length);
+                m_lastRect3DResult.yaw = static_cast<track_t>(rect.yaw);
+            }
+        }
+        if (m_initialRect3Ds.size() == MIN_INIT_VALS)
+        {
+            std::vector<Point_t> initialPoints;
+            cv::Point3f averageSize(0, 0, 0);
+            float averageZ = 0;
+            float averageYaw = 0;
+            for (const auto& r : m_initialRect3Ds)
+            {
+                initialPoints.emplace_back(static_cast<track_t>(r.center.x), static_cast<track_t>(r.center.y));
+                averageZ += r.center.z;
+                averageSize.x += r.size.width;
+                averageSize.y += r.size.height;
+                averageSize.z += r.size.length;
+                averageYaw += r.yaw;
+            }
+            averageZ /= MIN_INIT_VALS;
+            averageSize.x /= MIN_INIT_VALS;
+            averageSize.y /= MIN_INIT_VALS;
+            averageSize.z /= MIN_INIT_VALS;
+            averageYaw /= 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);
+            Rect3D rect0(cv::Point3f(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by, averageZ), Size3D(averageSize.x, averageSize.y,averageSize.z), averageYaw);
+            cv::Point3f rectv0(kx, ky, 0);
+
+            switch (m_type)
+            {
+            case tracking::KalmanLinear:
+				if (m_useAcceleration)
+                    CreateLinearAcceleration3D(rect0, rectv0);
+				else
+                    CreateLinear3D(rect0, rectv0);
+                break;
+            }
+        }
+    }
+
+    if (m_initialized)
+    {
+        cv::Mat measurement(7, 1, Mat_t(1));
+        if (!dataCorrect)
+        {
+            measurement.at<track_t>(0) = m_lastRect3DResult.center.x;  // update using prediction
+            measurement.at<track_t>(1) = m_lastRect3DResult.center.y;
+            measurement.at<track_t>(2) = m_lastRect3DResult.center.z;
+            measurement.at<track_t>(3) = m_lastRect3DResult.size.width;
+            measurement.at<track_t>(4) = m_lastRect3DResult.size.height;
+            measurement.at<track_t>(5) = m_lastRect3DResult.size.length;
+            measurement.at<track_t>(6) = m_lastRect3DResult.yaw;
+        }
+        else
+        {
+            measurement.at<track_t>(0) = static_cast<track_t>(rect.center.x);  // update using measurements
+            measurement.at<track_t>(1) = static_cast<track_t>(rect.center.y);
+            measurement.at<track_t>(2) = static_cast<track_t>(rect.center.z);
+            measurement.at<track_t>(3) = static_cast<track_t>(rect.size.width);
+            measurement.at<track_t>(4) = static_cast<track_t>(rect.size.height);
+            measurement.at<track_t>(5) = static_cast<track_t>(rect.size.length);
+            measurement.at<track_t>(6) = static_cast<track_t>(rect.yaw);
+        }
+        // Correction
+        cv::Mat estimated;
+        switch (m_type)
+        {
+        case tracking::KalmanLinear:
+        {
+            estimated = m_linearKalman.correct(measurement);
+
+            m_lastRect3DResult.center.x = estimated.at<track_t>(0);   //update using measurements
+            m_lastRect3DResult.center.y = estimated.at<track_t>(1);
+            m_lastRect3DResult.center.z = estimated.at<track_t>(2);
+            m_lastRect3DResult.size.width = estimated.at<track_t>(3);
+            m_lastRect3DResult.size.height = estimated.at<track_t>(4);
+            m_lastRect3DResult.size.length = estimated.at<track_t>(5);
+            m_lastRect3DResult.yaw = estimated.at<track_t>(6);
+
+            // Inertia correction
+			if (!m_useAcceleration)
+			{
+                track_t currDist = sqrtf(sqr(estimated.at<track_t>(0) - rect.center.x) + sqr(estimated.at<track_t>(1) - rect.center.y)+ sqr(estimated.at<track_t>(2) - rect.center.z) + sqr(estimated.at<track_t>(3) - rect.size.width) + sqr(estimated.at<track_t>(4) - rect.size.height) + sqr(estimated.at<track_t>(5) - rect.size.length) + sqr(estimated.at<track_t>(6) - rect.yaw));
+				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, 7) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(1, 8) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(2, 9) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(3, 10) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(4, 11) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(5, 12) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(6, 13) = m_deltaTime;
+			}
+            break;
+        }
+        }
+    }
+    else
+    {
+        if (dataCorrect)
+        {
+            m_lastRect3DResult.center.x = static_cast<track_t>(rect.center.x);
+            m_lastRect3DResult.center.y = static_cast<track_t>(rect.center.y);
+            m_lastRect3DResult.center.z = static_cast<track_t>(rect.center.z);
+            m_lastRect3DResult.size.width = static_cast<track_t>(rect.size.width);
+            m_lastRect3DResult.size.height = static_cast<track_t>(rect.size.height);
+            m_lastRect3DResult.size.length = static_cast<track_t>(rect.size.length);
+            m_lastRect3DResult.yaw = static_cast<track_t>(rect.yaw);
+        }
+    }
+    return m_lastRect3DResult;
+}
+
+//---------------------------------------------------------------------------
+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 > 5)
+                {
+                    indX = 4;
+                    indY = 5;
+                    if (m_linearKalman.statePre.rows > 8)
+                    {
+                        indX = 7;
+                        indY = 8;
+                    }
+                    res[0] = m_linearKalman.statePre.at<track_t>(indX);
+                    res[1] = m_linearKalman.statePre.at<track_t>(indY);
+                }
+            }
+            break;
+        }
+
+        }
+    }
+    return res;
+}

src/detection/detection_lidar_PointPillars_MultiHead_1025/Tracker/Kalman.h

@@ -0,0 +1,120 @@
+#pragma once
+#include "defines.h"
+#include <memory>
+#include <deque>
+
+#include <opencv2/opencv.hpp>
+
+///
+/// \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;
+
+    cv::Point3f GetPointPrediction();
+    cv::Point3f Update(cv::Point3f pt, bool dataCorrect);
+
+    cv::Rect GetRectPrediction();
+    cv::Rect Update(cv::Rect rect, bool dataCorrect);
+
+    Rect3D GetRect3DPrediction();
+    Rect3D Update(Rect3D rect, bool dataCorrect);
+
+	cv::Vec<track_t, 2> GetVelocity() const;
+
+private:
+    tracking::KalmanType m_type = tracking::KalmanLinear;
+    cv::KalmanFilter m_linearKalman;
+    static const size_t MIN_INIT_VALS = 4;
+    std::deque<cv::Point3f> m_initialPoints;
+    cv::Point3f m_lastPointResult;
+
+    std::deque<cv::Rect> m_initialRects;
+    cv::Rect_<track_t> m_lastRectResult;
+    cv::Rect_<track_t> m_lastRect;
+
+    std::deque<Rect3D> m_initialRect3Ds;
+    Rect3D m_lastRect3DResult;
+    cv::Rect_<track_t> m_lastRect3D;
+
+    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(cv::Point3f xy0, cv::Point3f xyv0);
+    void CreateLinear(cv::Rect_<track_t> rect0, Point_t rectv0);
+    void CreateLinear3D(Rect3D rect0, cv::Point3f rectv0);
+
+	// Constant acceleration model
+	// https://www.mathworks.com/help/driving/ug/linear-kalman-filters.html
+    void CreateLinearAcceleration(cv::Point3f xy0, cv::Point3f xyv0);
+    void CreateLinearAcceleration(cv::Rect_<track_t> rect0, Point_t rectv0);
+    void CreateLinearAcceleration3D(Rect3D rect0, cv::Point3f rectv0);
+};
+
+//---------------------------------------------------------------------------
+///
+/// \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/detection/detection_lidar_PointPillars_MultiHead_1025/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/detection/detection_lidar_PointPillars_MultiHead_1025/Tracker/Tracking.hpp

@@ -0,0 +1,159 @@
+#pragma once
+#include "objectarray.pb.h"
+#include "Ctracker.h"
+#include <iostream>
+#include <vector>
+#define FPS 8 
+#define MIN_DETECTEDFRAMES 5
+///
+/// \brief 跟踪器参数设置
+///
+bool InitTracker(CTracker& tracker)
+{
+    TrackerSettings settings;
+    settings.SetDistance(tracking::DistRect3Ds); // 代价矩阵：两中心点之间的距离
+    settings.m_kalmanType = tracking::KalmanLinear; // 滤波器类型：卡尔曼线性滤波器
+    settings.m_filterGoal = tracking::FilterRect3D; // 滤波对象：Rect3D
+    settings.m_lostTrackType = tracking::TrackNone; // 丢失了的目标，不再追踪
+    settings.m_matchType = tracking::MatchHungrian; // 匹配算法：匈牙利
+    settings.m_dt = 1.f; // 卡尔曼滤波器的时间步长
+    settings.m_accelNoiseMag = 0.5f; // 卡尔曼的噪声放大器
+    settings.m_distThres = 30.f; // 匹配算法中的距离阈值
+    settings.m_minAreaRadiusPix = -1.f;//frame.rows / 20.f; // 目标的最小面积半径（像素）
+    settings.m_maximumAllowedSkippedFrames = 3; // 被跟踪目标允许未匹配到的最大次数，当超过这一数值，该目标的跟踪器将被移除
+    settings.m_maxTraceLength = 5; // 最大跟踪长度，即历史轨迹保留的最大长度
+
+    tracker.setSettings(settings);
+
+    return true;
+}
+
+///
+/// \brief 对融合后的目标进行跟踪，并以跟踪后的最优估计值更新融合目标的状态信息
+///
+iv::lidar::objectarray Tracking(iv::lidar::objectarray& lidarobjvec, CTracker& tracker)
+{
+#ifdef DEBUG_SHOW
+    std::cout<<"-------------------------------------------------"<<std::endl;
+#endif
+    iv::lidar::objectarray trackedobjvec;
+    trackedobjvec.clear_obj();
+    trackedobjvec.set_timestamp(lidarobjvec.timestamp());
+    regions_t regions;
+    cv::Point3f pointXYZ;
+    for(int i = 0;i<lidarobjvec.obj_size();i++)
+    {
+        pointXYZ.x = lidarobjvec.obj(i).centroid().x();
+        pointXYZ.y = lidarobjvec.obj(i).centroid().y();
+        pointXYZ.z = lidarobjvec.obj(i).centroid().z();
+        Rect3D rect;
+        rect.center = pointXYZ;
+        rect.size.width = lidarobjvec.obj(i).dimensions().x();//w
+        rect.size.height = lidarobjvec.obj(i).dimensions().y();//l
+        rect.size.length = lidarobjvec.obj(i).dimensions().z();//h
+        rect.yaw = lidarobjvec.obj(i).tyaw();
+        CRegion region = CRegion(rect,lidarobjvec.obj(i).mntype(),lidarobjvec.obj(i).score());
+        regions.push_back(region);
+#ifdef DEBUG_SHOW
+        std::cout<<"old id:"<<i<<std::endl;
+        std::cout<<"old type:"<<lidarobjvec.obj(i).mntype()<<std::endl;
+        std::cout<<"old x,y,z,w,h,l,yaw:"<<rect.center.x<<","<<rect.center.y<<","<<rect.center.z<<"  "<<rect.size.width<<","<<rect.size.height<<","<<rect.size.length<<"  "<<rect.yaw<<std::endl;
+#endif
+
+    }
+    tracker.Update(regions, cv::UMat(), 30);
+    auto tracks = tracker.GetTracks();
+#ifdef DEBUG_SHOW
+    std::cout<<"detect size, tracker size:"<<regions.size()<<","<<tracks.size()<<std::endl;
+#endif
+    for (size_t i = 0; i < tracks.size(); i++)
+    {
+        const auto& track = tracks[i];
+        if(track.m_detectedFrames < MIN_DETECTEDFRAMES) continue;
+        int obj_id = track.m_regionID;
+        iv::lidar::lidarobject lidar_object;
+        if(obj_id != -1) // 当前融合目标成功匹配上跟踪器中的已有目标，则根据跟踪所得的最优估计值更新融合目标状态
+        {
+            lidar_object = lidarobjvec.obj(obj_id);
+            lidar_object.set_id(track.m_ID);
+
+            iv::lidar::PointXYZ centroid;
+            iv::lidar::PointXYZ *centerpoint;
+            centroid.set_x(track.m_rect.center.x);
+            centroid.set_y(track.m_rect.center.y);
+            centroid.set_z(track.m_rect.center.z);
+            centerpoint=lidar_object.mutable_centroid();
+            centerpoint->CopyFrom(centroid);
+/* not update */
+//            iv::lidar::Dimension dimension;
+//            iv::lidar::Dimension *obj_dimension;
+//            dimension.set_x(track.m_rect.size.width);
+//            dimension.set_y(track.m_rect.size.height);
+//            dimension.set_z(track.m_rect.size.length);
+//            obj_dimension=lidar_object.mutable_dimensions();
+//            obj_dimension->CopyFrom(dimension);
+
+//            lidar_object.set_tyaw(track.m_rect.yaw);
+
+            iv::lidar::VelXY vel_relative;
+            iv::lidar::VelXY *velrelative;
+            vel_relative.set_x(track.m_velocity[0]*FPS);
+            vel_relative.set_y(track.m_velocity[1]*FPS);
+            velrelative = lidar_object.mutable_vel_relative();
+            velrelative->CopyFrom(vel_relative);
+
+            iv::lidar::lidarobject *pe = trackedobjvec.add_obj();
+            pe->CopyFrom(lidar_object);
+        }else{ // 当前时刻没有融合目标与跟踪器中的已有目标匹配上，则将跟踪器中已有目标的预测结果增加到融合结果中            
+            lidar_object.set_id(track.m_ID);
+            lidar_object.set_mntype(track.m_region.m_type);
+            lidar_object.set_score(track.m_region.m_confidence);
+
+            iv::lidar::PointXYZ centroid;
+            iv::lidar::PointXYZ *centerpoint;
+            centroid.set_x(track.m_rect.center.x);
+            centroid.set_y(track.m_rect.center.y);
+            centroid.set_z(track.m_rect.center.z);
+            centerpoint=lidar_object.mutable_centroid();
+            centerpoint->CopyFrom(centroid);
+
+            iv::lidar::Dimension dimension;
+            iv::lidar::Dimension *obj_dimension;
+            dimension.set_x(track.m_region.m_rect.size.width);
+            dimension.set_y(track.m_region.m_rect.size.height);
+            dimension.set_z(track.m_region.m_rect.size.length);
+            obj_dimension=lidar_object.mutable_dimensions();
+            obj_dimension->CopyFrom(dimension);
+
+            lidar_object.set_tyaw(track.m_region.m_rect.yaw);
+
+            iv::lidar::VelXY vel_relative;
+            iv::lidar::VelXY *velrelative;
+            vel_relative.set_x(track.m_velocity[0]*FPS);
+            vel_relative.set_y(track.m_velocity[1]*FPS);
+            velrelative = lidar_object.mutable_vel_relative();
+            velrelative->CopyFrom(vel_relative);
+
+            iv::lidar::lidarobject *pe = trackedobjvec.add_obj();
+            pe->CopyFrom(lidar_object);
+        }
+#ifdef DEBUG_SHOW
+        std::cout<<"id:"<<lidar_object.id()<<"  "<<obj_id<<std::endl;
+        std::cout<<"type:"<<lidar_object.mntype()<<std::endl;
+        std::cout<<"update x,y,z,w,h,l,yaw,vx,vy:"<<lidar_object.centroid().x()<<","<<lidar_object.centroid().y()<<","<<lidar_object.centroid().z()<<"  "<<lidar_object.dimensions().x()<<","<<lidar_object.dimensions().y()<<","<<lidar_object.dimensions().z()<<"  "<<lidar_object.tyaw()<<"  "<<lidar_object.vel_relative().x()<<","<<lidar_object.vel_relative().y()<<std::endl;
+#endif
+    }
+#ifdef DEBUG_SHOW
+    std::cout<<"trackedobjvec size:"<<trackedobjvec.obj_size()<<std::endl;
+#endif
+    //    for (size_t i = 0; i < trackedobjvec.obj_size(); i++)
+    //    {
+    //        iv::lidar::lidarobject lidar_object;
+    //        lidar_object = trackedobjvec.obj(i);
+    //        std::cout<<"historical size:"<<lidar_object.point_historical_size()<<std::endl;
+    //    }
+    regions.clear();
+    tracks.clear();
+    return trackedobjvec;
+}
+

src/detection/detection_lidar_PointPillars_MultiHead_1025/Tracker/defines.h

@@ -0,0 +1,147 @@
+#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;
+typedef struct Size3D{
+    Size3D()
+    {
+    }
+    Size3D(float w, float h, float l)
+        :
+          width(w),
+          height(h),
+          length(l)
+    {
+    }
+    float length;
+    float width;
+    float height;
+} Size3D;
+typedef struct Rect3D{
+    Rect3D()
+    {
+    }
+    Rect3D(cv::Point3f c, Size3D s, float y)
+        :
+          center(c),
+          size(s),
+          yaw(y)
+    {
+    }
+    cv::Point3f center;
+    Size3D size;
+    float yaw;
+} Rect3D;
+///
+/// \brief config_t
+///
+typedef std::multimap<std::string, std::string> config_t;
+
+///
+/// \brief The CRegion class
+///
+class CRegion
+{
+public:
+    CRegion()
+        : m_type(-1), m_type_name(""), m_confidence(-1)
+    {
+    }
+
+    CRegion(const Rect3D& rect, const int& type, float confidence)
+        : m_rect(rect), m_type(type), m_confidence(confidence)
+    {
+        RBRect();
+    }
+
+    CRegion(const Rect3D& rect, const std::string& type, float confidence)
+        : m_rect(rect), m_type_name(type), m_confidence(confidence)
+    {
+        RBRect();
+    }
+    Rect3D m_rect;
+    cv::RotatedRect m_rrect;
+    cv::Rect m_brect;
+    int m_type = -1;
+    std::string m_type_name = "";
+    float m_confidence = -1;
+private:
+    ///
+    /// \brief B2RRect
+    /// \return
+    ///
+    cv::RotatedRect RBRect()
+    {
+        m_rrect = cv::RotatedRect(Point_t(m_rect.center.x,m_rect.center.y), cv::Size(m_rect.size.width,m_rect.size.height), 0);
+        m_brect = cv::Rect(m_rect.center.x-m_rect.size.width/2,m_rect.center.y-m_rect.size.height/2, m_rect.size.width,m_rect.size.height);
+        return m_rrect;
+    }
+};
+
+typedef std::vector<CRegion> regions_t;
+
+///
+///
+///
+namespace tracking
+{
+
+///
+/// \brief The DistType enum
+///
+enum DistType
+{
+    DistCenters,   // Euclidean distance between centers, pixels
+    DistRects,     // Euclidean distance between bounding rectangles, pixels
+    DistRect3Ds,     // Euclidean distance between bounding rectangles, pixels
+	DistsCount
+};
+
+///
+/// \brief The FilterGoal enum
+///
+enum FilterGoal
+{
+    FilterCenter, // x,y
+    FilterRect,   // x,y,w,h
+    FilterRect3D  // x,y,z,w,h,l,yaw
+};
+
+///
+/// \brief The KalmanType enum
+///
+enum KalmanType
+{
+    KalmanLinear
+};
+
+///
+/// \brief The MatchType enum
+///
+enum MatchType
+{
+    MatchHungrian
+};
+
+///
+/// \brief The LostTrackType enum
+///
+enum LostTrackType
+{
+    TrackNone
+};
+}

src/detection/detection_lidar_PointPillars_MultiHead_1025/Tracker/track.cpp

@@ -0,0 +1,492 @@
+#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,
+        tracking::FilterGoal filterGoal,
+        tracking::LostTrackType externalTrackerForLost
+        )
+    :
+      m_trackID(trackID),
+      m_regionID(regionID),
+      m_skippedFrames(0),
+      m_lastRegion(region),
+      m_predictionPoint(region.m_rect.center),
+      m_predictionRect(region.m_rrect),
+      m_predictionRect3D(region.m_rect),
+      m_kalman(kalmanType, useAcceleration, deltaTime, accelNoiseMag),
+      m_filterGoal(filterGoal),
+      m_outOfTheFrame(false),
+      m_externalTrackerForLost(externalTrackerForLost)
+{
+    if (filterGoal == tracking::FilterRect)
+        m_kalman.Update(region.m_brect, true);
+    else if(filterGoal == tracking::FilterRect3D)
+        m_kalman.Update(region.m_rect, 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
+{
+    cv::Point3f diff = m_predictionPoint - reg.m_rect.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::CalcDistRect3D
+/// \param reg
+/// \return
+///
+track_t CTrack::CalcDistRect3D(const CRegion& reg) const
+{
+    std::array<track_t, 7> diff;
+    diff[0] = reg.m_rect.center.x - m_lastRegion.m_rect.center.x;
+    diff[1] = reg.m_rect.center.y - m_lastRegion.m_rect.center.y;
+    diff[2] = 0;//reg.m_rect.center.z - m_lastRegion.m_rect.center.z;
+    diff[3] = static_cast<track_t>(m_lastRegion.m_rect.size.width - reg.m_rect.size.width);
+    diff[4] = static_cast<track_t>(m_lastRegion.m_rect.size.height - reg.m_rect.size.height);
+    diff[5] = static_cast<track_t>(m_lastRegion.m_rect.size.length - reg.m_rect.size.length);
+    diff[6] = 0;//static_cast<track_t>(m_lastRegion.m_rect.yaw - reg.m_rect.yaw);
+
+    track_t dist = 0;
+    for (size_t i = 0; i < diff.size(); ++i)
+    {
+        dist += sqr(diff[i]);
+    }
+    return sqrtf(dist);
+}
+
+///
+/// \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_filterGoal == tracking::FilterRect) // Kalman filter for object coordinates and size
+        RectUpdate(region, dataCorrect, prevFrame, currFrame);
+    if (m_filterGoal == tracking::FilterRect3D) // Kalman filter for object coordinates and size
+        Rect3DUpdate(region, dataCorrect, prevFrame, currFrame);
+    else // Kalman filter only for object center
+        PointUpdate(region.m_rect.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_rect.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, fabs(d.x)), std::max(minRadius.height, 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_rect.size.width < reg.m_rect.size.width)
+        return m_lastRegion.m_rect.size.width / reg.m_rect.size.width;
+    else
+        return reg.m_rect.size.width / m_lastRegion.m_rect.size.width;
+}
+
+///
+/// \brief CTrack::HeightDist
+/// \param reg
+/// \return
+///
+track_t CTrack::HeightDist(const CRegion& reg) const
+{
+    if (m_lastRegion.m_rect.size.height < reg.m_rect.size.height)
+        return m_lastRegion.m_rect.size.height / reg.m_rect.size.height;
+    else
+        return reg.m_rect.size.height / m_lastRegion.m_rect.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_rect;
+            }
+
+            ++m_staticFrames;
+            m_isStatic = true;
+        }
+        else
+        {
+            m_isStatic = false;
+            m_staticFrames = 0;
+            m_staticFrame = cv::UMat();
+        }
+    }
+
+    return m_isStatic;
+}
+
+///
+/// \brief GetLastRect
+/// \return
+///
+Rect3D CTrack::GetLastRect() const
+{
+    if (m_filterGoal == tracking::FilterRect)
+        return Rect3D(cv::Point3f(m_predictionRect.center.x,m_predictionRect.center.y,0),Size3D(m_predictionRect.boundingRect().width,m_predictionRect.boundingRect().height,0),0);
+    else if(m_filterGoal == tracking::FilterRect3D)
+        return m_predictionRect3D;
+    else
+        return Rect3D(cv::Point3f(m_predictionPoint.x, m_predictionPoint.y,0),Size3D(0,0,0),0);
+
+}
+///
+/// \brief CTrack::LastRegion
+/// \return
+///
+const CRegion& CTrack::LastRegion() const
+{
+    return m_lastRegion;
+}
+
+///
+/// \brief CTrack::ConstructObject
+/// \return
+///
+TrackingObject CTrack::ConstructObject() const
+{
+    return TrackingObject(GetLastRect(), m_lastRegion, m_trackID, m_regionID, m_trace, IsStatic(), IsOutOfTheFrame(), m_kalman.GetVelocity(), m_detectedFrames);
+}
+
+///
+/// \brief CTrack::SkippedFrames
+/// \return
+///
+size_t CTrack::SkippedFrames() const
+{
+    return m_skippedFrames;
+}
+
+///
+/// \brief CTrack::SkippedFrames
+/// \return
+///
+size_t& CTrack::SkippedFrames()
+{
+    return m_skippedFrames;
+}
+///
+/// \brief CTrack::DetectedFrames
+/// \return
+///
+size_t& CTrack::DetectedFrames()
+{
+    return m_detectedFrames;
+}
+///
+/// \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;
+    }
+
+    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);
+
+    m_outOfTheFrame = (dx != 0) || (dy != 0) || (brect.width < 2) || (brect.height < 2);
+
+    m_predictionPoint = cv::Point3f(m_predictionRect.center.x,m_predictionRect.center.y,0);
+
+    //std::cout << "brect = " << brect << ", dx = " << dx << ", dy = " << dy << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
+}
+///
+/// \brief Rect3DUpdate
+/// \param region
+/// \param dataCorrect
+/// \param prevFrame
+/// \param currFrame
+///
+void CTrack::Rect3DUpdate(
+        const CRegion& region,
+        bool dataCorrect,
+        cv::UMat prevFrame,
+        cv::UMat currFrame
+        )
+{
+    m_kalman.GetRect3DPrediction();
+
+    switch (m_externalTrackerForLost)
+    {
+    case tracking::TrackNone:
+        break;
+    }
+
+    m_predictionRect3D = m_kalman.Update(region.m_rect, dataCorrect);
+
+    m_predictionPoint = m_predictionRect3D.center;
+
+    //std::cout << "brect = " << brect << ", dx = " << dx << ", dy = " << dy << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
+}
+
+///
+/// \brief PointUpdate
+/// \param pt
+/// \param dataCorrect
+///
+void CTrack::PointUpdate(
+        const cv::Point3f& 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/detection/detection_lidar_PointPillars_MultiHead_1025/Tracker/track.h

@@ -0,0 +1,303 @@
+#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"
+
+// --------------------------------------------------------------------------
+///
+/// \brief The TrajectoryPoint struct
+///
+struct TrajectoryPoint
+{
+    ///
+    /// \brief TrajectoryPoint
+    ///
+    TrajectoryPoint()
+        : m_hasRaw(false)
+    {
+    }
+
+    ///
+    /// \brief TrajectoryPoint
+    /// \param prediction
+    ///
+    TrajectoryPoint(const cv::Point3f& prediction)
+        :
+          m_hasRaw(false),
+          m_prediction(prediction)
+    {
+    }
+
+    ///
+    /// \brief TrajectoryPoint
+    /// \param prediction
+    /// \param raw
+    ///
+    TrajectoryPoint(const cv::Point3f& prediction, const cv::Point3f& raw)
+        :
+          m_hasRaw(true),
+          m_prediction(prediction),
+          m_raw(raw)
+    {
+    }
+
+    bool m_hasRaw = false;
+    cv::Point3f m_prediction;
+    cv::Point3f m_raw;
+};
+
+// --------------------------------------------------------------------------
+///
+/// \brief The Trace class
+///
+class Trace
+{
+public:
+    ///
+    /// \brief operator []
+    /// \param i
+    /// \return
+    ///
+    const cv::Point3f& operator[](size_t i) const
+    {
+        return m_trace[i].m_prediction;
+    }
+
+    ///
+    /// \brief operator []
+    /// \param i
+    /// \return
+    ///
+    cv::Point3f& 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 cv::Point3f& prediction)
+    {
+        m_trace.emplace_back(prediction);
+    }
+    void push_back(const cv::Point3f& prediction, const cv::Point3f& 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
+{
+    Rect3D m_rect;           // Coordinates
+    CRegion m_region;         // detect region
+    Trace m_trace;                     // Trajectory
+    size_t m_ID = 0;                   // Objects ID
+    size_t m_regionID = 0;                   // Objects ID
+    bool m_isStatic = false;           // Object is abandoned
+    bool m_outOfTheFrame = false;      // Is object out of freme
+    cv::Vec<track_t, 2> m_velocity;    // pixels/sec
+    size_t m_detectedFrames = 0;       // detected frames' count
+    ///
+    TrackingObject(const Rect3D& rect, const CRegion& region, size_t ID, size_t regionID, const Trace& trace,
+                   bool isStatic, bool outOfTheFrame, cv::Vec<track_t, 2> velocity, size_t detectedFrames)
+        :
+          m_rect(rect), m_region(region), m_ID(ID), m_regionID(regionID), m_isStatic(isStatic), m_outOfTheFrame(outOfTheFrame), m_velocity(velocity), m_detectedFrames(detectedFrames)
+    {
+        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_rect.size.width / m_rect.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,
+           tracking::FilterGoal filterObjectGoal,
+           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 CalcDist
+    /// Euclidean distance in pixels between object contours on two N and N+1 frames
+    /// \param reg
+    /// \return
+    ///
+    track_t CalcDistRect3D(const CRegion& reg) 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;
+
+    Rect3D GetLastRect() const;
+
+    const cv::Point3f& AveragePoint() const;
+    cv::Point3f& AveragePoint();
+    const CRegion& LastRegion() const;
+    size_t SkippedFrames() const;
+    size_t& SkippedFrames();
+    size_t& DetectedFrames();
+
+    TrackingObject ConstructObject() const;
+public:
+    size_t m_regionID = 0;
+private:
+    Trace m_trace;
+    size_t m_trackID = 0;
+    size_t m_skippedFrames = 0;
+    size_t m_detectedFrames = 0;
+    CRegion m_lastRegion;
+
+    cv::Point3f m_predictionPoint;
+    cv::RotatedRect m_predictionRect;
+    Rect3D m_predictionRect3D;
+    TKalmanFilter m_kalman;
+    tracking::FilterGoal m_filterGoal = tracking::FilterCenter;
+    bool m_outOfTheFrame = false;
+
+    tracking::LostTrackType m_externalTrackerForLost;
+
+    void RectUpdate(const CRegion& region, bool dataCorrect, cv::UMat prevFrame, cv::UMat currFrame);
+    void Rect3DUpdate(const CRegion& region, bool dataCorrect, cv::UMat prevFrame, cv::UMat currFrame);
+    void PointUpdate(const cv::Point3f& 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;
+    Rect3D m_staticRect;
+};
+
+typedef std::vector<std::unique_ptr<CTrack>> tracks_t;
+

src/detection/detection_lidar_PointPillars_MultiHead_1025/cfgs/cbgs_pp_multihead.yaml

@@ -0,0 +1,246 @@
+CLASS_NAMES: ['car','truck', 'construction_vehicle', 'bus', 'trailer',
+              'barrier', 'motorcycle', 'bicycle', 'pedestrian', 'traffic_cone']
+
+DATA_CONFIG:
+    _BASE_CONFIG_: cfgs/dataset_configs/nuscenes_dataset.yaml
+
+    POINT_CLOUD_RANGE: [-51.2, -51.2, -5.0, 51.2, 51.2, 3.0]
+    DATA_PROCESSOR:
+        -   NAME: mask_points_and_boxes_outside_range
+            REMOVE_OUTSIDE_BOXES: True
+
+        -   NAME: shuffle_points
+            SHUFFLE_ENABLED: {
+                'train': True,
+                'test': True
+            }
+
+        -   NAME: transform_points_to_voxels
+            VOXEL_SIZE: [0.2, 0.2, 8.0]
+            MAX_POINTS_PER_VOXEL: 20
+            MAX_NUMBER_OF_VOXELS: {
+                'train': 30000,
+                'test': 30000
+            }
+
+MODEL:
+    NAME: PointPillar
+
+    VFE:
+        NAME: PillarVFE
+        WITH_DISTANCE: False
+        USE_ABSLOTE_XYZ: True
+        USE_NORM: True
+        NUM_FILTERS: [64]
+
+    MAP_TO_BEV:
+        NAME: PointPillarScatter
+        NUM_BEV_FEATURES: 64
+
+    BACKBONE_2D:
+        NAME: BaseBEVBackbone
+        LAYER_NUMS: [3, 5, 5]
+        LAYER_STRIDES: [2, 2, 2]
+        NUM_FILTERS: [64, 128, 256]
+        UPSAMPLE_STRIDES: [0.5, 1, 2]
+        NUM_UPSAMPLE_FILTERS: [128, 128, 128]
+
+    DENSE_HEAD:
+        NAME: AnchorHeadMulti
+        CLASS_AGNOSTIC: False
+
+        DIR_OFFSET: 0.78539
+        DIR_LIMIT_OFFSET: 0.0
+        NUM_DIR_BINS: 2
+
+        USE_MULTIHEAD: True
+        SEPARATE_MULTIHEAD: True
+        ANCHOR_GENERATOR_CONFIG: [
+            {
+                'class_name': car,
+                'anchor_sizes': [[4.63, 1.97, 1.74]],
+                'anchor_rotations': [0, 1.57],
+                'anchor_bottom_heights': [-0.95],
+                'align_center': False,
+                'feature_map_stride': 4,
+                'matched_threshold': 0.6,
+                'unmatched_threshold': 0.45
+            },
+            {
+                'class_name': truck,
+                'anchor_sizes': [[6.93, 2.51, 2.84]],
+                'anchor_rotations': [0, 1.57],
+                'anchor_bottom_heights': [-0.6],
+                'align_center': False,
+                'feature_map_stride': 4,
+                'matched_threshold': 0.55,
+                'unmatched_threshold': 0.4
+            },
+            {
+                'class_name': construction_vehicle,
+                'anchor_sizes': [[6.37, 2.85, 3.19]],
+                'anchor_rotations': [0, 1.57],
+                'anchor_bottom_heights': [-0.225],
+                'align_center': False,
+                'feature_map_stride': 4,
+                'matched_threshold': 0.5,
+                'unmatched_threshold': 0.35
+            },
+            {
+                'class_name': bus,
+                'anchor_sizes': [[10.5, 2.94, 3.47]],
+                'anchor_rotations': [0, 1.57],
+                'anchor_bottom_heights': [-0.085],
+                'align_center': False,
+                'feature_map_stride': 4,
+                'matched_threshold': 0.55,
+                'unmatched_threshold': 0.4
+            },
+            {
+                'class_name': trailer,
+                'anchor_sizes': [[12.29, 2.90, 3.87]],
+                'anchor_rotations': [0, 1.57],
+                'anchor_bottom_heights': [0.115],
+                'align_center': False,
+                'feature_map_stride': 4,
+                'matched_threshold': 0.5,
+                'unmatched_threshold': 0.35
+            },
+            {
+                'class_name': barrier,
+                'anchor_sizes': [[0.50, 2.53, 0.98]],
+                'anchor_rotations': [0, 1.57],
+                'anchor_bottom_heights': [-1.33],
+                'align_center': False,
+                'feature_map_stride': 4,
+                'matched_threshold': 0.55,
+                'unmatched_threshold': 0.4
+            },
+            {
+                'class_name': motorcycle,
+                'anchor_sizes': [[2.11, 0.77, 1.47]],
+                'anchor_rotations': [0, 1.57],
+                'anchor_bottom_heights': [-1.085],
+                'align_center': False,
+                'feature_map_stride': 4,
+                'matched_threshold': 0.5,
+                'unmatched_threshold': 0.3
+            },
+            {
+                'class_name': bicycle,
+                'anchor_sizes': [[1.70, 0.60, 1.28]],
+                'anchor_rotations': [0, 1.57],
+                'anchor_bottom_heights': [-1.18],
+                'align_center': False,
+                'feature_map_stride': 4,
+                'matched_threshold': 0.5,
+                'unmatched_threshold': 0.35
+            },
+            {
+                'class_name': pedestrian,
+                'anchor_sizes': [[0.73, 0.67, 1.77]],
+                'anchor_rotations': [0, 1.57],
+                'anchor_bottom_heights': [-0.935],
+                'align_center': False,
+                'feature_map_stride': 4,
+                'matched_threshold': 0.6,
+                'unmatched_threshold': 0.4
+            },
+            {
+                'class_name': traffic_cone,
+                'anchor_sizes': [[0.41, 0.41, 1.07]],
+                'anchor_rotations': [0, 1.57],
+                'anchor_bottom_heights': [-1.285],
+                'align_center': False,
+                'feature_map_stride': 4,
+                'matched_threshold': 0.6,
+                'unmatched_threshold': 0.4
+            },
+        ]
+
+        SHARED_CONV_NUM_FILTER: 64
+
+        RPN_HEAD_CFGS: [
+            {
+                'HEAD_CLS_NAME': ['car'],
+            },
+            {
+                'HEAD_CLS_NAME': ['truck', 'construction_vehicle'],
+            },
+            {
+                'HEAD_CLS_NAME': ['bus', 'trailer'],
+            },
+            {
+                'HEAD_CLS_NAME': ['barrier'],
+            },
+            {
+                'HEAD_CLS_NAME': ['motorcycle', 'bicycle'],
+            },
+            {
+                'HEAD_CLS_NAME': ['pedestrian', 'traffic_cone'],
+            },
+        ]
+        SEPARATE_REG_CONFIG: 
+            NUM_MIDDLE_CONV: 1
+            NUM_MIDDLE_FILTER: 64
+            REG_LIST: ['reg:2', 'height:1', 'size:3', 'angle:2', 'velo:2']
+
+        TARGET_ASSIGNER_CONFIG:
+            NAME: AxisAlignedTargetAssigner
+            POS_FRACTION: -1.0
+            SAMPLE_SIZE: 512
+            NORM_BY_NUM_EXAMPLES: False
+            MATCH_HEIGHT: False
+            BOX_CODER: ResidualCoder
+            BOX_CODER_CONFIG: {
+                'code_size': 9,
+                'encode_angle_by_sincos': True
+            }
+
+
+        LOSS_CONFIG:
+            REG_LOSS_TYPE: WeightedL1Loss
+            LOSS_WEIGHTS: {
+                'pos_cls_weight': 1.0,
+                'neg_cls_weight': 2.0,
+                'cls_weight': 1.0,
+                'loc_weight': 0.25,
+                'dir_weight': 0.2,
+                'code_weights': [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.2, 0.2]
+            }
+
+    POST_PROCESSING:
+        RECALL_THRESH_LIST: [0.3, 0.5, 0.7]
+        SCORE_THRESH: 0.1
+        OUTPUT_RAW_SCORE: False
+
+        EVAL_METRIC: kitti
+
+        NMS_CONFIG:
+            MULTI_CLASSES_NMS: True
+            NMS_TYPE: nms_gpu
+            NMS_THRESH: 0.2
+            NMS_PRE_MAXSIZE: 1000
+            NMS_POST_MAXSIZE: 83
+
+
+OPTIMIZATION:
+    BATCH_SIZE_PER_GPU: 4
+    NUM_EPOCHS: 20
+
+    OPTIMIZER: adam_onecycle
+    LR: 0.001
+    WEIGHT_DECAY: 0.01
+    MOMENTUM: 0.9
+
+    MOMS: [0.95, 0.85]
+    PCT_START: 0.4
+    DIV_FACTOR: 10
+    DECAY_STEP_LIST: [35, 45]
+    LR_DECAY: 0.1
+    LR_CLIP: 0.0000001
+
+    LR_WARMUP: False
+    WARMUP_EPOCH: 1
+
+    GRAD_NORM_CLIP: 10

src/detection/detection_lidar_PointPillars_MultiHead_1025/common.h

@@ -0,0 +1,141 @@
+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @author Kosuke Murakami
+ * @date 2019/02/26
+ */
+
+/**
+* @author Yan haixu
+* Contact: just github.com/hova88
+* @date 2021/04/30
+*/
+
+
+
+#pragma once
+
+// headers in STL
+#include <stdio.h>
+#include <assert.h>
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <vector>
+// headers in CUDA
+#include "cuda_runtime_api.h"
+
+using namespace std;
+// using MACRO to allocate memory inside CUDA kernel
+#define NUM_3D_BOX_CORNERS_MACRO 8
+
+#define NUM_2D_BOX_CORNERS_MACRO 4
+
+#define NUM_THREADS_MACRO 64
+
+// need to be changed when num_threads_ is changed
+
+#define DIVUP(m, n) ((m) / (n) + ((m) % (n) > 0))
+
+#define GPU_CHECK(ans)                    \
+  {                                       \
+    GPUAssert((ans), __FILE__, __LINE__); \
+  }
+inline void GPUAssert(cudaError_t code, const char *file, int line,
+                      bool abort = true)
+{
+  if (code != cudaSuccess)
+  {
+    fprintf(stderr, "GPUassert: %s %s %d\n", cudaGetErrorString(code), file,
+            line);
+    if (abort)
+      exit(code);
+  }
+};
+
+template <typename T>
+void HOST_SAVE(T *array, int size, string filename, string root = "../test/result", string postfix = ".txt")
+{
+  string filepath = root + "/" + filename + postfix;
+  if (postfix == ".bin")
+  {
+    fstream file(filepath, ios::out | ios::binary);
+    file.write(reinterpret_cast<char *>(array), sizeof(size * sizeof(T)));
+    file.close();
+    std::cout << "|>>>|  Data has been written in " << filepath << "  |<<<|" << std::endl;
+    return;
+  }
+  else if (postfix == ".txt")
+  {
+    ofstream file(filepath, ios::out);
+    for (int i = 0; i < size; ++i)
+      file << array[i] << " ";
+    file.close();
+    std::cout << "|>>>|  Data has been written in " << filepath << "  |<<<|" << std::endl;
+    return;
+  }
+};
+
+template <typename T>
+void DEVICE_SAVE(T *array, int size, string filename, string root = "../test/result", string postfix = ".txt")
+{
+  T *temp_ = new T[size];
+  cudaMemcpy(temp_, array, size * sizeof(T), cudaMemcpyDeviceToHost);
+  HOST_SAVE<T>(temp_, size, filename, root, postfix);
+  delete[] temp_;
+};
+
+
+// int TXTtoArrary( float* &points_array , string file_name , int num_feature = 4)
+// {
+//   ifstream InFile;
+//   InFile.open(file_name.data());
+//   assert(InFile.is_open());
+
+//   vector<float> temp_points;
+//   string c;
+
+//   while (!InFile.eof())
+//   {
+//       InFile >> c;
+
+//       temp_points.push_back(atof(c.c_str()));
+//   }
+//   points_array = new float[temp_points.size()];
+//   for (int i = 0 ; i < temp_points.size() ; ++i) {
+//     points_array[i] = temp_points[i];
+//   }
+
+//   InFile.close();  
+//   return temp_points.size() / num_feature;
+// };

src/detection/detection_lidar_PointPillars_MultiHead_1025/detection_lidar_PointPillars_MultiHead.pro

@@ -0,0 +1,181 @@
+QT -= gui
+
+CONFIG += c++14 console
+CONFIG -= app_bundle
+
+QMAKE_CXXFLAGS += -std=gnu++17
+QMAKE_LFLAGS += -no-pie  -Wl,--no-as-needed
+
+# 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
+#DEFINES += DEBUG_SHOW
+# You can also make your code fail to compile if you use 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 += main.cpp \
+    pointpillars.cc \
+    ../../include/msgtype/object.pb.cc \
+    ../../include/msgtype/objectarray.pb.cc \
+    Tracker/Ctracker.cpp \
+    Tracker/HungarianAlg.cpp \
+    Tracker/Kalman.cpp \
+    Tracker/track.cpp
+
+DISTFILES += \
+    nms.cu \
+    postprocess.cu \
+    preprocess.cu \
+    scatter.cu
+
+HEADERS += \
+    common.h \
+    nms.h \
+    pointpillars.h \
+    postprocess.h \
+    preprocess.h \
+    scatter.h \
+    ../../include/msgtype/object.pb.h \
+    ../../include/msgtype/objectarray.pb.h \
+    Tracker/Ctracker.h \
+    Tracker/defines.h \
+    Tracker/HungarianAlg.h \
+    Tracker/Kalman.h \
+    Tracker/ShortPathCalculator.h \
+    Tracker/track.h \
+    Tracker/Tracking.hpp
+
+INCLUDEPATH+=Tracker
+
+CUDA_SOURCES +=  \
+    nms.cu \
+    postprocess.cu \
+    preprocess.cu \
+    scatter.cu
+
+CUDA_SDK = "/usr/local/cuda/"   # cudaSDK路径
+
+CUDA_DIR = "/usr/local/cuda/"            # CUDA tookit路径
+
+SYSTEM_NAME = linux         # 自己系统环境 'Win32', 'x64', or 'Win64'
+
+SYSTEM_TYPE = 64           #操作系统位数 '32' or '64',
+
+CUDA_ARCH = sm_72         # cuda架构, for example 'compute_10', 'compute_11', 'sm_10'
+
+NVCC_OPTIONS = --use_fast_math --compiler-options "-fPIC"
+
+
+# include paths
+
+INCLUDEPATH += $$CUDA_DIR/include
+#INCLUDEPATH += /usr/local/cuda-10.0/targets/aarch64-linux/include/crt
+
+# library directories
+
+QMAKE_LIBDIR += $$CUDA_DIR/lib/
+
+CUDA_OBJECTS_DIR = ./
+
+# The following library conflicts with something in Cuda
+
+#QMAKE_LFLAGS_RELEASE = /NODEFAULTLIB:msvcrt.lib
+
+#QMAKE_LFLAGS_DEBUG   = /NODEFAULTLIB:msvcrtd.lib
+
+# Add the necessary libraries
+
+CUDA_LIBS =  cudart cufft
+
+# The following makes sure all path names (which often include spaces) are put between quotation marks
+
+CUDA_INC = $$join(INCLUDEPATH,'" -I"','-I"','"')
+
+NVCC_LIBS = $$join(CUDA_LIBS,' -l','-l', '')
+
+#LIBS += $$join(CUDA_LIBS,'.so ', '', '.so')
+
+# Configuration of the Cuda compiler
+
+CONFIG(debug, debug|release) {
+
+    # Debug mode
+
+    cuda_d.input = CUDA_SOURCES
+
+    cuda_d.output = $$CUDA_OBJECTS_DIR/${QMAKE_FILE_BASE}_cuda.o
+
+    cuda_d.commands = $$CUDA_DIR/bin/nvcc -D_DEBUG $$NVCC_OPTIONS $$CUDA_INC $$NVCC_LIBS --machine $$SYSTEM_TYPE -arch=$$CUDA_ARCH -c -o ${QMAKE_FILE_OUT} ${QMAKE_FILE_NAME}
+
+    cuda_d.dependency_type = TYPE_C
+
+    QMAKE_EXTRA_COMPILERS += cuda_d
+
+}
+
+else {
+
+    # Release mode
+
+    cuda.input = CUDA_SOURCES
+
+    cuda.output = $$CUDA_OBJECTS_DIR/${QMAKE_FILE_BASE}_cuda.o
+
+    cuda.commands = $$CUDA_DIR/bin/nvcc $$NVCC_OPTIONS $$CUDA_INC $$NVCC_LIBS --machine $$SYSTEM_TYPE -arch=$$CUDA_ARCH -O3 -c -o ${QMAKE_FILE_OUT} ${QMAKE_FILE_NAME}
+
+    cuda.dependency_type = TYPE_C
+
+    QMAKE_EXTRA_COMPILERS += cuda
+
+}
+
+
+LIBS += -L/usr/local/cuda-10.2/targets/aarch64-linux/lib
+
+LIBS += -lcudart -lcufft -lyaml-cpp
+
+#LIBS += -L/home/adc/soft/cudnn-10.2-linux-x64-v7.6.5.32/cuda/lib64 -lcudnn
+
+LIBS +=  -lmyelin -lnvinfer -lnvonnxparser -lnvcaffe_parser
+
+#LIBS += -L/home/nvidia/git/libtorch_gpu-1.6.0-linux-aarch64/lib -ltorch_cuda  -ltorch -lc10 -ltorch_cpu
+
+unix:INCLUDEPATH += /usr/include/eigen3
+unix:INCLUDEPATH += /usr/include/pcl-1.7
+unix:INCLUDEPATH += /usr/include/pcl-1.8
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+LIBS += -lboost_system
+
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization
+
+INCLUDEPATH += /usr/include/opencv4/
+LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so

src/detection/detection_lidar_PointPillars_MultiHead_1025/main.cpp

@@ -0,0 +1,376 @@
+#include <QCoreApplication>
+#include <QDateTime>
+#include <iostream>
+#include "pointpillars.h"
+#include <iostream>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include "xmlparam.h"
+#include "modulecomm.h"
+#include "ivfault.h"
+#include "ivlog.h"
+#include "ivexit.h"
+#include "ivversion.h"
+#include <thread>
+#include "objectarray.pb.h"
+//#include "ivbacktrace.h"
+#include "Tracking.hpp"
+iv::Ivfault *gfault = nullptr;
+iv::Ivlog *givlog = nullptr;
+
+std::thread * gpthread;
+PointPillars * pPillars = nullptr ;
+void * gpa;
+void * gpdetect;
+int gnothavedatatime = 0;
+const int kNumPointFeature = 5;
+const int kOutputNumBoxFeature = 7;
+std::string gstrinput;
+std::string gstroutput;
+
+TrackerSettings settings;
+CTracker tracker(settings);
+bool m_isTrackerInitialized = false;
+
+void PclToArray(
+        const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr,
+        float* out_points_array, const float normalizing_factor) {
+    for (size_t i = 0; i < in_pcl_pc_ptr->size(); ++i) {
+        pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
+        out_points_array[i * 4 + 0] = point.x;
+        out_points_array[i * 4 + 1] = point.y;
+        out_points_array[i * 4 + 2] = point.z;
+        out_points_array[i * 4 + 3] =
+                static_cast<float>(point.intensity / normalizing_factor);
+    }
+}
+
+void PclXYZITToArray(
+        const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr,
+        float* out_points_array, const float normalizing_factor) {
+    for (size_t i = 0; i < in_pcl_pc_ptr->size(); ++i) {
+        pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
+        out_points_array[i * 5 + 0] = point.x;
+        out_points_array[i * 5 + 1] = point.y;
+        out_points_array[i * 5 + 2] = point.z;
+        out_points_array[i * 5 + 3] =
+                static_cast<float>(point.intensity / normalizing_factor);
+        out_points_array[i * 5 + 4] = 0;
+    }
+}
+
+void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
+                 std::vector<float> out_scores,iv::lidar::objectarray & lidarobjvec)
+{
+    int i;
+    int obj_size = out_detections.size()/kOutputNumBoxFeature;
+    //    givlog->verbose("OBJ","object size is %d",obj_size);
+    for(i=0;i<obj_size;i++)
+    {
+        iv::lidar::lidarobject lidarobj;
+        if (out_scores.at(i) < 0.10) continue;
+
+        lidarobj.set_tyaw(out_detections.at(i*7+6));
+        iv::lidar::PointXYZ centroid;
+        iv::lidar::PointXYZ * _centroid;
+        centroid.set_x(out_detections.at(i*7));
+        centroid.set_y(out_detections.at(i*7+1));
+        centroid.set_z(out_detections.at(i*7+2));
+        _centroid = lidarobj.mutable_centroid();
+        _centroid->CopyFrom(centroid);
+
+        iv::lidar::PointXYZ min_point;
+        iv::lidar::PointXYZ * _min_point;
+        min_point.set_x(0);
+        min_point.set_y(0);
+        min_point.set_z(0);
+        _min_point = lidarobj.mutable_min_point();
+        _min_point->CopyFrom(min_point);
+
+        iv::lidar::PointXYZ max_point;
+        iv::lidar::PointXYZ * _max_point;
+        max_point.set_x(0);
+        max_point.set_y(0);
+        max_point.set_z(0);
+        _max_point = lidarobj.mutable_max_point();
+        _max_point->CopyFrom(max_point);
+
+        iv::lidar::PointXYZ position;
+        iv::lidar::PointXYZ * _position;
+        position.set_x(out_detections.at(i*7));
+        position.set_y(out_detections.at(i*7+1));
+        position.set_z(out_detections.at(i*7+2));
+        _position = lidarobj.mutable_position();
+        _position->CopyFrom(position);
+        lidarobj.set_mntype(out_labels.at(i));
+        // label 2  8
+        if(out_labels.at(i)==2){
+            lidarobj.set_mntype(8);
+        }else if(out_labels.at(i)==8){
+            lidarobj.set_mntype(2);
+        }
+        lidarobj.set_score(out_scores.at(i));
+        lidarobj.add_type_probs(out_scores.at(i));
+
+        iv::lidar::PointXYZI point_cloud;
+        iv::lidar::PointXYZI * _point_cloud;
+        point_cloud.set_x(out_detections.at(i*7));
+        point_cloud.set_y(out_detections.at(i*7+1));
+        point_cloud.set_z(out_detections.at(i*7+2));
+        point_cloud.set_i(0);
+
+        _point_cloud = lidarobj.add_cloud();
+        _point_cloud->CopyFrom(point_cloud);
+
+        iv::lidar::Dimension ld;
+        iv::lidar::Dimension * pld;
+        ld.set_x(out_detections.at(i*7+3));// w
+        ld.set_y(out_detections.at(i*7+4));// l
+        ld.set_z(out_detections.at(i*7+5));// h
+        pld = lidarobj.mutable_dimensions();
+        pld->CopyFrom(ld);
+
+        //        std::cout<<"x y z   :  "<<out_detections.at(i*7+3)<<"    "<< out_detections.at(i*7+4)<<"    "<<out_detections.at(i*7+5)<<std::endl;
+        iv::lidar::lidarobject * po = lidarobjvec.add_obj();
+        po->CopyFrom(lidarobj);
+    }
+
+}
+
+void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr)
+{
+    std::shared_ptr<float> points_array_ptr = std::shared_ptr<float>(new float[pc_ptr->size() * kNumPointFeature]);
+    //   float* points_array = new float[pc_ptr->size() * kNumPointFeature];
+    PclXYZITToArray(pc_ptr, points_array_ptr.get(), 1.0);
+
+    int    in_num_points = pc_ptr->width;
+
+    std::vector<float> out_detections;
+    std::vector<int> out_labels;
+    std::vector<float> out_scores;
+
+    QTime xTime;
+
+    xTime.start();
+
+    cudaDeviceSynchronize();
+    pPillars->DoInference(points_array_ptr.get(), in_num_points, &out_detections, &out_labels , &out_scores);
+    cudaDeviceSynchronize();
+
+    //    givlog->verbose("obj size is %d", num_objects);
+    //    std::cout<<"obj size is "<<num_objects<<std::endl;
+
+    //    std::vector<iv::lidar::lidarobject> lidarobjvec;
+    iv::lidar::objectarray lidarobjvec;
+    GetLidarObj(out_detections,out_labels,out_scores,lidarobjvec);
+
+    double timex = pc_ptr->header.stamp;
+    timex = timex/1000.0;
+    lidarobjvec.set_timestamp(pc_ptr->header.stamp);
+    //---------------------------------------------  init tracker  -------------------------------------------------
+    if (!m_isTrackerInitialized)
+    {
+        m_isTrackerInitialized = InitTracker(tracker);
+        if (!m_isTrackerInitialized)
+        {
+            std::cerr << "Tracker initialize error!!!" << std::endl;
+        }
+    }
+    iv::lidar::objectarray trackedobjvec = Tracking(lidarobjvec, tracker);
+    //    std::<<"track    end"<<std::endl;
+
+    //    --------------------------------------------  end tracking  --------------------------------------------------
+    int ntlen;
+    std::string out = trackedobjvec.SerializeAsString();
+    iv::modulecomm::ModuleSendMsg(gpdetect,out.data(),out.length());
+
+    //    givlog->verbose("lenth is %d",out.length());
+}
+
+void ListenPointCloud(const char *strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    //    std::cout<<" is  ok  ------------  "<<std::endl;
+    if(nSize <=16)return;
+    unsigned int * pHeadSize = (unsigned int *)strdata;
+    if(*pHeadSize > nSize)
+    {
+        givlog->verbose("ListenPointCloud data is small headsize = %d, data size is %d", *pHeadSize, nSize);
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
+    }
+
+    gnothavedatatime = 0;
+    QTime xTime;
+    xTime.start();
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    int nNameSize;
+    nNameSize = *pHeadSize - 4-4-8;
+    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
+    std::shared_ptr<char> str_ptr;
+    str_ptr.reset(strName);
+    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
+    point_cloud->header.frame_id = strName;
+    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
+    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
+    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZI xp;
+        memcpy(&xp,p,sizeof(pcl::PointXYZI));
+        xp.z = xp.z;
+        point_cloud->push_back(xp);
+        p++;
+    }
+
+    DectectOnePCD(point_cloud);
+    std::cout<<"time is "<<(QDateTime::currentMSecsSinceEpoch() % 1000)<<" "<<xTime.elapsed()<<std::endl;
+    gfault->SetFaultState(0, 0, "ok");
+
+}
+
+bool gbstate = true;
+void statethread()
+{
+    int nstate = 0;
+    int nlaststate = 0;
+    while (gbstate)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        if(gnothavedatatime < 100000) gnothavedatatime++;
+
+        if (gnothavedatatime  < 100){
+            nstate = 0;
+        }
+        if (gnothavedatatime > 1000)
+        {
+            nstate = 1;
+        }
+        if (gnothavedatatime > 6000)
+        {
+            nstate = 2;
+        }
+        if (nstate != nlaststate) {
+            switch (nstate) {
+            case 0:
+                givlog->info("detection_lidar_pointpillar is ok");
+                gfault->SetFaultState(0,0,"data is ok.");
+                break;
+            case 1:
+                givlog->info(" more than 10 seconds not have lidar pointcloud.");
+                gfault->SetFaultState(1,1,"more than 10 seconds not have lidar pointcloud.");
+                break;
+            case 2:
+                givlog->info(" more than 60 seconds not have lidar pointcloud.");
+                gfault->SetFaultState(2,2, "more than 60 seconds not have lidar pointcloud.");
+                break;
+            default:
+                break;
+            }
+        }
+    }
+}
+
+void exitfunc()
+{
+    gbstate = false;
+    gpthread->join();
+    std::cout<<" state thread closed."<<std::endl;
+    iv::modulecomm::Unregister(gpa);
+    iv::modulecomm::Unregister(gpdetect);
+    std::cout<<"exit func complete"<<std::endl;
+}
+#include <QFile>
+bool trtisexist(std::string strpfe,std::string strbackbone)
+{
+    QFile xFile;
+    xFile.setFileName(strpfe.data());
+    if(xFile.exists() == false)
+    {
+        return false;
+    }
+    xFile.setFileName(strbackbone.data());
+    if(xFile.exists() == false)
+    {
+        return false;
+    }
+    return true;
+}
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    //    RegisterIVBackTrace();
+    tracker.setSettings(settings);
+    gfault = new iv::Ivfault("lidar_pointpillar");
+    givlog = new iv::Ivlog("lidar_pointpillar");
+
+    gfault->SetFaultState(0,0,"pointpillar initialize. ");
+
+    char * strhome = getenv("HOME");
+    std::string pfe_file = strhome;
+    pfe_file += "/models/lidar/cbgs_pp_multihead_pfe.onnx";
+    std::string pfe_trt_file = pfe_file.substr(0, pfe_file.find(".")) + ".trt";
+
+    std::string backbone_file = strhome;
+    backbone_file += "/models/lidar/cbgs_pp_multihead_backbone.onnx";
+    std::string backbone_trt_file = backbone_file.substr(0, backbone_file.find(".")) + ".trt";
+
+    bool btrtexist = trtisexist(pfe_trt_file,backbone_trt_file);
+
+
+    QString strpath = QCoreApplication::applicationDirPath();
+    std::string pp_config = strpath.toStdString() ;
+    pp_config += "/cfgs/cbgs_pp_multihead.yaml";
+    if (argc < 2)
+        strpath = strpath + "/detection_lidar_pointpillar.xml";
+    else
+        strpath = argv[1];
+
+    std::cout<<pp_config<<std::endl;
+
+    iv::xmlparam::Xmlparam xparam(strpath.toStdString());
+    pfe_file = xparam.GetParam("pfe_file",pfe_file.data());
+    backbone_file = xparam.GetParam("backbone_file",backbone_file.data());
+    gstrinput = xparam.GetParam("input","lidar_pc");
+    gstroutput = xparam.GetParam("output","lidar_pointpillar");
+
+    if(btrtexist == false)
+    {
+
+        std::cout<<"use onnx model."<<std::endl;
+        pPillars = new PointPillars(
+                    0.4,
+                    0.2,
+                    true,
+                    pfe_file,
+                    backbone_file,
+                    pp_config
+                    );
+    }
+    else
+    {
+        std::cout<<"use trt model."<<std::endl;
+        pPillars = new PointPillars(
+                    0.1,
+                    0.2,
+                    false,
+                    pfe_trt_file,
+                    backbone_trt_file,
+                    pp_config
+                    );
+    }
+    std::cout<<"PointPillars Init OK."<<std::endl;
+
+    gpa = iv::modulecomm::RegisterRecv(gstrinput.data(),ListenPointCloud);
+    gpdetect = iv::modulecomm::RegisterSend(gstroutput.data(), 10000000,1);
+    gpthread = new std::thread(statethread);
+
+    iv::ivexit::RegIVExitCall(exitfunc);
+    return a.exec();
+}

src/detection/detection_lidar_PointPillars_MultiHead_1025/nms.cu

@@ -0,0 +1,422 @@
+/*
+3D IoU Calculation and Rotated NMS(modified from 2D NMS written by others)
+Written by Shaoshuai Shi
+All Rights Reserved 2019-2020.
+*/
+
+/**
+* @author Yan haixu
+* Contact: just github.com/hova88
+* @date 2021/04/30
+*/
+
+#include <stdio.h>
+#include "nms.h"
+#include "common.h"
+#define THREADS_PER_BLOCK 16
+#define DIVUP(m, n) ((m) / (n) + ((m) % (n) > 0))
+
+// #define DEBUG
+const int THREADS_PER_BLOCK_NMS = sizeof(unsigned long long) * 8;
+const float EPS = 1e-8;
+struct Point {
+    float x, y;
+    __device__ Point() {}
+    __device__ Point(double _x, double _y){
+        x = _x, y = _y;
+    }
+
+    __device__ void set(float _x, float _y){
+        x = _x; y = _y;
+    }
+
+    __device__ Point operator +(const Point &b)const{
+        return Point(x + b.x, y + b.y);
+    }
+
+    __device__ Point operator -(const Point &b)const{
+        return Point(x - b.x, y - b.y);
+    }
+};
+
+__device__ inline float cross(const Point &a, const Point &b){
+    return a.x * b.y - a.y * b.x;
+}
+
+__device__ inline float cross(const Point &p1, const Point &p2, const Point &p0){
+    return (p1.x - p0.x) * (p2.y - p0.y) - (p2.x - p0.x) * (p1.y - p0.y);
+}
+
+__device__ int check_rect_cross(const Point &p1, const Point &p2, const Point &q1, const Point &q2){
+    int ret = min(p1.x,p2.x) <= max(q1.x,q2.x)  &&
+              min(q1.x,q2.x) <= max(p1.x,p2.x) &&
+              min(p1.y,p2.y) <= max(q1.y,q2.y) &&
+              min(q1.y,q2.y) <= max(p1.y,p2.y);
+    return ret;
+}
+
+__device__ inline int check_in_box2d(const float *box, const Point &p){
+    //params: (7) [x, y, z, dx, dy, dz, heading]
+    const float MARGIN = 1e-2;
+
+    float center_x = box[0], center_y = box[1];
+    float angle_cos = cos(-box[6]), angle_sin = sin(-box[6]);  // rotate the point in the opposite direction of box
+    float rot_x = (p.x - center_x) * angle_cos + (p.y - center_y) * (-angle_sin);
+    float rot_y = (p.x - center_x) * angle_sin + (p.y - center_y) * angle_cos;
+
+    return (fabs(rot_x) < box[3] / 2 + MARGIN && fabs(rot_y) < box[4] / 2 + MARGIN);
+}
+
+__device__ inline int intersection(const Point &p1, const Point &p0, const Point &q1, const Point &q0, Point &ans){
+    // fast exclusion
+    if (check_rect_cross(p0, p1, q0, q1) == 0) return 0;
+
+    // check cross standing
+    float s1 = cross(q0, p1, p0);
+    float s2 = cross(p1, q1, p0);
+    float s3 = cross(p0, q1, q0);
+    float s4 = cross(q1, p1, q0);
+
+    if (!(s1 * s2 > 0 && s3 * s4 > 0)) return 0;
+
+    // calculate intersection of two lines
+    float s5 = cross(q1, p1, p0);
+    if(fabs(s5 - s1) > EPS){
+        ans.x = (s5 * q0.x - s1 * q1.x) / (s5 - s1);
+        ans.y = (s5 * q0.y - s1 * q1.y) / (s5 - s1);
+
+    }
+    else{
+        float a0 = p0.y - p1.y, b0 = p1.x - p0.x, c0 = p0.x * p1.y - p1.x * p0.y;
+        float a1 = q0.y - q1.y, b1 = q1.x - q0.x, c1 = q0.x * q1.y - q1.x * q0.y;
+        float D = a0 * b1 - a1 * b0;
+
+        ans.x = (b0 * c1 - b1 * c0) / D;
+        ans.y = (a1 * c0 - a0 * c1) / D;
+    }
+
+    return 1;
+}
+
+__device__ inline void rotate_around_center(const Point &center, const float angle_cos, const float angle_sin, Point &p){
+    float new_x = (p.x - center.x) * angle_cos + (p.y - center.y) * (-angle_sin) + center.x;
+    float new_y = (p.x - center.x) * angle_sin + (p.y - center.y) * angle_cos + center.y;
+    p.set(new_x, new_y);
+}
+
+__device__ inline int point_cmp(const Point &a, const Point &b, const Point &center){
+    return atan2(a.y - center.y, a.x - center.x) > atan2(b.y - center.y, b.x - center.x);
+}
+
+__device__ inline float box_overlap(const float *box_a, const float *box_b){
+    // params box_a: [x, y, z, dx, dy, dz, heading]
+    // params box_b: [x, y, z, dx, dy, dz, heading]
+
+    float a_angle = box_a[6], b_angle = box_b[6];
+    float a_dx_half = box_a[3] / 2, b_dx_half = box_b[3] / 2, a_dy_half = box_a[4] / 2, b_dy_half = box_b[4] / 2;
+    float a_x1 = box_a[0] - a_dx_half, a_y1 = box_a[1] - a_dy_half;
+    float a_x2 = box_a[0] + a_dx_half, a_y2 = box_a[1] + a_dy_half;
+    float b_x1 = box_b[0] - b_dx_half, b_y1 = box_b[1] - b_dy_half;
+    float b_x2 = box_b[0] + b_dx_half, b_y2 = box_b[1] + b_dy_half;
+
+    Point center_a(box_a[0], box_a[1]);
+    Point center_b(box_b[0], box_b[1]);
+
+#ifdef DEBUG
+    printf("a: (%.3f, %.3f, %.3f, %.3f, %.3f), b: (%.3f, %.3f, %.3f, %.3f, %.3f)\n", a_x1, a_y1, a_x2, a_y2, a_angle,
+           b_x1, b_y1, b_x2, b_y2, b_angle);
+    printf("center a: (%.3f, %.3f), b: (%.3f, %.3f)\n", center_a.x, center_a.y, center_b.x, center_b.y);
+#endif
+
+    Point box_a_corners[5];
+    box_a_corners[0].set(a_x1, a_y1);
+    box_a_corners[1].set(a_x2, a_y1);
+    box_a_corners[2].set(a_x2, a_y2);
+    box_a_corners[3].set(a_x1, a_y2);
+
+    Point box_b_corners[5];
+    box_b_corners[0].set(b_x1, b_y1);
+    box_b_corners[1].set(b_x2, b_y1);
+    box_b_corners[2].set(b_x2, b_y2);
+    box_b_corners[3].set(b_x1, b_y2);
+
+    // get oriented corners
+    float a_angle_cos = cos(a_angle), a_angle_sin = sin(a_angle);
+    float b_angle_cos = cos(b_angle), b_angle_sin = sin(b_angle);
+
+    for (int k = 0; k < 4; k++){
+#ifdef DEBUG
+        printf("before corner %d: a(%.3f, %.3f), b(%.3f, %.3f) \n", k, box_a_corners[k].x, box_a_corners[k].y, box_b_corners[k].x, box_b_corners[k].y);
+#endif
+        rotate_around_center(center_a, a_angle_cos, a_angle_sin, box_a_corners[k]);
+        rotate_around_center(center_b, b_angle_cos, b_angle_sin, box_b_corners[k]);
+#ifdef DEBUG
+        printf("corner %d: a(%.3f, %.3f), b(%.3f, %.3f) \n", k, box_a_corners[k].x, box_a_corners[k].y, box_b_corners[k].x, box_b_corners[k].y);
+#endif
+    }
+
+    box_a_corners[4] = box_a_corners[0];
+    box_b_corners[4] = box_b_corners[0];
+
+    // get intersection of lines
+    Point cross_points[16];
+    Point poly_center;
+    int cnt = 0, flag = 0;
+
+    poly_center.set(0, 0);
+    for (int i = 0; i < 4; i++){
+        for (int j = 0; j < 4; j++){
+            flag = intersection(box_a_corners[i + 1], box_a_corners[i], box_b_corners[j + 1], box_b_corners[j], cross_points[cnt]);
+            if (flag){
+                poly_center = poly_center + cross_points[cnt];
+                cnt++;
+#ifdef DEBUG
+                printf("Cross points (%.3f, %.3f): a(%.3f, %.3f)->(%.3f, %.3f), b(%.3f, %.3f)->(%.3f, %.3f) \n",
+                    cross_points[cnt - 1].x, cross_points[cnt - 1].y,
+                    box_a_corners[i].x, box_a_corners[i].y, box_a_corners[i + 1].x, box_a_corners[i + 1].y,
+                    box_b_corners[i].x, box_b_corners[i].y, box_b_corners[i + 1].x, box_b_corners[i + 1].y);
+#endif
+            }
+        }
+    }
+
+    // check corners
+    for (int k = 0; k < 4; k++){
+        if (check_in_box2d(box_a, box_b_corners[k])){
+            poly_center = poly_center + box_b_corners[k];
+            cross_points[cnt] = box_b_corners[k];
+            cnt++;
+#ifdef DEBUG
+                printf("b corners in a: corner_b(%.3f, %.3f)", cross_points[cnt - 1].x, cross_points[cnt - 1].y);
+#endif
+        }
+        if (check_in_box2d(box_b, box_a_corners[k])){
+            poly_center = poly_center + box_a_corners[k];
+            cross_points[cnt] = box_a_corners[k];
+            cnt++;
+#ifdef DEBUG
+                printf("a corners in b: corner_a(%.3f, %.3f)", cross_points[cnt - 1].x, cross_points[cnt - 1].y);
+#endif
+        }
+    }
+
+    poly_center.x /= cnt;
+    poly_center.y /= cnt;
+
+    // sort the points of polygon
+    Point temp;
+    for (int j = 0; j < cnt - 1; j++){
+        for (int i = 0; i < cnt - j - 1; i++){
+            if (point_cmp(cross_points[i], cross_points[i + 1], poly_center)){
+                temp = cross_points[i];
+                cross_points[i] = cross_points[i + 1];
+                cross_points[i + 1] = temp;
+            }
+        }
+    }
+
+#ifdef DEBUG
+    printf("cnt=%d\n", cnt);
+    for (int i = 0; i < cnt; i++){
+        printf("All cross point %d: (%.3f, %.3f)\n", i, cross_points[i].x, cross_points[i].y);
+    }
+#endif
+
+    // get the overlap areas
+    float area = 0;
+    for (int k = 0; k < cnt - 1; k++){
+        area += cross(cross_points[k] - cross_points[0], cross_points[k + 1] - cross_points[0]);
+    }
+
+    return fabs(area) / 2.0;
+}
+
+__device__ inline float iou_bev(const float *box_a, const float *box_b){
+    // params box_a: [x, y, z, dx, dy, dz, heading]
+    // params box_b: [x, y, z, dx, dy, dz, heading]
+    float sa = box_a[3] * box_a[4];
+    float sb = box_b[3] * box_b[4];
+    float s_overlap = box_overlap(box_a, box_b);
+    return s_overlap / fmaxf(sa + sb - s_overlap, EPS);
+}
+
+__global__ void boxes_overlap_kernel(const int num_a, const float *boxes_a, const int num_b, const float *boxes_b, float *ans_overlap){
+    // params boxes_a: (N, 7) [x, y, z, dx, dy, dz, heading]
+    // params boxes_b: (M, 7) [x, y, z, dx, dy, dz, heading]
+    const int a_idx = blockIdx.y * THREADS_PER_BLOCK + threadIdx.y;
+    const int b_idx = blockIdx.x * THREADS_PER_BLOCK + threadIdx.x;
+
+    if (a_idx >= num_a || b_idx >= num_b){
+        return;
+    }
+    const float * cur_box_a = boxes_a + a_idx * 7;
+    const float * cur_box_b = boxes_b + b_idx * 7;
+    float s_overlap = box_overlap(cur_box_a, cur_box_b);
+    ans_overlap[a_idx * num_b + b_idx] = s_overlap;
+}
+
+__global__ void boxes_iou_bev_kernel(const int num_a, const float *boxes_a, const int num_b, const float *boxes_b, float *ans_iou){
+    // params boxes_a: (N, 7) [x, y, z, dx, dy, dz, heading]
+    // params boxes_b: (M, 7) [x, y, z, dx, dy, dz, heading]
+    const int a_idx = blockIdx.y * THREADS_PER_BLOCK + threadIdx.y;
+    const int b_idx = blockIdx.x * THREADS_PER_BLOCK + threadIdx.x;
+
+    if (a_idx >= num_a || b_idx >= num_b){
+        return;
+    }
+
+    const float * cur_box_a = boxes_a + a_idx * 7;
+    const float * cur_box_b = boxes_b + b_idx * 7;
+    float cur_iou_bev = iou_bev(cur_box_a, cur_box_b);
+    ans_iou[a_idx * num_b + b_idx] = cur_iou_bev;
+}
+
+__global__ void nms_kernel(const int boxes_num, const float nms_overlap_thresh,
+                           const float *boxes, unsigned long long *mask){
+    //params: boxes (N, 7) [x, y, z, dx, dy, dz, heading]
+    //params: mask (N, N/THREADS_PER_BLOCK_NMS)
+
+    const int row_start = blockIdx.y;
+    const int col_start = blockIdx.x;
+
+    // if (row_start > col_start) return;
+
+    const int row_size = fminf(boxes_num - row_start * THREADS_PER_BLOCK_NMS, THREADS_PER_BLOCK_NMS);
+    const int col_size = fminf(boxes_num - col_start * THREADS_PER_BLOCK_NMS, THREADS_PER_BLOCK_NMS);
+
+    __shared__ float block_boxes[THREADS_PER_BLOCK_NMS * 7];
+
+    if (threadIdx.x < col_size) {
+        block_boxes[threadIdx.x * 7 + 0] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 0];
+        block_boxes[threadIdx.x * 7 + 1] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 1];
+        block_boxes[threadIdx.x * 7 + 2] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 2];
+        block_boxes[threadIdx.x * 7 + 3] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 3];
+        block_boxes[threadIdx.x * 7 + 4] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 4];
+        block_boxes[threadIdx.x * 7 + 5] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 5];
+        block_boxes[threadIdx.x * 7 + 6] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 6];
+    }
+    __syncthreads();
+
+    if (threadIdx.x < row_size) {
+        const int cur_box_idx = THREADS_PER_BLOCK_NMS * row_start + threadIdx.x;
+        const float *cur_box = boxes + cur_box_idx * 7;
+
+        int i = 0;
+        unsigned long long t = 0;
+        int start = 0;
+        if (row_start == col_start) {
+          start = threadIdx.x + 1;
+        }
+        for (i = start; i < col_size; i++) {
+            if (iou_bev(cur_box, block_boxes + i * 7) > nms_overlap_thresh){
+                t |= 1ULL << i;
+            }
+        }
+        const int col_blocks = DIVUP(boxes_num, THREADS_PER_BLOCK_NMS);
+        mask[cur_box_idx * col_blocks + col_start] = t;
+    }
+}
+
+
+__device__ inline float iou_normal(float const * const a, float const * const b) {
+    //params: a: [x, y, z, dx, dy, dz, heading]
+    //params: b: [x, y, z, dx, dy, dz, heading]
+
+    float left = fmaxf(a[0] - a[3] / 2, b[0] - b[3] / 2), right = fminf(a[0] + a[3] / 2, b[0] + b[3] / 2);
+    float top = fmaxf(a[1] - a[4] / 2, b[1] - b[4] / 2), bottom = fminf(a[1] + a[4] / 2, b[1] + b[4] / 2);
+    float width = fmaxf(right - left, 0.f), height = fmaxf(bottom - top, 0.f);
+    float interS = width * height;
+    float Sa = a[3] * a[4];
+    float Sb = b[3] * b[4];
+    return interS / fmaxf(Sa + Sb - interS, EPS);
+}
+
+
+__global__ void nms_normal_kernel(const int boxes_num, const float nms_overlap_thresh,
+                           const float *boxes, unsigned long long *mask){
+    //params: boxes (N, 7) [x, y, z, dx, dy, dz, heading]
+    //params: mask (N, N/THREADS_PER_BLOCK_NMS)
+
+    const int row_start = blockIdx.y;
+    const int col_start = blockIdx.x;
+
+    // if (row_start > col_start) return;
+
+    const int row_size = fminf(boxes_num - row_start * THREADS_PER_BLOCK_NMS, THREADS_PER_BLOCK_NMS);
+    const int col_size = fminf(boxes_num - col_start * THREADS_PER_BLOCK_NMS, THREADS_PER_BLOCK_NMS);
+
+    __shared__ float block_boxes[THREADS_PER_BLOCK_NMS * 7];
+
+    if (threadIdx.x < col_size) {
+        block_boxes[threadIdx.x * 7 + 0] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 0];
+        block_boxes[threadIdx.x * 7 + 1] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 1];
+        block_boxes[threadIdx.x * 7 + 2] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 2];
+        block_boxes[threadIdx.x * 7 + 3] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 3];
+        block_boxes[threadIdx.x * 7 + 4] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 4];
+        block_boxes[threadIdx.x * 7 + 5] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 5];
+        block_boxes[threadIdx.x * 7 + 6] = boxes[(THREADS_PER_BLOCK_NMS * col_start + threadIdx.x) * 7 + 6];
+    }
+    __syncthreads();
+
+    if (threadIdx.x < row_size) {
+        const int cur_box_idx = THREADS_PER_BLOCK_NMS * row_start + threadIdx.x;
+        const float *cur_box = boxes + cur_box_idx * 7;
+
+        int i = 0;
+        unsigned long long t = 0;
+        int start = 0;
+        if (row_start == col_start) {
+          start = threadIdx.x + 1;
+        }
+        for (i = start; i < col_size; i++) {
+            if (iou_normal(cur_box, block_boxes + i * 7) > nms_overlap_thresh){
+                t |= 1ULL << i;
+            }
+        }
+        const int col_blocks = DIVUP(boxes_num, THREADS_PER_BLOCK_NMS);
+        mask[cur_box_idx * col_blocks + col_start] = t;
+    }
+}
+
+
+NmsCuda::NmsCuda(const int num_threads, const int num_box_corners,
+    const float nms_overlap_threshold)
+: num_threads_(num_threads),
+  num_box_corners_(num_box_corners),
+  nms_overlap_threshold_(nms_overlap_threshold) {}
+
+void NmsCuda::DoNmsCuda(const int host_filter_count,
+    float *dev_sorted_box_for_nms, long *out_keep_inds,
+    int *out_num_to_keep) {
+
+    const int col_blocks = DIVUP(host_filter_count, num_threads_);
+    unsigned long long *dev_mask = NULL;
+    GPU_CHECK(cudaMalloc(&dev_mask, host_filter_count * col_blocks * sizeof(unsigned long long)));    
+
+    dim3 blocks(DIVUP(host_filter_count, num_threads_),
+                DIVUP(host_filter_count, num_threads_));
+    dim3 threads(num_threads_);
+
+    nms_kernel<<<blocks, threads>>>(host_filter_count, nms_overlap_threshold_, dev_sorted_box_for_nms, dev_mask);
+    // postprocess for nms output
+    std::vector<unsigned long long> host_mask(host_filter_count * col_blocks);
+    GPU_CHECK(cudaMemcpy(&host_mask[0], dev_mask,
+            sizeof(unsigned long long) * host_filter_count * col_blocks,
+            cudaMemcpyDeviceToHost));
+    std::vector<unsigned long long> remv(col_blocks);
+    memset(&remv[0], 0, sizeof(unsigned long long) * col_blocks);
+
+    for (int i = 0; i < host_filter_count; ++i) {
+        int nblock = i / num_threads_;
+        int inblock = i % num_threads_;
+
+        if (!(remv[nblock] & (1ULL << inblock))) {
+            out_keep_inds[(*out_num_to_keep)++] = i;
+            unsigned long long *p = &host_mask[0] + i * col_blocks;
+            for (int j = nblock; j < col_blocks; ++j) {
+                remv[j] |= p[j];
+            }
+        }
+    }
+    GPU_CHECK(cudaFree(dev_mask));
+}

src/detection/detection_lidar_PointPillars_MultiHead_1025/nms.h

@@ -0,0 +1,64 @@
+
+/*
+3D IoU Calculation and Rotated NMS(modified from 2D NMS written by others)
+Written by Shaoshuai Shi
+All Rights Reserved 2019-2020.
+*/
+
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @author Kosuke Murakami
+ * @date 2019/02/26
+ */
+
+/**
+* @author Yan haixu
+* Contact: just github.com/hova88
+* @date 2021/04/30
+*/
+
+
+class NmsCuda {
+ private:
+  const int num_threads_;
+  const int num_box_corners_;
+  const float nms_overlap_threshold_;
+
+ public:
+  /**
+   * @brief Constructor
+   * @param[in] num_threads Number of threads when launching cuda kernel
+   * @param[in] num_box_corners Number of corners for 2D box
+   * @param[in] nms_overlap_threshold IOU threshold for NMS
+   * @details Captital variables never change after the compile, Non-captital
+   * variables could be chaned through rosparam
+   */
+  NmsCuda(const int num_threads, const int num_box_corners,
+          const float nms_overlap_threshold);
+
+  /**
+   * @brief GPU Non-Maximum Suppresion for network output
+   * @param[in] host_filter_count Number of filtered output
+   * @param[in] dev_sorted_box_for_nms Bounding box output sorted by score
+   * @param[out] out_keep_inds Indexes of selected bounding box
+   * @param[out] out_num_to_keep Number of kept bounding boxes
+   * @details NMS in GPU and postprocessing for selecting box in CPU
+   */
+  void DoNmsCuda(const int host_filter_count, float* dev_sorted_box_for_nms,
+                 long* out_keep_inds, int* out_num_to_keep);
+};

src/detection/detection_lidar_PointPillars_MultiHead_1025/pointpillars.cc

@@ -0,0 +1,507 @@
+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @author Kosuke Murakami
+ * @date 2019/02/26
+ */
+
+/**
+* @author Yan haixu
+* Contact: just github.com/hova88
+* @date 2021/04/30
+*/
+
+
+
+
+#include "pointpillars.h"
+
+#include <chrono>
+#include <iostream>
+#include <iostream>
+
+PointPillars::PointPillars(const float score_threshold,
+                           const float nms_overlap_threshold,
+                           const bool use_onnx,
+                           const std::string pfe_file,
+                           const std::string backbone_file,
+                           const std::string pp_config)
+    : score_threshold_(score_threshold),
+      nms_overlap_threshold_(nms_overlap_threshold),
+      use_onnx_(use_onnx),
+      pfe_file_(pfe_file),
+      backbone_file_(backbone_file),
+      pp_config_(pp_config)
+{
+    InitParams();
+    InitTRT(use_onnx_);
+    DeviceMemoryMalloc();
+
+    preprocess_points_cuda_ptr_.reset(new PreprocessPointsCuda(
+        kNumThreads,
+        kMaxNumPillars,
+        kMaxNumPointsPerPillar,
+        kNumPointFeature,
+        kNumIndsForScan,
+        kGridXSize,kGridYSize, kGridZSize,
+        kPillarXSize,kPillarYSize, kPillarZSize,
+        kMinXRange, kMinYRange, kMinZRange));
+
+    scatter_cuda_ptr_.reset(new ScatterCuda(kNumThreads, kGridXSize, kGridYSize));
+
+    const float float_min = std::numeric_limits<float>::lowest();
+    const float float_max = std::numeric_limits<float>::max();
+    postprocess_cuda_ptr_.reset(
+      new PostprocessCuda(kNumThreads,
+                          float_min, float_max, 
+                          kNumClass,kNumAnchorPerCls,
+                          kMultiheadLabelMapping,
+                          score_threshold_, 
+                          nms_overlap_threshold_,
+                          kNmsPreMaxsize, 
+                          kNmsPostMaxsize,
+                          kNumBoxCorners, 
+                          kNumInputBoxFeature,
+                          7));  /*kNumOutputBoxFeature*/
+    
+}
+
+PointPillars::~PointPillars() {
+    // for pillars 
+    GPU_CHECK(cudaFree(dev_num_points_per_pillar_));
+    GPU_CHECK(cudaFree(dev_x_coors_));
+    GPU_CHECK(cudaFree(dev_y_coors_));
+    GPU_CHECK(cudaFree(dev_pillar_point_feature_));
+    GPU_CHECK(cudaFree(dev_pillar_coors_));
+    // for sparse map
+    GPU_CHECK(cudaFree(dev_sparse_pillar_map_));    
+    GPU_CHECK(cudaFree(dev_cumsum_along_x_));
+    GPU_CHECK(cudaFree(dev_cumsum_along_y_));
+    // for pfe forward
+    GPU_CHECK(cudaFree(dev_pfe_gather_feature_));
+      
+    GPU_CHECK(cudaFree(pfe_buffers_[0]));
+    GPU_CHECK(cudaFree(pfe_buffers_[1]));
+
+    GPU_CHECK(cudaFree(rpn_buffers_[0]));
+    GPU_CHECK(cudaFree(rpn_buffers_[1]));
+    GPU_CHECK(cudaFree(rpn_buffers_[2]));
+    GPU_CHECK(cudaFree(rpn_buffers_[3]));
+    GPU_CHECK(cudaFree(rpn_buffers_[4]));
+    GPU_CHECK(cudaFree(rpn_buffers_[5]));
+    GPU_CHECK(cudaFree(rpn_buffers_[6]));
+    GPU_CHECK(cudaFree(rpn_buffers_[7]));
+    pfe_context_->destroy();
+    backbone_context_->destroy();
+    pfe_engine_->destroy();
+    backbone_engine_->destroy();
+    // for post process
+    GPU_CHECK(cudaFree(dev_scattered_feature_));
+    GPU_CHECK(cudaFree(dev_filtered_box_));
+    GPU_CHECK(cudaFree(dev_filtered_score_));
+    GPU_CHECK(cudaFree(dev_filtered_label_));
+    GPU_CHECK(cudaFree(dev_filtered_dir_));
+    GPU_CHECK(cudaFree(dev_box_for_nms_));
+    GPU_CHECK(cudaFree(dev_filter_count_));
+
+
+}
+
+void PointPillars::InitParams()
+{
+    YAML::Node params = YAML::LoadFile(pp_config_);
+    kPillarXSize = params["DATA_CONFIG"]["DATA_PROCESSOR"][2]["VOXEL_SIZE"][0].as<float>();
+    kPillarYSize = params["DATA_CONFIG"]["DATA_PROCESSOR"][2]["VOXEL_SIZE"][1].as<float>();
+    kPillarZSize = params["DATA_CONFIG"]["DATA_PROCESSOR"][2]["VOXEL_SIZE"][2].as<float>();
+    kMinXRange = params["DATA_CONFIG"]["POINT_CLOUD_RANGE"][0].as<float>();
+    kMinYRange = params["DATA_CONFIG"]["POINT_CLOUD_RANGE"][1].as<float>();
+    kMinZRange = params["DATA_CONFIG"]["POINT_CLOUD_RANGE"][2].as<float>();
+    kMaxXRange = params["DATA_CONFIG"]["POINT_CLOUD_RANGE"][3].as<float>();
+    kMaxYRange = params["DATA_CONFIG"]["POINT_CLOUD_RANGE"][4].as<float>();
+    kMaxZRange = params["DATA_CONFIG"]["POINT_CLOUD_RANGE"][5].as<float>();
+    kNumClass = params["CLASS_NAMES"].size();
+    kMaxNumPillars = params["DATA_CONFIG"]["DATA_PROCESSOR"][2]["MAX_NUMBER_OF_VOXELS"]["test"].as<int>();
+    kMaxNumPointsPerPillar = params["DATA_CONFIG"]["DATA_PROCESSOR"][2]["MAX_POINTS_PER_VOXEL"].as<int>();
+    kNumPointFeature = 5; // [x, y, z, i,0]
+    kNumInputBoxFeature = 7;
+    kNumOutputBoxFeature = params["MODEL"]["DENSE_HEAD"]["TARGET_ASSIGNER_CONFIG"]["BOX_CODER_CONFIG"]["code_size"].as<int>();
+    kBatchSize = 1;
+    kNumIndsForScan = 1024;
+    kNumThreads = 64;
+    kNumBoxCorners = 8;
+    kAnchorStrides = 4;
+    kNmsPreMaxsize = params["MODEL"]["POST_PROCESSING"]["NMS_CONFIG"]["NMS_PRE_MAXSIZE"].as<int>();
+    kNmsPostMaxsize = params["MODEL"]["POST_PROCESSING"]["NMS_CONFIG"]["NMS_POST_MAXSIZE"].as<int>();
+    //params for initialize anchors
+    //Adapt to OpenPCDet
+    kAnchorNames = params["CLASS_NAMES"].as<std::vector<std::string>>();
+    for (int i = 0; i < kAnchorNames.size(); ++i)
+    {
+        kAnchorDxSizes.emplace_back(params["MODEL"]["DENSE_HEAD"]["ANCHOR_GENERATOR_CONFIG"][i]["anchor_sizes"][0][0].as<float>());
+        kAnchorDySizes.emplace_back(params["MODEL"]["DENSE_HEAD"]["ANCHOR_GENERATOR_CONFIG"][i]["anchor_sizes"][0][1].as<float>());
+        kAnchorDzSizes.emplace_back(params["MODEL"]["DENSE_HEAD"]["ANCHOR_GENERATOR_CONFIG"][i]["anchor_sizes"][0][2].as<float>());
+        kAnchorBottom.emplace_back(params["MODEL"]["DENSE_HEAD"]["ANCHOR_GENERATOR_CONFIG"][i]["anchor_bottom_heights"][0].as<float>());
+    }
+    for (int idx_head = 0; idx_head < params["MODEL"]["DENSE_HEAD"]["RPN_HEAD_CFGS"].size(); ++idx_head)
+    {
+        int num_cls_per_head = params["MODEL"]["DENSE_HEAD"]["RPN_HEAD_CFGS"][idx_head]["HEAD_CLS_NAME"].size();
+        std::vector<int> value;
+        for (int i = 0; i < num_cls_per_head; ++i)
+        {
+            value.emplace_back(idx_head + i);
+        }
+        kMultiheadLabelMapping.emplace_back(value);
+    }
+
+    // Generate secondary parameters based on above.
+    kGridXSize = static_cast<int>((kMaxXRange - kMinXRange) / kPillarXSize); //512
+    kGridYSize = static_cast<int>((kMaxYRange - kMinYRange) / kPillarYSize); //512
+    kGridZSize = static_cast<int>((kMaxZRange - kMinZRange) / kPillarZSize); //1
+    kRpnInputSize = 64 * kGridYSize * kGridXSize;
+
+    kNumAnchorXinds = static_cast<int>(kGridXSize / kAnchorStrides); //Width
+    kNumAnchorYinds = static_cast<int>(kGridYSize / kAnchorStrides); //Hight
+    kNumAnchor = kNumAnchorXinds * kNumAnchorYinds * 2 * kNumClass;  // H * W * Ro * N = 196608
+
+    kNumAnchorPerCls = kNumAnchorXinds * kNumAnchorYinds * 2; //H * W * Ro = 32768
+    kRpnBoxOutputSize = kNumAnchor * kNumOutputBoxFeature;
+    kRpnClsOutputSize = kNumAnchor * kNumClass;
+    kRpnDirOutputSize = kNumAnchor * 2;
+}
+
+void PointPillars::DeviceMemoryMalloc() {
+    // for pillars 
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_num_points_per_pillar_), kMaxNumPillars * sizeof(float))); // M
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_x_coors_), kMaxNumPillars * sizeof(int))); // M
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_y_coors_), kMaxNumPillars * sizeof(int))); // M
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_pillar_point_feature_), kMaxNumPillars * kMaxNumPointsPerPillar * kNumPointFeature * sizeof(float))); // [M , m , 4]
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_pillar_coors_),  kMaxNumPillars * 4 * sizeof(float))); // [M , 4]
+    // for sparse map
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_sparse_pillar_map_), kNumIndsForScan * kNumIndsForScan * sizeof(int))); // [1024 , 1024]
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_cumsum_along_x_), kNumIndsForScan * kNumIndsForScan * sizeof(int))); // [1024 , 1024]
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_cumsum_along_y_), kNumIndsForScan * kNumIndsForScan * sizeof(int)));// [1024 , 1024]
+
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_pfe_gather_feature_),
+                        kMaxNumPillars * kMaxNumPointsPerPillar *
+                            kNumGatherPointFeature * sizeof(float)));
+    // for trt inference
+    // create GPU buffers and a stream
+
+    GPU_CHECK(
+        cudaMalloc(&pfe_buffers_[0], kMaxNumPillars * kMaxNumPointsPerPillar *
+                                        kNumGatherPointFeature * sizeof(float)));
+    GPU_CHECK(cudaMalloc(&pfe_buffers_[1], kMaxNumPillars * 64 * sizeof(float)));
+
+    GPU_CHECK(cudaMalloc(&rpn_buffers_[0],  kRpnInputSize * sizeof(float)));
+
+    GPU_CHECK(cudaMalloc(&rpn_buffers_[1],  kNumAnchorPerCls  * sizeof(float)));  //classes
+    GPU_CHECK(cudaMalloc(&rpn_buffers_[2],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+    GPU_CHECK(cudaMalloc(&rpn_buffers_[3],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+    GPU_CHECK(cudaMalloc(&rpn_buffers_[4],  kNumAnchorPerCls  * sizeof(float)));
+    GPU_CHECK(cudaMalloc(&rpn_buffers_[5],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+    GPU_CHECK(cudaMalloc(&rpn_buffers_[6],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+    
+    GPU_CHECK(cudaMalloc(&rpn_buffers_[7],  kNumAnchorPerCls * kNumClass * kNumOutputBoxFeature * sizeof(float))); //boxes
+
+    // for scatter kernel
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_scattered_feature_),
+                        kNumThreads * kGridYSize * kGridXSize * sizeof(float)));
+    // for filter
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_filtered_box_),
+                        kNumAnchor * kNumOutputBoxFeature * sizeof(float)));
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_filtered_score_),
+                        kNumAnchor * sizeof(float)));
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_filtered_label_),
+                        kNumAnchor * sizeof(int)));
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_filtered_dir_),
+                        kNumAnchor * sizeof(int)));
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_box_for_nms_),
+                        kNumAnchor * kNumBoxCorners * sizeof(float)));
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_filter_count_), kNumClass * sizeof(int)));
+
+}
+
+void PointPillars::SetDeviceMemoryToZero() {
+
+    GPU_CHECK(cudaMemset(dev_num_points_per_pillar_, 0, kMaxNumPillars * sizeof(float)));
+    GPU_CHECK(cudaMemset(dev_x_coors_,               0, kMaxNumPillars * sizeof(int)));
+    GPU_CHECK(cudaMemset(dev_y_coors_,               0, kMaxNumPillars * sizeof(int)));
+    GPU_CHECK(cudaMemset(dev_pillar_point_feature_,  0, kMaxNumPillars * kMaxNumPointsPerPillar * kNumPointFeature * sizeof(float)));
+    GPU_CHECK(cudaMemset(dev_pillar_coors_,          0, kMaxNumPillars * 4 * sizeof(float)));
+    // GPU_CHECK(cudaMemset(dev_sparse_pillar_map_,     0, kNumIndsForScan * kNumIndsForScan * sizeof(int)));
+    GPU_CHECK(cudaMemset(dev_pfe_gather_feature_,    0, kMaxNumPillars * kMaxNumPointsPerPillar * kNumGatherPointFeature * sizeof(float)));
+    
+    // GPU_CHECK(cudaMemset(pfe_buffers_[0],       0, kMaxNumPillars * kMaxNumPointsPerPillar * kNumGatherPointFeature * sizeof(float)));
+    // GPU_CHECK(cudaMemset(pfe_buffers_[1],       0, kMaxNumPillars * 64 * sizeof(float)));
+
+    GPU_CHECK(cudaMemset(dev_scattered_feature_,    0, kNumThreads * kGridYSize * kGridXSize * sizeof(float)));
+    
+    // GPU_CHECK(cudaMemset(rpn_buffers_[0],    0, kRpnInputSize * sizeof(float)));
+    // GPU_CHECK(cudaMemset(rpn_buffers_[1],    0, kNumAnchorPerCls * kNumOutputBoxFeature * sizeof(float)));
+    // GPU_CHECK(cudaMemset(rpn_buffers_[2],    0, kNumAnchorPerCls     * sizeof(float)));
+    // GPU_CHECK(cudaMemset(rpn_buffers_[3],    0, kNumAnchorPerCls * 2 * kNumOutputBoxFeature * sizeof(float)));
+    // GPU_CHECK(cudaMemset(rpn_buffers_[4],    0, kNumAnchorPerCls * 4 * sizeof(float)));
+    // GPU_CHECK(cudaMemset(rpn_buffers_[5],    0, kNumAnchorPerCls * kNumOutputBoxFeature * sizeof(float)));
+    // GPU_CHECK(cudaMemset(rpn_buffers_[6],    0, kNumAnchorPerCls     * sizeof(float)));
+    // GPU_CHECK(cudaMemset(rpn_buffers_[7],    0, kNumAnchorPerCls * 2 * kNumOutputBoxFeature * sizeof(float)));
+    // GPU_CHECK(cudaMemset(rpn_buffers_[8],    0, kNumAnchorPerCls * 4 * sizeof(float)));
+    // GPU_CHECK(cudaMemset(rpn_buffers_[9],    0, kNumAnchorPerCls * kNumOutputBoxFeature * sizeof(float)));
+    // GPU_CHECK(cudaMemset(rpn_buffers_[10],   0, kNumAnchorPerCls     * sizeof(float)));
+
+    GPU_CHECK(cudaMemset(dev_filtered_box_,     0, kNumAnchor * kNumOutputBoxFeature * sizeof(float)));
+    GPU_CHECK(cudaMemset(dev_filtered_score_,   0, kNumAnchor * sizeof(float)));
+    GPU_CHECK(cudaMemset(dev_filter_count_,     0, kNumClass * sizeof(int)));
+}
+
+void PointPillars::InitTRT(const bool use_onnx) {
+  if (use_onnx_) {
+    // create a TensorRT model from the onnx model and load it into an engine
+    OnnxToTRTModel(pfe_file_, &pfe_engine_);
+    SaveEngine(pfe_engine_, pfe_file_.substr(0, pfe_file_.find(".")) + ".trt");
+    OnnxToTRTModel(backbone_file_, &backbone_engine_);
+    SaveEngine(backbone_engine_, backbone_file_.substr(0, backbone_file_.find(".")) + ".trt");
+  }else {
+    EngineToTRTModel(pfe_file_, &pfe_engine_);
+    EngineToTRTModel(backbone_file_, &backbone_engine_);
+  }
+    if (pfe_engine_ == nullptr || backbone_engine_ == nullptr) {
+        std::cerr << "Failed to load ONNX file.";
+    }
+
+    // create execution context from the engine
+    pfe_context_ = pfe_engine_->createExecutionContext();
+    backbone_context_ = backbone_engine_->createExecutionContext();
+    if (pfe_context_ == nullptr || backbone_context_ == nullptr) {
+        std::cerr << "Failed to create TensorRT Execution Context.";
+    }
+  
+}
+
+void PointPillars::OnnxToTRTModel(
+    const std::string& model_file,  // name of the onnx model
+    nvinfer1::ICudaEngine** engine_ptr) {
+    int verbosity = static_cast<int>(nvinfer1::ILogger::Severity::kWARNING);
+
+    // create the builder
+    const auto explicit_batch =
+        static_cast<uint32_t>(kBatchSize) << static_cast<uint32_t>(
+            nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH);
+    nvinfer1::IBuilder* builder = nvinfer1::createInferBuilder(g_logger_);
+    nvinfer1::INetworkDefinition* network =
+        builder->createNetworkV2(explicit_batch);
+
+    // parse onnx model
+    auto parser = nvonnxparser::createParser(*network, g_logger_);
+    if (!parser->parseFromFile(model_file.c_str(), verbosity)) {
+        std::string msg("failed to parse onnx file");
+        g_logger_.log(nvinfer1::ILogger::Severity::kERROR, msg.c_str());
+        exit(EXIT_FAILURE);
+    }
+
+    // Build the engine
+    builder->setMaxBatchSize(kBatchSize);
+    builder->setHalf2Mode(true);
+    nvinfer1::IBuilderConfig* config = builder->createBuilderConfig();
+    config->setMaxWorkspaceSize(1 << 25);
+    nvinfer1::ICudaEngine* engine =
+        builder->buildEngineWithConfig(*network, *config);
+
+    *engine_ptr = engine;
+    parser->destroy();
+    network->destroy();
+    config->destroy();
+    builder->destroy();
+}
+
+void PointPillars::SaveEngine(const nvinfer1::ICudaEngine* engine, const std::string& engine_filepath)
+{
+    // serialize the engine, then close everything down
+    nvinfer1::IHostMemory& trtModelStream = *(engine->serialize());
+    std::ofstream file;
+    file.open(engine_filepath, std::ios::binary | std::ios::out);
+    if(!file.is_open())
+    {
+        std::cout << "read create engine file" << engine_filepath <<" failed" << std::endl;
+        return;
+    }
+    file.write((const char*)trtModelStream.data(), std::streamsize(trtModelStream.size()));
+    file.close();
+}
+
+void PointPillars::EngineToTRTModel(
+    const std::string &engine_file ,     
+    nvinfer1::ICudaEngine** engine_ptr)  {
+    int verbosity = static_cast<int>(nvinfer1::ILogger::Severity::kWARNING);
+    std::stringstream gieModelStream; 
+    gieModelStream.seekg(0, gieModelStream.beg); 
+
+    std::ifstream cache(engine_file); 
+    gieModelStream << cache.rdbuf();
+    cache.close(); 
+    nvinfer1::IRuntime* runtime = nvinfer1::createInferRuntime(g_logger_); 
+
+    if (runtime == nullptr) {
+        std::string msg("failed to build runtime parser");
+        g_logger_.log(nvinfer1::ILogger::Severity::kERROR, msg.c_str());
+        exit(EXIT_FAILURE);
+    }
+    gieModelStream.seekg(0, std::ios::end);
+    const int modelSize = gieModelStream.tellg(); 
+
+    gieModelStream.seekg(0, std::ios::beg);
+    void* modelMem = malloc(modelSize); 
+    gieModelStream.read((char*)modelMem, modelSize);
+
+    std::cout << "                                                                  "<< std::endl;
+    std::cout << "------------------------------------------------------------------"<< std::endl;
+    std::cout << ">>>>                                                          >>>>"<< std::endl;
+    std::cout << "                                                                  "<< std::endl;
+    std::cout << "Input filename:   " << engine_file << std::endl;
+    std::cout << "                                                                  "<< std::endl;
+    std::cout << ">>>>                                                          >>>>"<< std::endl;
+    std::cout << "------------------------------------------------------------------"<< std::endl;
+    std::cout << "                                                                  "<< std::endl;
+
+    nvinfer1::ICudaEngine* engine = runtime->deserializeCudaEngine(modelMem, modelSize, NULL); 
+    if (engine == nullptr) {
+        std::string msg("failed to build engine parser");
+        g_logger_.log(nvinfer1::ILogger::Severity::kERROR, msg.c_str());
+        exit(EXIT_FAILURE);
+    }
+    *engine_ptr = engine;
+
+}
+
+void PointPillars::DoInference(const float* in_points_array,
+                                const int in_num_points,
+                                std::vector<float>* out_detections,
+                                std::vector<int>* out_labels,
+                                std::vector<float>* out_scores) 
+{
+    SetDeviceMemoryToZero();
+    cudaDeviceSynchronize();
+    // [STEP 1] : load pointcloud
+    float* dev_points;
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_points),
+                        in_num_points * kNumPointFeature * sizeof(float))); // in_num_points , 5
+    GPU_CHECK(cudaMemset(dev_points, 0, in_num_points * kNumPointFeature * sizeof(float)));
+    GPU_CHECK(cudaMemcpy(dev_points, in_points_array,
+                        in_num_points * kNumPointFeature * sizeof(float),
+                        cudaMemcpyHostToDevice));
+    
+    // [STEP 2] : preprocess
+    host_pillar_count_[0] = 0;
+    auto preprocess_start = std::chrono::high_resolution_clock::now();
+    preprocess_points_cuda_ptr_->DoPreprocessPointsCuda(
+          dev_points, in_num_points, dev_x_coors_, dev_y_coors_,
+          dev_num_points_per_pillar_, dev_pillar_point_feature_, dev_pillar_coors_,
+          dev_sparse_pillar_map_, host_pillar_count_ ,
+          dev_pfe_gather_feature_ );
+    cudaDeviceSynchronize();
+    auto preprocess_end = std::chrono::high_resolution_clock::now();
+    // DEVICE_SAVE<float>(dev_pfe_gather_feature_,  kMaxNumPillars * kMaxNumPointsPerPillar * kNumGatherPointFeature  , "0_Model_pfe_input_gather_feature");
+
+    // [STEP 3] : pfe forward
+    cudaStream_t stream;
+    GPU_CHECK(cudaStreamCreate(&stream));
+    auto pfe_start = std::chrono::high_resolution_clock::now();
+    GPU_CHECK(cudaMemcpyAsync(pfe_buffers_[0], dev_pfe_gather_feature_,
+                            kMaxNumPillars * kMaxNumPointsPerPillar * kNumGatherPointFeature * sizeof(float), ///kNumGatherPointFeature
+                            cudaMemcpyDeviceToDevice, stream));
+    pfe_context_->enqueueV2(pfe_buffers_, stream, nullptr);
+    cudaDeviceSynchronize();
+    auto pfe_end = std::chrono::high_resolution_clock::now();
+    // DEVICE_SAVE<float>(reinterpret_cast<float*>(pfe_buffers_[1]),  kMaxNumPillars * 64 , "1_Model_pfe_output_buffers_[1]");
+
+    // [STEP 4] : scatter pillar feature
+    auto scatter_start = std::chrono::high_resolution_clock::now();
+    scatter_cuda_ptr_->DoScatterCuda(
+        host_pillar_count_[0], dev_x_coors_, dev_y_coors_,
+        reinterpret_cast<float*>(pfe_buffers_[1]), dev_scattered_feature_);
+    cudaDeviceSynchronize();
+    auto scatter_end = std::chrono::high_resolution_clock::now();   
+    // DEVICE_SAVE<float>(dev_scattered_feature_ ,  kRpnInputSize,"2_Model_backbone_input_dev_scattered_feature");
+
+    // [STEP 5] : backbone forward
+    auto backbone_start = std::chrono::high_resolution_clock::now();
+    GPU_CHECK(cudaMemcpyAsync(rpn_buffers_[0], dev_scattered_feature_,
+                            kBatchSize * kRpnInputSize * sizeof(float),
+                            cudaMemcpyDeviceToDevice, stream));
+    backbone_context_->enqueueV2(rpn_buffers_, stream, nullptr);
+    cudaDeviceSynchronize();
+    auto backbone_end = std::chrono::high_resolution_clock::now();
+    // DEVICE_SAVE<float>(reinterpret_cast<float*>(rpn_buffers_[1]) ,  kNumAnchorPerCls    ,"3_rpn_buffers_[1]");
+    // DEVICE_SAVE<float>(reinterpret_cast<float*>(rpn_buffers_[2]) ,  kNumAnchorPerCls * 4,"3_rpn_buffers_[2]");
+    // DEVICE_SAVE<float>(reinterpret_cast<float*>(rpn_buffers_[3]) ,  kNumAnchorPerCls * 4,"3_rpn_buffers_[3]");
+    // DEVICE_SAVE<float>(reinterpret_cast<float*>(rpn_buffers_[4]) ,  kNumAnchorPerCls    ,"3_rpn_buffers_[4]");
+    // DEVICE_SAVE<float>(reinterpret_cast<float*>(rpn_buffers_[5]) ,  kNumAnchorPerCls * 4,"3_rpn_buffers_[5]");
+    // DEVICE_SAVE<float>(reinterpret_cast<float*>(rpn_buffers_[6]) ,  kNumAnchorPerCls * 4,"3_rpn_buffers_[6]");
+    // DEVICE_SAVE<float>(reinterpret_cast<float*>(rpn_buffers_[7]) ,  kNumAnchorPerCls * kNumClass * 9 ,"3_rpn_buffers_[7]");
+
+    // [STEP 6]: postprocess (multihead)
+    auto postprocess_start = std::chrono::high_resolution_clock::now();
+    GPU_CHECK(cudaMemset(dev_filter_count_, 0, kNumClass * sizeof(int)));
+    postprocess_cuda_ptr_->DoPostprocessCuda(
+        reinterpret_cast<float*>(rpn_buffers_[1]), // [cls]   kNumAnchorPerCls 
+        reinterpret_cast<float*>(rpn_buffers_[2]), // [cls]   kNumAnchorPerCls * 2 * 2
+        reinterpret_cast<float*>(rpn_buffers_[3]), // [cls]   kNumAnchorPerCls * 2 * 2
+        reinterpret_cast<float*>(rpn_buffers_[4]), // [cls]   kNumAnchorPerCls 
+        reinterpret_cast<float*>(rpn_buffers_[5]), // [cls]   kNumAnchorPerCls * 2 * 2
+        reinterpret_cast<float*>(rpn_buffers_[6]), // [cls]   kNumAnchorPerCls * 2 * 2
+        reinterpret_cast<float*>(rpn_buffers_[7]), // [boxes] kNumAnchorPerCls * kNumClass * kNumOutputBoxFeature
+        dev_filtered_box_, dev_filtered_score_, dev_filter_count_,
+        *out_detections, *out_labels , *out_scores);
+    cudaDeviceSynchronize();
+    auto postprocess_end = std::chrono::high_resolution_clock::now();
+
+    // release the stream and the buffers
+//    std::chrono::duration<double> preprocess_cost = preprocess_end - preprocess_start;
+//    std::chrono::duration<double> pfe_cost = pfe_end - pfe_start;
+//    std::chrono::duration<double> scatter_cost = scatter_end - scatter_start;
+//    std::chrono::duration<double> backbone_cost = backbone_end - backbone_start;
+//    std::chrono::duration<double> postprocess_cost = postprocess_end - postprocess_start;
+
+//    std::chrono::duration<double> pointpillars_cost = postprocess_end - preprocess_start;
+//    std::cout << "------------------------------------" << std::endl;
+//    std::cout << setiosflags(ios::left)  << setw(14) << "Module" << setw(12)  << "Time"  << resetiosflags(ios::left) << std::endl;
+//    std::cout << "------------------------------------" << std::endl;
+//    std::string Modules[] = {"Preprocess" , "Pfe" , "Scatter" , "Backbone" , "Postprocess" , "Summary"};
+//    double Times[] = {preprocess_cost.count() , pfe_cost.count() , scatter_cost.count() , backbone_cost.count() , postprocess_cost.count() , pointpillars_cost.count()};
+
+//    for (int i =0 ; i < 6 ; ++i) {
+//        std::cout << setiosflags(ios::left) << setw(14) << Modules[i]  << setw(8)  << Times[i] * 1000 << " ms" << resetiosflags(ios::left) << std::endl;
+//    }
+//    std::cout << "------------------------------------" << std::endl;
+    cudaStreamDestroy(stream);
+
+}

src/detection/detection_lidar_PointPillars_MultiHead_1025/pointpillars.h

@@ -0,0 +1,287 @@
+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @author Kosuke Murakami
+ * @date 2019/02/26
+ */
+
+/**
+* @author Yan haixu
+* Contact: just github.com/hova88
+* @date 2021/04/30
+*/
+
+
+#pragma once
+
+// headers in STL
+#include <algorithm>
+#include <cmath>
+#include <iomanip>
+#include <limits>
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+#include <iostream>
+#include <sstream>
+#include <fstream>
+// headers in TensorRT
+#include "NvInfer.h"
+#include "NvOnnxParser.h"
+
+// headers in local files
+// #include "params.h"
+#include "common.h"
+#include <yaml-cpp/yaml.h>
+#include "preprocess.h"
+#include "scatter.h"
+#include "postprocess.h"
+
+using namespace std;
+
+// Logger for TensorRT info/warning/errors
+class Logger : public nvinfer1::ILogger {
+ public:
+  explicit Logger(Severity severity = Severity::kWARNING)
+      : reportable_severity(severity) {}
+
+  void log(Severity severity, const char* msg) override {
+    // suppress messages with severity enum value greater than the reportable
+    if (severity > reportable_severity) return;
+
+    switch (severity) {
+      case Severity::kINTERNAL_ERROR:
+        std::cerr << "INTERNAL_ERROR: ";
+        break;
+      case Severity::kERROR:
+        std::cerr << "ERROR: ";
+        break;
+      case Severity::kWARNING:
+        std::cerr << "WARNING: ";
+        break;
+      case Severity::kINFO:
+        std::cerr << "INFO: ";
+        break;
+      default:
+        std::cerr << "UNKNOWN: ";
+        break;
+    }
+    std::cerr << msg << std::endl;
+  }
+
+  Severity reportable_severity;
+};
+
+
+
+class PointPillars {
+ private:
+    // initialize in initializer list
+    const float score_threshold_;
+    const float nms_overlap_threshold_;
+    const bool use_onnx_;
+    const std::string pfe_file_;
+    const std::string backbone_file_;
+    const std::string pp_config_;
+    // end initializer list
+    // voxel size
+    float kPillarXSize;
+    float kPillarYSize;
+    float kPillarZSize;
+    // point cloud range
+    float kMinXRange;
+    float kMinYRange;
+    float kMinZRange;
+    float kMaxXRange;
+    float kMaxYRange;
+    float kMaxZRange;
+    // hyper parameters
+    int kNumClass;
+    int kMaxNumPillars;
+    int kMaxNumPointsPerPillar;
+    int kNumPointFeature;
+    int kNumGatherPointFeature = 11;
+    int kGridXSize;
+    int kGridYSize;
+    int kGridZSize;
+    int kNumAnchorXinds;
+    int kNumAnchorYinds;
+    int kRpnInputSize;
+    int kNumAnchor;
+    int kNumInputBoxFeature;
+    int kNumOutputBoxFeature;
+    int kRpnBoxOutputSize;
+    int kRpnClsOutputSize;
+    int kRpnDirOutputSize;
+    int kBatchSize;
+    int kNumIndsForScan;
+    int kNumThreads;
+    // if you change kNumThreads, need to modify NUM_THREADS_MACRO in
+    // common.h
+    int kNumBoxCorners;
+    int kNmsPreMaxsize;
+    int kNmsPostMaxsize;
+    //params for initialize anchors
+    //Adapt to OpenPCDet
+    int kAnchorStrides;
+    std::vector<string> kAnchorNames;
+    std::vector<float> kAnchorDxSizes;
+    std::vector<float> kAnchorDySizes;
+    std::vector<float> kAnchorDzSizes;
+    std::vector<float> kAnchorBottom;
+    std::vector<std::vector<int>> kMultiheadLabelMapping;
+    int kNumAnchorPerCls;
+    int host_pillar_count_[1];
+
+    int* dev_x_coors_;
+    int* dev_y_coors_;
+    float* dev_num_points_per_pillar_;
+    int* dev_sparse_pillar_map_;
+    int* dev_cumsum_along_x_;
+    int* dev_cumsum_along_y_;
+
+    float* dev_pillar_point_feature_;
+    float* dev_pillar_coors_;
+    float* dev_points_mean_;
+
+    float* dev_pfe_gather_feature_;
+    void* pfe_buffers_[2];
+    //variable for doPostprocessCudaMultiHead
+    void* rpn_buffers_[8];
+    
+    std::vector<float*> rpn_box_output_; 
+    std::vector<float*> rpn_cls_output_;
+
+    float* dev_scattered_feature_;
+
+    float* dev_filtered_box_;
+    float* dev_filtered_score_;
+    int*   dev_filtered_label_;
+    int*   dev_filtered_dir_;
+    float* dev_box_for_nms_;
+    int*   dev_filter_count_;
+
+    std::unique_ptr<PreprocessPointsCuda> preprocess_points_cuda_ptr_;
+    std::unique_ptr<ScatterCuda> scatter_cuda_ptr_;
+    std::unique_ptr<PostprocessCuda> postprocess_cuda_ptr_;
+
+    Logger g_logger_;
+    nvinfer1::ICudaEngine* pfe_engine_;
+    nvinfer1::ICudaEngine* backbone_engine_;
+    nvinfer1::IExecutionContext* pfe_context_;
+    nvinfer1::IExecutionContext* backbone_context_;
+
+    /**
+     * @brief Memory allocation for device memory
+     * @details Called in the constructor
+     */
+    void DeviceMemoryMalloc();
+
+    /**
+     * @brief Memory set to 0 for device memory
+     * @details Called in the DoInference
+     */
+    void SetDeviceMemoryToZero();
+
+    /**
+     * @brief Initializing paraments from pointpillars.yaml
+     * @details Called in the constructor
+     */
+    void InitParams();
+    /**
+     * @brief Initializing TensorRT instances
+     * @param[in] usr_onnx_ if true, parse ONNX 
+     * @details Called in the constructor
+     */
+    void InitTRT(const bool use_onnx);
+    void SaveEngine(const nvinfer1::ICudaEngine* engine, const std::string& engine_filepath);
+    /**
+     * @brief Convert ONNX to TensorRT model
+     * @param[in] model_file ONNX model file path
+     * @param[out] engine_ptr TensorRT model engine made out of ONNX model
+     * @details Load ONNX model, and convert it to TensorRT model
+     */
+    void OnnxToTRTModel(const std::string& model_file,
+                        nvinfer1::ICudaEngine** engine_ptr);
+
+    /**
+     * @brief Convert Engine to TensorRT model
+     * @param[in] model_file Engine(TensorRT) model file path
+     * @param[out] engine_ptr TensorRT model engine made 
+     * @details Load Engine model, and convert it to TensorRT model
+     */
+    void EngineToTRTModel(const std::string &engine_file ,     
+                        nvinfer1::ICudaEngine** engine_ptr) ;
+
+    /**
+     * @brief Preproces points
+     * @param[in] in_points_array Point cloud array
+     * @param[in] in_num_points Number of points
+     * @details Call CPU or GPU preprocess
+     */
+    void Preprocess(const float* in_points_array, const int in_num_points);
+
+    public:
+    /**
+     * @brief Constructor
+     * @param[in] score_threshold Score threshold for filtering output
+     * @param[in] nms_overlap_threshold IOU threshold for NMS
+     * @param[in] use_onnx if true,using onnx file ,else using engine file
+     * @param[in] pfe_file Pillar Feature Extractor ONNX file path
+     * @param[in] rpn_file Region Proposal Network ONNX file path
+     * @details Variables could be changed through point_pillars_detection
+     */
+    PointPillars(const float score_threshold,
+                const float nms_overlap_threshold,
+                const bool use_onnx,
+                const std::string pfe_file,
+                const std::string rpn_file,
+                const std::string pp_config);
+    ~PointPillars();
+
+    /**
+     * @brief Call PointPillars for the inference
+     * @param[in] in_points_array Point cloud array
+     * @param[in] in_num_points Number of points
+     * @param[out] out_detections Network output bounding box
+     * @param[out] out_labels Network output object's label
+     * @details This is an interface for the algorithm
+     */
+    void DoInference(const float* in_points_array,
+                    const int in_num_points,
+                    std::vector<float>* out_detections,
+                    std::vector<int>* out_labels,
+                    std::vector<float>* out_scores);
+};
+

src/detection/detection_lidar_PointPillars_MultiHead_1025/postprocess.cu

@@ -0,0 +1,383 @@
+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @author Kosuke Murakami
+ * @date 2019/02/26
+ */
+
+/**
+* @author Yan haixu
+* Contact: just github.com/hova88
+* @date 2021/04/30
+*/
+
+
+// headers in CUDA
+#include <thrust/sort.h>
+
+// headers in local files
+#include "common.h"
+#include "postprocess.h"
+#include <stdio.h>
+
+
+// sigmoid_filter_warp
+__device__ void box_decode_warp(int head_offset, const float* box_pred, 
+    int tid , int num_anchors_per_head , int counter, float* filtered_box) 
+{
+    filtered_box[blockIdx.z * num_anchors_per_head * 7  + counter * 7 + 0] = box_pred[ head_offset + tid * 9 + 0];
+    filtered_box[blockIdx.z * num_anchors_per_head * 7  + counter * 7 + 1] = box_pred[ head_offset + tid * 9 + 1];
+    filtered_box[blockIdx.z * num_anchors_per_head * 7  + counter * 7 + 2] = box_pred[ head_offset + tid * 9 + 2];
+    filtered_box[blockIdx.z * num_anchors_per_head * 7  + counter * 7 + 3] = box_pred[ head_offset + tid * 9 + 3];
+    filtered_box[blockIdx.z * num_anchors_per_head * 7  + counter * 7 + 4] = box_pred[ head_offset + tid * 9 + 4];
+    filtered_box[blockIdx.z * num_anchors_per_head * 7  + counter * 7 + 5] = box_pred[ head_offset + tid * 9 + 5];
+    filtered_box[blockIdx.z * num_anchors_per_head * 7  + counter * 7 + 6] = box_pred[ head_offset + tid * 9 + 6];
+}
+
+
+__global__ void sigmoid_filter_kernel(
+
+    float* cls_pred_0,
+    float* cls_pred_12,
+    float* cls_pred_34,
+    float* cls_pred_5,
+    float* cls_pred_67,
+    float* cls_pred_89,
+
+    const float* box_pred_0,
+
+    const float* box_pred_1,
+    const float* box_pred_2,
+
+    const float* box_pred_3,
+    const float* box_pred_4,
+
+    const float* box_pred_5,
+
+    const float* box_pred_6,
+    const float* box_pred_7,
+
+    const float* box_pred_8,
+    const float* box_pred_9,
+
+    float* filtered_box, 
+    float* filtered_score, 
+    int* filter_count,
+
+    const float score_threshold) {   
+
+    // cls_pred_34 
+    // 32768*2 , 2
+
+    int num_anchors_per_head = gridDim.x * gridDim.y * blockDim.x;
+    // 16 * 4 * 512 = 32768
+    extern __shared__ float cls_score[];
+    cls_score[threadIdx.x + blockDim.x] = -1.0f;
+
+    int tid = blockIdx.x * gridDim.y * blockDim.x + blockIdx.y *  blockDim.x + threadIdx.x; 
+
+
+    if ( blockIdx.z == 0) cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_0[ tid ]));
+    if ( blockIdx.z == 1) {
+        cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_12[ tid * 2 ]));
+        cls_score[ threadIdx.x + blockDim.x] = 1 / (1 + expf(-cls_pred_12[ (num_anchors_per_head + tid) * 2]));}
+    if ( blockIdx.z == 2) {
+        cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_12[ tid * 2 + 1]));
+        cls_score[ threadIdx.x + blockDim.x] = 1 / (1 + expf(-cls_pred_12[ (num_anchors_per_head + tid) * 2 + 1]));}
+
+    if ( blockIdx.z == 3) {
+        cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_34[ tid * 2 ]));
+        cls_score[ threadIdx.x + blockDim.x] = 1 / (1 + expf(-cls_pred_34[ (num_anchors_per_head + tid) * 2]));}
+    if ( blockIdx.z == 4) {
+        cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_34[ tid * 2 + 1 ]));
+        cls_score[ threadIdx.x + blockDim.x] = 1 / (1 + expf(-cls_pred_34[ (num_anchors_per_head + tid) * 2 + 1]));}
+
+    if ( blockIdx.z == 5) cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_5[ tid ]));
+
+    if ( blockIdx.z == 6) {
+        cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_67[ tid * 2 ]));
+        cls_score[ threadIdx.x + blockDim.x] = 1 / (1 + expf(-cls_pred_67[ (num_anchors_per_head + tid) * 2]));}
+    if ( blockIdx.z == 7) {
+        cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_67[ tid * 2 + 1 ]));
+        cls_score[ threadIdx.x + blockDim.x] = 1 / (1 + expf(-cls_pred_67[ (num_anchors_per_head + tid) * 2 + 1]));}
+
+    if ( blockIdx.z == 8) {
+        cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_89[ tid * 2 ]));
+        cls_score[ threadIdx.x + blockDim.x] = 1 / (1 + expf(-cls_pred_89[ (num_anchors_per_head + tid) * 2]));}
+    if ( blockIdx.z == 9) {
+        cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_89[ tid * 2 + 1 ]));
+        cls_score[ threadIdx.x + blockDim.x] = 1 / (1 + expf(-cls_pred_89[ (num_anchors_per_head + tid) * 2 + 1]));}
+    
+    __syncthreads();
+    
+    if( cls_score[ threadIdx.x ] > score_threshold) 
+    {
+        int counter = atomicAdd(&filter_count[blockIdx.z], 1);
+        if ( blockIdx.z == 0) {
+            box_decode_warp(0 ,box_pred_0 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else
+        if ( blockIdx.z == 1) {
+            box_decode_warp(0 ,box_pred_1 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else
+        if ( blockIdx.z == 2) {
+            box_decode_warp(0 ,box_pred_1 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else
+        if ( blockIdx.z == 3) {
+            box_decode_warp(0 ,box_pred_3 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else 
+        if (blockIdx.z == 4) {
+            box_decode_warp(0 ,box_pred_3 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];            
+        }else
+        if ( blockIdx.z == 5) {
+            box_decode_warp(0 ,box_pred_5 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else
+        if ( blockIdx.z == 6) {
+            box_decode_warp(0 ,box_pred_6 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else
+        if ( blockIdx.z == 7) {
+            box_decode_warp(0 ,box_pred_6 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else
+        if ( blockIdx.z == 8) {
+
+            box_decode_warp(0 ,box_pred_8 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else
+        if ( blockIdx.z == 9) {
+            box_decode_warp(0 ,box_pred_8 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }
+    }
+    __syncthreads();  
+    if( cls_score[ threadIdx.x + blockDim.x ] > score_threshold)  {     
+            int counter = atomicAdd(&filter_count[blockIdx.z], 1);
+            // printf("counter : %d \n" , counter);
+            if (blockIdx.z == 1) {
+                box_decode_warp(0 ,box_pred_2 , tid , num_anchors_per_head , counter , filtered_box);
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+            }else 
+            if (blockIdx.z == 2) {
+                box_decode_warp(0 ,box_pred_2 , tid , num_anchors_per_head , counter , filtered_box);
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+            }else 
+            if (blockIdx.z == 3) {
+                box_decode_warp(0 ,box_pred_4 , tid , num_anchors_per_head , counter , filtered_box);
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+            }else 
+            if (blockIdx.z == 4) {
+                box_decode_warp(0 ,box_pred_4 , tid , num_anchors_per_head , counter , filtered_box);
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+            }else 
+            if (blockIdx.z == 6) {
+                box_decode_warp(0 ,box_pred_7 , tid , num_anchors_per_head , counter , filtered_box);
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+            }else 
+            if (blockIdx.z == 7) {
+                box_decode_warp(0 ,box_pred_7 , tid , num_anchors_per_head , counter , filtered_box);
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+            }else 
+            if (blockIdx.z == 8) {
+                box_decode_warp(0 ,box_pred_9 , tid , num_anchors_per_head , counter , filtered_box);
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+            }else 
+            if (blockIdx.z == 9) {
+                box_decode_warp(0 ,box_pred_9 , tid , num_anchors_per_head , counter , filtered_box);
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+            }
+    }
+}
+
+__global__ void sort_boxes_by_indexes_kernel(float* filtered_box, float* filtered_scores, int* indexes, int filter_count,
+    float* sorted_filtered_boxes, float* sorted_filtered_scores,
+    const int num_output_box_feature)
+{
+    int tid = threadIdx.x + blockIdx.x * blockDim.x;
+    if(tid < filter_count)  {
+
+        int sort_index = indexes[tid];
+        sorted_filtered_boxes[tid * num_output_box_feature + 0] = filtered_box[sort_index * num_output_box_feature + 0];
+        sorted_filtered_boxes[tid * num_output_box_feature + 1] = filtered_box[sort_index * num_output_box_feature + 1];
+        sorted_filtered_boxes[tid * num_output_box_feature + 2] = filtered_box[sort_index * num_output_box_feature + 2];
+        sorted_filtered_boxes[tid * num_output_box_feature + 3] = filtered_box[sort_index * num_output_box_feature + 3];
+        sorted_filtered_boxes[tid * num_output_box_feature + 4] = filtered_box[sort_index * num_output_box_feature + 4];
+        sorted_filtered_boxes[tid * num_output_box_feature + 5] = filtered_box[sort_index * num_output_box_feature + 5];
+        sorted_filtered_boxes[tid * num_output_box_feature + 6] = filtered_box[sort_index * num_output_box_feature + 6];
+
+        // sorted_filtered_dir[tid] = filtered_dir[sort_index];
+        sorted_filtered_scores[tid] = filtered_scores[sort_index];
+    }
+}
+
+
+
+PostprocessCuda::PostprocessCuda(const int num_threads, const float float_min, const float float_max,
+    const int num_class,const int num_anchor_per_cls,
+    const std::vector<std::vector<int>> multihead_label_mapping,
+    const float score_threshold,  const float nms_overlap_threshold, 
+    const int nms_pre_maxsize, const int nms_post_maxsize,
+    const int num_box_corners, 
+    const int num_input_box_feature,
+    const int num_output_box_feature)
+: num_threads_(num_threads),
+  float_min_(float_min),
+  float_max_(float_max),
+  num_class_(num_class),
+  num_anchor_per_cls_(num_anchor_per_cls),
+  multihead_label_mapping_(multihead_label_mapping),
+  score_threshold_(score_threshold),
+  nms_overlap_threshold_(nms_overlap_threshold),
+  nms_pre_maxsize_(nms_pre_maxsize),
+  nms_post_maxsize_(nms_post_maxsize),
+  num_box_corners_(num_box_corners),
+  num_input_box_feature_(num_input_box_feature),
+  num_output_box_feature_(num_output_box_feature) {
+    nms_cuda_ptr_.reset(
+    new NmsCuda(num_threads_, num_box_corners_, nms_overlap_threshold_));
+
+}
+
+
+void PostprocessCuda::DoPostprocessCuda(
+    float* cls_pred_0,
+    float* cls_pred_12,
+    float* cls_pred_34,
+    float* cls_pred_5,
+    float* cls_pred_67,
+    float* cls_pred_89,
+
+    const float* box_preds,
+   
+    float* dev_filtered_box, 
+    float* dev_filtered_score, 
+    int* dev_filter_count,
+    std::vector<float>& out_detection, std::vector<int>& out_label , std::vector<float>& out_score) {
+    // 在此之前，先进行rpn_box_output的concat. 
+    // 128x128 的feature map， cls_pred 的shape为（32768，1），（32768,1），（32768,1），（65536,2），（32768，1）
+    dim3 gridsize(16, 4 , 10);  //16 *  4  * 512  = 32768 代表一个head的anchors
+    sigmoid_filter_kernel<<< gridsize, 512 , 512 * 2 * sizeof(float)>>>(
+        cls_pred_0,
+        cls_pred_12, 
+        cls_pred_34, 
+        cls_pred_5, 
+        cls_pred_67, 
+        cls_pred_89,
+
+        &box_preds[0 * 32768 * 9],
+        &box_preds[1 * 32768 * 9],
+        &box_preds[2 * 32768 * 9],
+        &box_preds[3 * 32768 * 9],
+        &box_preds[4 * 32768 * 9],
+        &box_preds[5 * 32768 * 9],
+        &box_preds[6 * 32768 * 9],
+        &box_preds[7 * 32768 * 9],
+        &box_preds[8 * 32768 * 9],
+        &box_preds[9 * 32768 * 9],
+
+        dev_filtered_box, 
+        dev_filtered_score,  
+        dev_filter_count, 
+    
+        score_threshold_);
+    cudaDeviceSynchronize();
+    
+    int host_filter_count[num_class_] = {0};
+    GPU_CHECK(cudaMemcpy(host_filter_count, dev_filter_count, num_class_ * sizeof(int), cudaMemcpyDeviceToHost));
+    
+    for (int i = 0; i < num_class_; ++ i) {
+        if(host_filter_count[i] <= 0) continue;
+
+        int* dev_indexes;
+        float* dev_sorted_filtered_box;
+        float* dev_sorted_filtered_scores;
+        GPU_CHECK(cudaMalloc((void**)&dev_indexes, host_filter_count[i] * sizeof(int)));
+        GPU_CHECK(cudaMalloc((void**)&dev_sorted_filtered_box, host_filter_count[i] * num_output_box_feature_ * sizeof(float)));
+        GPU_CHECK(cudaMalloc((void**)&dev_sorted_filtered_scores, host_filter_count[i]*sizeof(float)));
+        // GPU_CHECK(cudaMalloc((void**)&dev_sorted_box_for_nms, NUM_BOX_CORNERS_*host_filter_count[i]*sizeof(float)));
+        thrust::sequence(thrust::device, dev_indexes, dev_indexes + host_filter_count[i]);
+        thrust::sort_by_key(thrust::device, 
+                            &dev_filtered_score[i * num_anchor_per_cls_], 
+                            &dev_filtered_score[i * num_anchor_per_cls_ + host_filter_count[i]],
+                            dev_indexes, 
+                            thrust::greater<float>());
+
+        const int num_blocks = DIVUP(host_filter_count[i], num_threads_);
+
+        sort_boxes_by_indexes_kernel<<<num_blocks, num_threads_>>>(
+            &dev_filtered_box[i * num_anchor_per_cls_ * num_output_box_feature_], 
+            &dev_filtered_score[i * num_anchor_per_cls_], 
+            dev_indexes, 
+            host_filter_count[i],
+            dev_sorted_filtered_box, 
+            dev_sorted_filtered_scores,
+            num_output_box_feature_);
+
+        int num_box_for_nms = min(nms_pre_maxsize_, host_filter_count[i]);
+        long* keep_inds = new long[num_box_for_nms];  // index of kept box
+        memset(keep_inds, 0, num_box_for_nms * sizeof(int));
+        int num_out = 0;
+        nms_cuda_ptr_->DoNmsCuda(num_box_for_nms, dev_sorted_filtered_box, keep_inds, &num_out);
+
+        num_out = min(num_out, nms_post_maxsize_);
+
+        float* host_filtered_box = new float[host_filter_count[i] * num_output_box_feature_]();
+        float* host_filtered_scores = new float[host_filter_count[i]]();
+
+
+        cudaMemcpy(host_filtered_box, dev_sorted_filtered_box, host_filter_count[i] * num_output_box_feature_ * sizeof(float), cudaMemcpyDeviceToHost);
+        cudaMemcpy(host_filtered_scores, dev_sorted_filtered_scores, host_filter_count[i] * sizeof(float), cudaMemcpyDeviceToHost);
+        for (int j = 0; j < num_out; ++j)  {
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 0]);
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 1]);
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 2]);
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 3]);
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 4]);
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 5]);
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 6]);
+            out_score.emplace_back(host_filtered_scores[keep_inds[j]]);
+            out_label.emplace_back(i);
+        }
+        delete[] keep_inds;
+        delete[] host_filtered_scores;
+        delete[] host_filtered_box;
+
+        GPU_CHECK(cudaFree(dev_indexes));
+        GPU_CHECK(cudaFree(dev_sorted_filtered_box));
+    }
+}

src/detection/detection_lidar_PointPillars_MultiHead_1025/postprocess.h

@@ -0,0 +1,125 @@
+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @author Kosuke Murakami
+ * @date 2019/02/26
+ */
+
+/**
+* @author Yan haixu
+* Contact: just github.com/hova88
+* @date 2021/04/30
+*/
+
+#pragma once
+#include <memory>
+#include <vector>
+#include "nms.h"
+
+class PostprocessCuda {
+ private:
+    // initializer list
+
+    const int num_threads_;
+    const float float_min_;
+    const float float_max_;
+    const int num_class_;
+    const int num_anchor_per_cls_;
+    const float score_threshold_;
+    const float nms_overlap_threshold_;
+    const int nms_pre_maxsize_;
+    const int nms_post_maxsize_;
+    const int num_box_corners_;
+    const int num_input_box_feature_;
+    const int num_output_box_feature_;
+    const std::vector<std::vector<int>> multihead_label_mapping_;
+    // end initializer list
+
+    std::unique_ptr<NmsCuda> nms_cuda_ptr_;
+  public:
+  /**
+   * @brief Constructor
+   * @param[in] num_threads Number of threads when launching cuda kernel
+   * @param[in] float_min The lowest float value
+   * @param[in] float_max The maximum float value
+   * @param[in] num_class Number of classes 
+   * @param[in] num_anchor_per_cls Number anchor per category
+   * @param[in] multihead_label_mapping 
+   * @param[in] score_threshold Score threshold for filtering output
+   * @param[in] nms_overlap_threshold IOU threshold for NMS
+   * @param[in] nms_pre_maxsize Maximum number of boxes into NMS
+   * @param[in] nms_post_maxsize Maximum number of boxes after NMS
+   * @param[in] num_box_corners Number of box's corner
+   * @param[in] num_output_box_feature Number of output box's feature
+   * @details Captital variables never change after the compile, non-capital
+   * variables could be changed through rosparam
+   */
+  PostprocessCuda(const int num_threads, 
+                  const float float_min, const float float_max,
+                  const int num_class, const int num_anchor_per_cls, 
+                  const std::vector<std::vector<int>> multihead_label_mapping,
+                  const float score_threshold,  
+                  const float nms_overlap_threshold, 
+                  const int nms_pre_maxsize, 
+                  const int nms_post_maxsize,
+                  const int num_box_corners,
+                  const int num_input_box_feature, 
+                  const int num_output_box_feature);
+  ~PostprocessCuda(){}
+
+  /**
+   * @brief Postprocessing for the network output
+   * @param[in] rpn_box_output Box predictions from the network output
+   * @param[in] rpn_cls_output Class predictions from the network output
+   * @param[in] rpn_dir_output Direction predictions from the network output
+   * @param[in] dev_filtered_box Filtered box predictions
+   * @param[in] dev_filtered_score Filtered score predictions
+   * @param[in] dev_filter_count The number of filtered output
+   * @param[out] out_detection Output bounding boxes
+   * @param[out] out_label Output labels of objects
+   * @details dev_* represents device memory allocated variables
+   */
+  void DoPostprocessCuda(
+    float* cls_pred_0,
+    float* cls_pred_12,
+    float* cls_pred_34,
+    float* cls_pred_5,
+    float* cls_pred_67,
+    float* cls_pred_89,
+    
+    const float* box_preds,
+    float* dev_filtered_box, 
+    float* dev_filtered_score, 
+    int* dev_filter_count,
+    std::vector<float>& out_detection, std::vector<int>& out_label , std::vector<float>& out_score);
+};

src/detection/detection_lidar_PointPillars_MultiHead_1025/preprocess.cu

@@ -0,0 +1,410 @@
+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @author Kosuke Murakami
+ * @date 2019/02/26
+ */
+
+/**
+* @author Yan haixu
+* Contact: just github.com/hova88
+* @date 2021/04/30
+*/
+
+
+
+// headers in STL
+#include <stdio.h>
+
+// headers in local files
+#include "common.h"
+#include "preprocess.h"
+
+__global__ void make_pillar_histo_kernel(
+    const float* dev_points, float* dev_pillar_point_feature_in_coors,
+    int* pillar_count_histo, const int num_points,
+    const int max_points_per_pillar, const int grid_x_size,
+    const int grid_y_size, const int grid_z_size, const float min_x_range,
+    const float min_y_range, const float min_z_range, const float pillar_x_size,
+    const float pillar_y_size, const float pillar_z_size,
+    const int num_point_feature) {
+  int th_i = blockIdx.x * blockDim.x +  threadIdx.x ;
+  if (th_i >= num_points) {
+    return;
+  }
+  int x_coor = floor((dev_points[th_i * num_point_feature + 0] - min_x_range) / pillar_x_size);
+  int y_coor = floor((dev_points[th_i * num_point_feature + 1] - min_y_range) / pillar_y_size);
+  int z_coor = floor((dev_points[th_i * num_point_feature + 2] - min_z_range) / pillar_z_size);
+
+  if (x_coor >= 0 && x_coor < grid_x_size && y_coor >= 0 &&
+      y_coor < grid_y_size && z_coor >= 0 && z_coor < grid_z_size) {
+    int count =
+        atomicAdd(&pillar_count_histo[y_coor * grid_x_size + x_coor], 1);
+    if (count < max_points_per_pillar) {
+      int ind =
+          y_coor * grid_x_size * max_points_per_pillar * num_point_feature +
+          x_coor * max_points_per_pillar * num_point_feature +
+          count * num_point_feature;
+ 
+      for (int i = 0; i < num_point_feature; ++i) {
+        dev_pillar_point_feature_in_coors[ind + i] =
+            dev_points[th_i * num_point_feature + i];
+      }
+    }
+  }
+}
+
+__global__ void make_pillar_index_kernel(
+    int* dev_pillar_count_histo, int* dev_counter, int* dev_pillar_count,
+    int* dev_x_coors, int* dev_y_coors, float* dev_num_points_per_pillar,
+    int* dev_sparse_pillar_map, const int max_pillars,
+    const int max_points_per_pillar, const int grid_x_size,
+    const int num_inds_for_scan) {
+  int x = blockIdx.x;
+  int y = threadIdx.x;
+  int num_points_at_this_pillar = dev_pillar_count_histo[y * grid_x_size + x];
+  if (num_points_at_this_pillar == 0) {
+    return;
+  }
+
+  int count = atomicAdd(dev_counter, 1);
+  if (count < max_pillars) {
+    atomicAdd(dev_pillar_count, 1);
+    if (num_points_at_this_pillar >= max_points_per_pillar) {
+      dev_num_points_per_pillar[count] = max_points_per_pillar;
+    } else {
+      dev_num_points_per_pillar[count] = num_points_at_this_pillar;
+    }
+    dev_x_coors[count] = x;
+    dev_y_coors[count] = y;
+    dev_sparse_pillar_map[y * num_inds_for_scan + x] = 1;
+  }
+}
+
+__global__ void make_pillar_feature_kernel(
+    float* dev_pillar_point_feature_in_coors, float* dev_pillar_point_feature,
+    float* dev_pillar_coors, int* dev_x_coors, int* dev_y_coors,
+    float* dev_num_points_per_pillar, const int max_points,
+    const int num_point_feature, const int grid_x_size) {
+  int ith_pillar = blockIdx.x;
+  int num_points_at_this_pillar = dev_num_points_per_pillar[ith_pillar];
+  int ith_point = threadIdx.x;
+  if (ith_point >= num_points_at_this_pillar) {
+    return;
+  }
+  int x_ind = dev_x_coors[ith_pillar];
+  int y_ind = dev_y_coors[ith_pillar];
+  int pillar_ind = ith_pillar * max_points * num_point_feature +
+                   ith_point * num_point_feature;
+  int coors_ind = y_ind * grid_x_size * max_points * num_point_feature +
+                  x_ind * max_points * num_point_feature +
+                  ith_point * num_point_feature;
+  #pragma unroll 
+  for (int i = 0; i < num_point_feature; ++i) {
+    dev_pillar_point_feature[pillar_ind + i] =
+        dev_pillar_point_feature_in_coors[coors_ind + i];
+  }
+
+  float coor_x = static_cast<float>(x_ind);
+  float coor_y = static_cast<float>(y_ind);
+  dev_pillar_coors[ith_pillar * 4 + 0] = 0;  // batch idx
+  dev_pillar_coors[ith_pillar * 4 + 1] = 0;  // z
+  dev_pillar_coors[ith_pillar * 4 + 2] = coor_y;
+  dev_pillar_coors[ith_pillar * 4 + 3] = coor_x;
+}
+
+
+
+__global__ void pillar_mean_kernel(
+  float* dev_points_mean, 
+  const int num_point_feature,
+  const float* dev_pillar_point_feature, 
+  const float* dev_num_points_per_pillar, 
+  int max_pillars , 
+  int max_points_per_pillar) {
+
+    extern __shared__ float temp[];
+    int ith_pillar = blockIdx.x; 
+    int ith_point  = threadIdx.x;
+    int axis = threadIdx.y;
+  
+    int reduce_size = max_points_per_pillar > 32 ? 64 : 32;
+    temp[threadIdx.x * 3 + axis] =  dev_pillar_point_feature[ith_pillar * max_points_per_pillar * num_point_feature + ith_point * num_point_feature + axis];  
+    if (threadIdx.x < reduce_size - max_points_per_pillar) {
+        temp[(threadIdx.x + max_points_per_pillar) * 3 + axis] = 0.0f; //--> dummy placeholds will set as 0
+    }
+    __syncthreads();
+    int num_points_at_this_pillar = dev_num_points_per_pillar[ith_pillar];
+
+    if (ith_point >= num_points_at_this_pillar) {
+          return;
+    }
+
+    for (unsigned int d = reduce_size >> 1 ; d > 0.6; d >>= 1) {
+        if (ith_point < d) {
+            temp[ith_point*3 +axis] += temp[(ith_point + d) * 3 + axis];
+        }
+        __syncthreads();
+    }
+
+    if (ith_point == 0) {
+        dev_points_mean[ith_pillar * 3 + axis] = temp[ith_point + axis] / num_points_at_this_pillar ;
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+__device__ void warpReduce(volatile float* sdata , int ith_point , int axis) {
+    sdata[ith_point * blockDim.y + axis] += sdata[(ith_point + 8) * blockDim.y + axis];
+    sdata[ith_point * blockDim.y + axis] += sdata[(ith_point + 4) * blockDim.y + axis];
+    sdata[ith_point * blockDim.y + axis] += sdata[(ith_point + 2) * blockDim.y + axis];
+    sdata[ith_point * blockDim.y + axis] += sdata[(ith_point + 1) * blockDim.y + axis];
+}
+
+
+
+
+
+__global__ void make_pillar_mean_kernel(
+  float* dev_points_mean, 
+  const int num_point_feature,
+  const float* dev_pillar_point_feature, 
+  const float* dev_num_points_per_pillar, 
+  int max_pillars , 
+  int max_points_pre_pillar) {
+    extern __shared__ float temp[];
+    unsigned int ith_pillar = blockIdx.x;  // { 0 , 1, 2, ... , 10000+}
+    unsigned int ith_point  = threadIdx.x; // { 0 , 1, 2, ...,9}
+    unsigned int axis = threadIdx.y; 
+    unsigned int idx_pre  = ith_pillar * max_points_pre_pillar * num_point_feature \
+                     + ith_point  * num_point_feature;
+    unsigned int idx_post = ith_pillar * max_points_pre_pillar * num_point_feature \
+                     + (ith_point + blockDim.x)  * num_point_feature;
+
+    temp[ith_point * blockDim.y + axis] = 0.0;
+    unsigned int num_points_at_this_pillar = dev_num_points_per_pillar[ith_pillar];
+
+    // if (ith_point < num_points_at_this_pillar / 2) {
+      temp[ith_point * blockDim.y + axis] = dev_pillar_point_feature[idx_pre  + axis] 
+                                          + dev_pillar_point_feature[idx_post + axis];
+    // }
+    __syncthreads();
+
+    // do reduction in shared mem
+    // Sequential addressing. This solves the bank conflicts as
+    // the threads now access shared memory with a stride of one
+    // 32-bit word (unsigned int) now, which does not cause bank 
+    // conflicts
+    warpReduce(temp , ith_point , axis);
+
+	// // write result for this block to global mem
+    if (ith_point == 0)
+    dev_points_mean[ith_pillar * blockDim.y + axis] = temp[ith_point * blockDim.y + axis] / num_points_at_this_pillar ;
+}
+
+
+__global__ void gather_point_feature_kernel(
+  const int max_num_pillars_,const int max_num_points_per_pillar,const int num_point_feature,
+  const float min_x_range, const float min_y_range, const float min_z_range, 
+  const float pillar_x_size,  const float pillar_y_size, const float pillar_z_size,
+  const float* dev_pillar_point_feature, const float* dev_num_points_per_pillar, 
+  const float* dev_pillar_coors,
+  float* dev_points_mean, 
+  float* dev_pfe_gather_feature_){
+
+  int ith_pillar = blockIdx.x; 
+  int ith_point = threadIdx.x;
+  // int kNumPointFeature = 5;
+  int num_gather_feature = 11;
+  int num_points_at_this_pillar = dev_num_points_per_pillar[ith_pillar];
+
+  if (ith_point >= num_points_at_this_pillar){
+        return;
+    }
+
+
+    dev_pfe_gather_feature_[ith_pillar * max_num_points_per_pillar * num_gather_feature + ith_point * num_gather_feature + 0] 
+    =  dev_pillar_point_feature[ith_pillar * max_num_points_per_pillar * num_point_feature + ith_point * num_point_feature + 0]; 
+  
+    dev_pfe_gather_feature_[ith_pillar * max_num_points_per_pillar * num_gather_feature + ith_point * num_gather_feature + 1]  
+    =  dev_pillar_point_feature[ith_pillar * max_num_points_per_pillar * num_point_feature + ith_point * num_point_feature + 1];
+  
+    dev_pfe_gather_feature_[ith_pillar * max_num_points_per_pillar * num_gather_feature + ith_point * num_gather_feature + 2]  
+    =  dev_pillar_point_feature[ith_pillar * max_num_points_per_pillar * num_point_feature + ith_point * num_point_feature + 2];
+  
+    dev_pfe_gather_feature_[ith_pillar * max_num_points_per_pillar * num_gather_feature + ith_point * num_gather_feature + 3]  
+    =  dev_pillar_point_feature[ith_pillar * max_num_points_per_pillar * num_point_feature + ith_point * num_point_feature + 3];
+
+    dev_pfe_gather_feature_[ith_pillar * max_num_points_per_pillar * num_gather_feature + ith_point * num_gather_feature + 4]  
+    =  dev_pillar_point_feature[ith_pillar * max_num_points_per_pillar * num_point_feature + ith_point * num_point_feature + 4];
+  
+    // dev_pfe_gather_feature_[ith_pillar * max_num_points_per_pillar * num_gather_feature + ith_point * num_gather_feature + 4]  =  0.0f;
+    //   f_cluster = voxel_features[:, :, :3] - points_mean
+    dev_pfe_gather_feature_[ith_pillar * max_num_points_per_pillar * num_gather_feature + ith_point * num_gather_feature + 5]  
+    =  dev_pillar_point_feature[ith_pillar * max_num_points_per_pillar * num_point_feature + ith_point * num_point_feature + 0] - dev_points_mean[ith_pillar * 3 + 0 ];
+
+    dev_pfe_gather_feature_[ith_pillar * max_num_points_per_pillar * num_gather_feature + ith_point * num_gather_feature + 6] 
+    =  dev_pillar_point_feature[ith_pillar * max_num_points_per_pillar * num_point_feature + ith_point * num_point_feature + 1] - dev_points_mean[ith_pillar * 3 + 1 ];
+  
+    dev_pfe_gather_feature_[ith_pillar * max_num_points_per_pillar * num_gather_feature + ith_point * num_gather_feature + 7]  
+    =  dev_pillar_point_feature[ith_pillar * max_num_points_per_pillar * num_point_feature + ith_point * num_point_feature + 2] - dev_points_mean[ith_pillar * 3 + 2 ];
+
+    // f_center[:, :, 0] = voxel_features[:, :, 0] - (coords[:, 3].to(voxel_features.dtype).unsqueeze(1) * self.voxel_x + self.x_offset)
+    dev_pfe_gather_feature_[ith_pillar * max_num_points_per_pillar * num_gather_feature + ith_point * num_gather_feature + 8]  
+    =  dev_pillar_point_feature[ith_pillar * max_num_points_per_pillar * num_point_feature + ith_point * num_point_feature + 0] - (dev_pillar_coors[ith_pillar * 4 + 3] * pillar_x_size + (pillar_x_size/2 + min_x_range));
+  
+    dev_pfe_gather_feature_[ith_pillar * max_num_points_per_pillar * num_gather_feature + ith_point * num_gather_feature + 9]  
+    =  dev_pillar_point_feature[ith_pillar * max_num_points_per_pillar * num_point_feature + ith_point * num_point_feature + 1] - (dev_pillar_coors[ith_pillar * 4 + 2] * pillar_y_size + (pillar_y_size/2 + min_y_range));
+  
+    dev_pfe_gather_feature_[ith_pillar * max_num_points_per_pillar * num_gather_feature + ith_point * num_gather_feature + 10] 
+    =  dev_pillar_point_feature[ith_pillar * max_num_points_per_pillar * num_point_feature + ith_point * num_point_feature + 2] - (dev_pillar_coors[ith_pillar * 4 + 1] * pillar_z_size + (pillar_z_size/2 + min_z_range));
+
+}
+
+
+
+
+PreprocessPointsCuda::PreprocessPointsCuda(
+    const int num_threads, const int max_num_pillars,
+    const int max_points_per_pillar, const int num_point_feature,
+    const int num_inds_for_scan, const int grid_x_size, const int grid_y_size,
+    const int grid_z_size, const float pillar_x_size, const float pillar_y_size,
+    const float pillar_z_size, const float min_x_range, const float min_y_range,
+    const float min_z_range)
+    : num_threads_(num_threads),
+      max_num_pillars_(max_num_pillars),
+      max_num_points_per_pillar_(max_points_per_pillar),
+      num_point_feature_(num_point_feature),
+      num_inds_for_scan_(num_inds_for_scan),
+      grid_x_size_(grid_x_size),
+      grid_y_size_(grid_y_size),
+      grid_z_size_(grid_z_size),
+      pillar_x_size_(pillar_x_size),
+      pillar_y_size_(pillar_y_size),
+      pillar_z_size_(pillar_z_size),
+      min_x_range_(min_x_range),
+      min_y_range_(min_y_range),
+      min_z_range_(min_z_range) {
+    
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_pillar_point_feature_in_coors_),
+        grid_y_size_ * grid_x_size_ * max_num_points_per_pillar_ *  num_point_feature_ * sizeof(float)));
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_pillar_count_histo_),
+        grid_y_size_ * grid_x_size_ * sizeof(int)));
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_counter_), sizeof(int)));
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_pillar_count_), sizeof(int)));    
+    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_points_mean_), max_num_pillars_ * 3 *sizeof(float)));  
+    }
+
+PreprocessPointsCuda::~PreprocessPointsCuda() {
+    GPU_CHECK(cudaFree(dev_pillar_point_feature_in_coors_));
+    GPU_CHECK(cudaFree(dev_pillar_count_histo_));
+    GPU_CHECK(cudaFree(dev_counter_));
+    GPU_CHECK(cudaFree(dev_pillar_count_));
+    GPU_CHECK(cudaFree(dev_points_mean_));
+  }
+
+
+void PreprocessPointsCuda::DoPreprocessPointsCuda(
+    const float* dev_points, const int in_num_points, 
+    int* dev_x_coors,int* dev_y_coors, 
+    float* dev_num_points_per_pillar,
+    float* dev_pillar_point_feature, float* dev_pillar_coors,
+    int* dev_sparse_pillar_map, int* host_pillar_count , float* dev_pfe_gather_feature) {
+    // initialize paraments
+    GPU_CHECK(cudaMemset(dev_pillar_point_feature_in_coors_, 0 , grid_y_size_ * grid_x_size_ * max_num_points_per_pillar_ *  num_point_feature_ * sizeof(float)));
+    GPU_CHECK(cudaMemset(dev_pillar_count_histo_, 0 , grid_y_size_ * grid_x_size_ * sizeof(int)));
+    GPU_CHECK(cudaMemset(dev_counter_, 0, sizeof(int)));
+    GPU_CHECK(cudaMemset(dev_pillar_count_, 0, sizeof(int)));
+    GPU_CHECK(cudaMemset(dev_points_mean_, 0,  max_num_pillars_ * 3 * sizeof(float)));
+    int num_block = DIVUP(in_num_points , num_threads_);
+    make_pillar_histo_kernel<<<num_block , num_threads_>>>(
+        dev_points, dev_pillar_point_feature_in_coors_, dev_pillar_count_histo_,
+        in_num_points, max_num_points_per_pillar_, grid_x_size_, grid_y_size_,
+        grid_z_size_, min_x_range_, min_y_range_, min_z_range_, pillar_x_size_,
+        pillar_y_size_, pillar_z_size_, num_point_feature_);
+    
+    make_pillar_index_kernel<<<grid_x_size_, grid_y_size_>>>(
+        dev_pillar_count_histo_, dev_counter_, dev_pillar_count_, dev_x_coors,
+        dev_y_coors, dev_num_points_per_pillar, dev_sparse_pillar_map,
+        max_num_pillars_, max_num_points_per_pillar_, grid_x_size_,
+        num_inds_for_scan_);  
+
+    GPU_CHECK(cudaMemcpy(host_pillar_count, dev_pillar_count_, 1 * sizeof(int),
+        cudaMemcpyDeviceToHost));
+    make_pillar_feature_kernel<<<host_pillar_count[0],max_num_points_per_pillar_>>>(
+        dev_pillar_point_feature_in_coors_, dev_pillar_point_feature,
+        dev_pillar_coors, dev_x_coors, dev_y_coors, dev_num_points_per_pillar,
+        max_num_points_per_pillar_, num_point_feature_, grid_x_size_);
+    
+
+    dim3 mean_block(max_num_points_per_pillar_,3); //(32,3)
+
+    pillar_mean_kernel<<<host_pillar_count[0],mean_block,64 * 3 *sizeof(float)>>>(
+      dev_points_mean_  ,num_point_feature_, dev_pillar_point_feature, dev_num_points_per_pillar, 
+        max_num_pillars_ , max_num_points_per_pillar_);
+
+    // dim3 mean_block(10,3); // Unrolling the Last Warp
+    // make_pillar_mean_kernel<<<host_pillar_count[0], mean_block , 32 * 3 *sizeof(float)>>>(
+    //       dev_points_mean_  ,num_point_feature_, dev_pillar_point_feature, dev_num_points_per_pillar, 
+    //       max_num_pillars_ , max_num_points_per_pillar_);
+
+    gather_point_feature_kernel<<<max_num_pillars_, max_num_points_per_pillar_>>>(
+      max_num_pillars_,max_num_points_per_pillar_,num_point_feature_,
+      min_x_range_, min_y_range_, min_z_range_,
+      pillar_x_size_, pillar_y_size_, pillar_z_size_, 
+      dev_pillar_point_feature, dev_num_points_per_pillar, dev_pillar_coors,
+      dev_points_mean_,
+      dev_pfe_gather_feature);
+
+    // DEVICE_SAVE<float>(dev_pillar_point_feature , \
+    //     max_num_pillars_ * max_num_points_per_pillar_ * num_point_feature_ , "dev_pillar_point_feature");
+    // DEVICE_SAVE<float>(dev_num_points_per_pillar , \
+    //   max_num_pillars_ , "dev_num_points_per_pillar");
+    // DEVICE_SAVE<float>(dev_pfe_gather_feature , \
+    //   max_num_pillars_ * 11, "dev_pfe_gather_feature");
+}
+
+

src/detection/detection_lidar_PointPillars_MultiHead_1025/preprocess.h

@@ -0,0 +1,138 @@
+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @file preprocess_points_cuda.h
+ * @brief GPU version of preprocess points
+ * @author Kosuke Murakami
+ * @date 2019/02/26
+ */
+
+/**
+* @author Yan haixu
+* Contact: just github.com/hova88
+* @date 2021/04/30
+*/
+
+
+#pragma once
+
+
+class PreprocessPointsCuda {
+ private:
+    // initializer list
+    const int num_threads_;
+    const int max_num_pillars_;
+    const int max_num_points_per_pillar_;
+    const int num_point_feature_;
+    const int num_inds_for_scan_;
+    const int grid_x_size_;
+    const int grid_y_size_;
+    const int grid_z_size_;
+    const float pillar_x_size_;
+    const float pillar_y_size_;
+    const float pillar_z_size_;
+    const float min_x_range_;
+    const float min_y_range_;
+    const float min_z_range_;
+    // end initializer list
+
+    float* dev_pillar_point_feature_in_coors_;
+    int* dev_pillar_count_histo_;
+
+    int* dev_counter_;
+    int* dev_pillar_count_;
+    float* dev_points_mean_;
+
+
+
+ public:
+  /**
+   * @brief Constructor
+   * @param[in] num_threads Number of threads when launching cuda kernel
+   * @param[in] num_point_feature Number of features in a point
+   * @param[in] num_inds_for_scan Number of indexes for scan(cumsum)
+   * 
+   * @param[in] max_num_pillars Maximum number of pillars
+   * @param[in] max_points_per_pillar Maximum number of points per pillar
+
+   * @param[in] grid_x_size Number of pillars in x-coordinate
+   * @param[in] grid_y_size Number of pillars in y-coordinate
+   * @param[in] grid_z_size Number of pillars in z-coordinate
+   * 
+   * @param[in] pillar_x_size Size of x-dimension for a pillar
+   * @param[in] pillar_y_size Size of y-dimension for a pillar
+   * @param[in] pillar_z_size Size of z-dimension for a pillar
+   * 
+   * @param[in] min_x_range Minimum x value for point cloud
+   * @param[in] min_y_range Minimum y value for point cloud
+   * @param[in] min_z_range Minimum z value for point cloud
+   * @details Captital variables never change after the compile
+   */
+  PreprocessPointsCuda(const int num_threads, const int num_point_feature, const int num_inds_for_scan,
+                       const int max_num_pillars, const int max_points_per_pillar,  
+                       const int grid_x_size, const int grid_y_size, const int grid_z_size,  // grid size
+                       const float pillar_x_size, const float pillar_y_size, const float pillar_z_size, //voxel size
+                       const float min_x_range, const float min_y_range, const float min_z_range); // point cloud range
+  ~PreprocessPointsCuda();
+
+  /**
+   * @brief CUDA preprocessing for input point cloud
+   * @param[in] dev_points Point cloud array
+   * @param[in] in_num_points The number of points
+   * @param[mid] dev_x_coors X-coordinate indexes for corresponding pillars
+   * @param[mid] dev_y_coors Y-coordinate indexes for corresponding pillars
+   * @param[mid] dev_num_points_per_pillar
+   *   Number of points in corresponding pillars
+   * @param[mid] pillar_point_feature
+   *   Values of point feature in each pillar
+   * @param[mid] pillar_coors Array for coors of pillars
+   * @param[mid] dev_points_mean Array for calculate the point center
+   * @param[out] dev_sparse_pillar_map
+   *   Grid map representation for pillar-occupancy
+   * @param[out] host_pillar_count
+   *   The number of valid pillars for an input point cloud
+   * @param[out] dev_pfe_gather_feature
+   *   11 dimensions feature for pfe input channel
+   * @details Convert point cloud to pillar representation
+   */
+  void DoPreprocessPointsCuda(const float* dev_points,
+                              const int in_num_points,
+                              int* dev_x_coors,
+                              int* dev_y_coors,
+                              float* dev_num_points_per_pillar,
+                              float* dev_pillar_point_feature,
+                              float* dev_pillar_coors,
+                              int* dev_sparse_pillar_map,
+                              int* host_pillar_count,
+                              float* dev_pfe_gather_feature);
+};

src/detection/detection_lidar_PointPillars_MultiHead_1025/scatter.cu

@@ -0,0 +1,73 @@
+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @author Kosuke Murakami
+ * @date 2019/02/26
+ */
+
+/**
+* @author Yan haixu
+* Contact: just github.com/hova88
+* @date 2021/04/30
+*/
+
+
+
+//headers in local files
+#include "scatter.h"
+
+__global__ void scatter_kernel(int *x_coors, int *y_coors, float *pfe_output,
+  float *scattered_feature, const int grid_x_size,
+  const int grid_y_size) {
+int i_pillar = blockIdx.x;
+int i_feature = threadIdx.x;
+int x_ind = x_coors[i_pillar];
+int y_ind = y_coors[i_pillar];
+float feature = pfe_output[i_pillar * 64 + i_feature];
+scattered_feature[i_feature * grid_y_size * grid_x_size +
+y_ind * grid_x_size + x_ind] = feature;
+}
+
+ScatterCuda::ScatterCuda(const int num_threads, const int grid_x_size,
+const int grid_y_size)
+: num_threads_(num_threads),
+grid_x_size_(grid_x_size),
+grid_y_size_(grid_y_size) {}
+
+void ScatterCuda::DoScatterCuda(const int pillar_count, int *x_coors,
+   int *y_coors, float *pfe_output,
+   float *scattered_feature) {
+scatter_kernel<<<pillar_count, num_threads_>>>(x_coors, y_coors, pfe_output,
+                    scattered_feature,
+                    grid_x_size_, grid_y_size_);
+}

src/detection/detection_lidar_PointPillars_MultiHead_1025/scatter.h

@@ -0,0 +1,77 @@
+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * @author Kosuke Murakami
+ * @date 2019/02/26
+ */
+
+/**
+* @author Yan haixu
+* Contact: just github.com/hova88
+* @date 2021/04/30
+*/
+
+
+#pragma once
+
+class ScatterCuda {
+ private:
+  const int num_threads_;
+  const int grid_x_size_;
+  const int grid_y_size_;
+
+ public:
+  /**
+   * @brief Constructor
+   * @param[in] num_threads The number of threads to launch cuda kernel
+   * @param[in] grid_x_size Number of pillars in x-coordinate
+   * @param[in] grid_y_size Number of pillars in y-coordinate
+   * @details Captital variables never change after the compile
+   */
+  ScatterCuda(const int num_threads, const int grid_x_size,
+              const int grid_y_size);
+
+  /**
+   * @brief Call scatter cuda kernel
+   * @param[in] pillar_count The valid number of pillars
+   * @param[in] x_coors X-coordinate indexes for corresponding pillars
+   * @param[in] y_coors Y-coordinate indexes for corresponding pillars
+   * @param[in] pfe_output Output from Pillar Feature Extractor
+   * @param[out] scattered_feature Gridmap representation for pillars' feature
+   * @details Allocate pillars in gridmap based on index(coordinates)
+   * information
+   */
+  void DoScatterCuda(const int pillar_count, int* x_coors, int* y_coors,
+                     float* pfe_output, float* scattered_feature);
+};
+

src/fusion/lidar_radar_fusion_cnn_1025/Tracker/Ctracker.cpp

@@ -0,0 +1,294 @@
+#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 = 1e3;//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)
+            {
+#ifdef DEBUG_SHOW
+                std::cout<<costMatrix[i + assignment[i] * N]<<", ";
+#endif
+                if (costMatrix[i + assignment[i] * N] > m_settings.m_distThres)
+                {
+                    assignment[i] = -1;
+                    m_tracks[i]->SkippedFrames()++;
+                }
+            }
+            else
+            {
+#ifdef DEBUG_SHOW
+                std::cout<<-1<<", ";
+#endif
+                // If track have no assigned detect, then increment skipped frames counter.
+                m_tracks[i]->SkippedFrames()++;
+            }
+        }
+#ifdef DEBUG_SHOW
+                std::cout<<std::endl;
+#endif
+        // 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]->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,
+                                                        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]->DetectedFrames()++;
+            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);
+        }
+        m_tracks[i]->m_regionID = assignment[i];
+    }
+}
+
+///
+/// \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_rect.size.width;
+//            minRadius.height = m_settings.m_minAreaRadiusK * track->LastRegion().m_rect.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 (reg.m_type==-1?(m_settings.CheckType(m_tracks[i]->LastRegion().m_type_name, reg.m_type_name)):(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 0
+                    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 0
+                    track_t ellipseDist = track->IsInsideArea(reg.m_rect.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::DistRect3Ds)
+                {
+#if 0
+                    track_t ellipseDist = track->IsInsideArea(reg.m_rect.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->CalcDistRect3D(reg);
+#endif
+                }
+                ++ind;
+                assert(ind == tracking::DistsCount);
+            }
+            costMatrix[i + j * N] = dist;
+            if (dist > maxCost)
+                maxCost = dist;
+        }
+    }
+}

src/fusion/lidar_radar_fusion_cnn_1025/Tracker/Ctracker.h

@@ -0,0 +1,292 @@
+#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::TrackNone;
+    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<int, std::set<int>> m_nearTypes;
+
+    std::map<std::string, std::set<std::string>> m_nearTypeNames;
+
+	///
+	TrackerSettings()
+	{
+        m_distType[tracking::DistCenters] = 0.5f;
+        m_distType[tracking::DistRects] = 0.5f;
+        m_distType[tracking::DistRect3Ds] = 0.f;
+
+		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 int& type1, const int& type2, bool sym)
+	{
+        auto AddOne = [&](const int& type1, const int& type2)
+		{
+			auto it = m_nearTypes.find(type1);
+			if (it == std::end(m_nearTypes))
+			{
+                m_nearTypes[type1] = std::set<int>{ type2 };
+			}
+			else
+			{
+				it->second.insert(type2);
+			}
+		};
+		AddOne(type1, type2);
+		if (sym)
+        {
+			AddOne(type2, type1);
+        }
+	}
+
+	///
+    bool CheckType(const int& type1, const int& type2) const
+	{
+        bool res = type1==-1 || type2==-1 || (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;
+	}
+
+    ///
+    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_nearTypeNames.find(type1);
+            if (it == std::end(m_nearTypeNames))
+            {
+                m_nearTypeNames[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_nearTypeNames.find(type1);
+            if (it != std::end(m_nearTypeNames))
+            {
+                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::TrackNone);
+        return !needColor;
+    }
+
+	///
+	/// \brief CanColorFrameToTrack
+	/// \return
+	///
+	bool CanColorFrameToTrack() const
+	{
+		return true;
+	}
+
+    ///
+    /// \brief GetTracksCount
+    /// \return
+    ///
+	size_t GetTracksCount() const
+	{
+		return m_tracks.size();
+	}
+    ///
+    /// \brief GetTracks
+    /// \return
+    ///
+	std::vector<TrackingObject> GetTracks() const
+	{
+		std::vector<TrackingObject> tracks;
+		if (!m_tracks.empty())
+		{
+			tracks.reserve(m_tracks.size());
+			for (const auto& track : m_tracks)
+			{
+                tracks.push_back(track->ConstructObject());
+			}
+		}
+		return tracks;
+	}
+
+private:
+    TrackerSettings m_settings;
+
+	tracks_t m_tracks;
+
+    size_t m_nextTrackID;
+
+    cv::UMat m_prevFrame;
+
+    std::unique_ptr<ShortPathCalculator> m_SPCalculator;
+
+    void CreateDistaceMatrix(const regions_t& regions, distMatrix_t& costMatrix, track_t maxPossibleCost, track_t& maxCost, cv::UMat currFrame);
+    void UpdateTrackingState(const regions_t& regions, cv::UMat currFrame, float fps);
+};

src/fusion/lidar_radar_fusion_cnn_1025/Tracker/HungarianAlg.cpp

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

src/fusion/lidar_radar_fusion_cnn_1025/Tracker/HungarianAlg.h

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

src/fusion/lidar_radar_fusion_cnn_1025/Tracker/Kalman.cpp

@@ -0,0 +1,873 @@
+#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(cv::Point3f xy0, cv::Point3f 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::CreateLinear3D(Rect3D rect0, cv::Point3f 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.
+
+    // 14 state variables (x, y, z, width, height, length, yaw, d...), 7 measurements (x, y, z, width, height, length, yaw)
+    m_linearKalman.init(14, 7, 0, El_t);
+    // Transition cv::Matrix
+    m_linearKalman.transitionMatrix = (cv::Mat_<track_t>(14, 14) <<
+                                        1, 0, 0, 0, 0, 0, 0, m_deltaTime, 0,           0,           0,           0,           0,           0,
+                                        0, 1, 0, 0, 0, 0, 0, 0,           m_deltaTime, 0,           0,           0,           0,           0,
+                                        0, 0, 1, 0, 0, 0, 0, 0,           0,           m_deltaTime, 0,           0,           0,           0,
+                                        0, 0, 0, 1, 0, 0, 0, 0,           0,           0,           m_deltaTime, 0,           0,           0,
+                                        0, 0, 0, 0, 1, 0, 0, 0,           0,           0,           0,           m_deltaTime, 0,           0,
+                                        0, 0, 0, 0, 0, 1, 0, 0,           0,           0,           0,           0,           m_deltaTime, 0,
+                                        0, 0, 0, 0, 0, 0, 1, 0,           0,           0,           0,           0,           0,           m_deltaTime,
+                                        0, 0, 0, 0, 0, 0, 0, 1,           0,           0,           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,           0,           1,           0,           0,           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,           0,           1,           0,           0,
+                                        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,           0,           1);
+
+    // init...
+    m_linearKalman.statePre.at<track_t>(0) = rect0.center.x;      // x
+    m_linearKalman.statePre.at<track_t>(1) = rect0.center.y;      // y
+    m_linearKalman.statePre.at<track_t>(2) = rect0.center.z;      // z
+    m_linearKalman.statePre.at<track_t>(3) = rect0.size.width;  // width
+    m_linearKalman.statePre.at<track_t>(4) = rect0.size.height; // height
+    m_linearKalman.statePre.at<track_t>(5) = rect0.size.length; // length
+    m_linearKalman.statePre.at<track_t>(6) = rect0.yaw; // yaw
+    m_linearKalman.statePre.at<track_t>(7) = rectv0.x;     // dx
+    m_linearKalman.statePre.at<track_t>(8) = rectv0.y;     // dy
+    m_linearKalman.statePre.at<track_t>(9) = 0;            // dz
+    m_linearKalman.statePre.at<track_t>(10) = 0;            // dw
+    m_linearKalman.statePre.at<track_t>(11) = 0;            // dh
+    m_linearKalman.statePre.at<track_t>(12) = 0;            // dl
+    m_linearKalman.statePre.at<track_t>(13) = 0;            // dyaw
+
+    m_linearKalman.statePost.at<track_t>(0) = rect0.center.x;
+    m_linearKalman.statePost.at<track_t>(1) = rect0.center.y;
+    m_linearKalman.statePost.at<track_t>(2) = rect0.center.z;
+    m_linearKalman.statePost.at<track_t>(3) = rect0.size.width;
+    m_linearKalman.statePost.at<track_t>(4) = rect0.size.height;
+    m_linearKalman.statePost.at<track_t>(5) = rect0.size.length;
+    m_linearKalman.statePost.at<track_t>(6) = rect0.yaw;
+    m_linearKalman.statePost.at<track_t>(7) = rectv0.x;
+    m_linearKalman.statePost.at<track_t>(8) = rectv0.y;
+    m_linearKalman.statePost.at<track_t>(9) = 0;
+    m_linearKalman.statePost.at<track_t>(10) = 0;
+    m_linearKalman.statePost.at<track_t>(11) = 0;
+    m_linearKalman.statePost.at<track_t>(12) = 0;
+    m_linearKalman.statePost.at<track_t>(13) = 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>(14, 14) <<
+                                       n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,
+                                       0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,
+                                       0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,
+                                       0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,
+                                       0,  0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,  0,
+                                       0,  0,  0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2, 0,
+                                       0,  0,  0,  0,  0,  0,  n1, 0,  0,  0,  0,  0,  0,  n2,
+                                       n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,  0,  0,  0,
+                                       0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,  0,  0,
+                                       0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,  0,
+                                       0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,  0,
+                                       0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,  0,
+                                       0,  0,  0,  0,  0,  n2, 0,  0,  0,  0,  0,  0,  n3, 0,
+                                       0,  0,  0,  0,  0,  0,  n2, 0,  0,  0,  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(cv::Point3f xy0, cv::Point3f 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;
+}
+//---------------------------------------------------------------------------
+void TKalmanFilter::CreateLinearAcceleration3D(Rect3D rect0, cv::Point3f 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.center.x;      // x
+    m_linearKalman.statePre.at<track_t>(1) = rect0.center.y;      // y
+    m_linearKalman.statePre.at<track_t>(2) = rect0.size.width;  // width
+    m_linearKalman.statePre.at<track_t>(3) = rect0.size.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.center.x;
+    m_linearKalman.statePost.at<track_t>(1) = rect0.center.y;
+    m_linearKalman.statePost.at<track_t>(2) = rect0.size.width;
+    m_linearKalman.statePost.at<track_t>(3) = rect0.size.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;
+}
+
+//---------------------------------------------------------------------------
+cv::Point3f TKalmanFilter::GetPointPrediction()
+{
+    if (m_initialized)
+    {
+        cv::Mat prediction;
+
+        switch (m_type)
+        {
+        case tracking::KalmanLinear:
+            prediction = m_linearKalman.predict();
+            break;
+        }
+
+        m_lastPointResult = cv::Point3f(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2));
+    }
+    else
+    {
+
+    }
+    return m_lastPointResult;
+}
+
+//---------------------------------------------------------------------------
+cv::Point3f TKalmanFilter::Update(cv::Point3f 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
+            cv::Point3f xy0(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by, m_lastPointResult.z);
+            cv::Point3f xyv0(kx, ky,0);
+
+            switch (m_type)
+            {
+            case tracking::KalmanLinear:
+                if (m_useAcceleration)
+					CreateLinearAcceleration(xy0, xyv0);
+				else
+					CreateLinear(xy0, xyv0);
+                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;
+        }
+        }
+
+        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;
+        }
+
+        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;
+            }
+        }
+    }
+
+    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;
+        }
+        }
+    }
+    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));
+}
+//---------------------------------------------------------------------------
+Rect3D TKalmanFilter::GetRect3DPrediction()
+{
+    if (m_initialized)
+    {
+        cv::Mat prediction;
+
+        switch (m_type)
+        {
+        case tracking::KalmanLinear:
+            prediction = m_linearKalman.predict();
+            break;
+        }
+
+        m_lastRect3DResult = Rect3D(cv::Point3f(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2)), Size3D(prediction.at<track_t>(3), prediction.at<track_t>(4), prediction.at<track_t>(5)), prediction.at<track_t>(6));
+    }
+    else
+    {
+
+    }
+    return m_lastRect3DResult;
+}
+
+//---------------------------------------------------------------------------
+Rect3D TKalmanFilter::Update(Rect3D rect, bool dataCorrect)
+{
+    if (!m_initialized)
+    {
+        if (m_initialRect3Ds.size() < MIN_INIT_VALS)
+        {
+            if (dataCorrect)
+            {
+                m_initialRect3Ds.push_back(rect);
+                m_lastRect3DResult.center.x = static_cast<track_t>(rect.center.x);
+                m_lastRect3DResult.center.y = static_cast<track_t>(rect.center.y);
+                m_lastRect3DResult.center.z = static_cast<track_t>(rect.center.z);
+                m_lastRect3DResult.size.width = static_cast<track_t>(rect.size.width);
+                m_lastRect3DResult.size.height = static_cast<track_t>(rect.size.height);
+                m_lastRect3DResult.size.length = static_cast<track_t>(rect.size.length);
+                m_lastRect3DResult.yaw = static_cast<track_t>(rect.yaw);
+            }
+        }
+        if (m_initialRect3Ds.size() == MIN_INIT_VALS)
+        {
+            std::vector<Point_t> initialPoints;
+            cv::Point3f averageSize(0, 0, 0);
+            float averageZ = 0;
+            float averageYaw = 0;
+            for (const auto& r : m_initialRect3Ds)
+            {
+                initialPoints.emplace_back(static_cast<track_t>(r.center.x), static_cast<track_t>(r.center.y));
+                averageZ += r.center.z;
+                averageSize.x += r.size.width;
+                averageSize.y += r.size.height;
+                averageSize.z += r.size.length;
+                averageYaw += r.yaw;
+            }
+            averageZ /= MIN_INIT_VALS;
+            averageSize.x /= MIN_INIT_VALS;
+            averageSize.y /= MIN_INIT_VALS;
+            averageSize.z /= MIN_INIT_VALS;
+            averageYaw /= 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);
+            Rect3D rect0(cv::Point3f(kx * (MIN_INIT_VALS - 1) + bx, ky * (MIN_INIT_VALS - 1) + by, averageZ), Size3D(averageSize.x, averageSize.y,averageSize.z), averageYaw);
+            cv::Point3f rectv0(kx, ky, 0);
+
+            switch (m_type)
+            {
+            case tracking::KalmanLinear:
+				if (m_useAcceleration)
+                    CreateLinearAcceleration3D(rect0, rectv0);
+				else
+                    CreateLinear3D(rect0, rectv0);
+                break;
+            }
+        }
+    }
+
+    if (m_initialized)
+    {
+        cv::Mat measurement(7, 1, Mat_t(1));
+        if (!dataCorrect)
+        {
+            measurement.at<track_t>(0) = m_lastRect3DResult.center.x;  // update using prediction
+            measurement.at<track_t>(1) = m_lastRect3DResult.center.y;
+            measurement.at<track_t>(2) = m_lastRect3DResult.center.z;
+            measurement.at<track_t>(3) = m_lastRect3DResult.size.width;
+            measurement.at<track_t>(4) = m_lastRect3DResult.size.height;
+            measurement.at<track_t>(5) = m_lastRect3DResult.size.length;
+            measurement.at<track_t>(6) = m_lastRect3DResult.yaw;
+        }
+        else
+        {
+            measurement.at<track_t>(0) = static_cast<track_t>(rect.center.x);  // update using measurements
+            measurement.at<track_t>(1) = static_cast<track_t>(rect.center.y);
+            measurement.at<track_t>(2) = static_cast<track_t>(rect.center.z);
+            measurement.at<track_t>(3) = static_cast<track_t>(rect.size.width);
+            measurement.at<track_t>(4) = static_cast<track_t>(rect.size.height);
+            measurement.at<track_t>(5) = static_cast<track_t>(rect.size.length);
+            measurement.at<track_t>(6) = static_cast<track_t>(rect.yaw);
+        }
+        // Correction
+        cv::Mat estimated;
+        switch (m_type)
+        {
+        case tracking::KalmanLinear:
+        {
+            estimated = m_linearKalman.correct(measurement);
+
+            m_lastRect3DResult.center.x = estimated.at<track_t>(0);   //update using measurements
+            m_lastRect3DResult.center.y = estimated.at<track_t>(1);
+            m_lastRect3DResult.center.z = estimated.at<track_t>(2);
+            m_lastRect3DResult.size.width = estimated.at<track_t>(3);
+            m_lastRect3DResult.size.height = estimated.at<track_t>(4);
+            m_lastRect3DResult.size.length = estimated.at<track_t>(5);
+            m_lastRect3DResult.yaw = estimated.at<track_t>(6);
+
+            // Inertia correction
+			if (!m_useAcceleration)
+			{
+                track_t currDist = sqrtf(sqr(estimated.at<track_t>(0) - rect.center.x) + sqr(estimated.at<track_t>(1) - rect.center.y)+ sqr(estimated.at<track_t>(2) - rect.center.z) + sqr(estimated.at<track_t>(3) - rect.size.width) + sqr(estimated.at<track_t>(4) - rect.size.height) + sqr(estimated.at<track_t>(5) - rect.size.length) + sqr(estimated.at<track_t>(6) - rect.yaw));
+				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, 7) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(1, 8) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(2, 9) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(3, 10) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(4, 11) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(5, 12) = m_deltaTime;
+                m_linearKalman.transitionMatrix.at<track_t>(6, 13) = m_deltaTime;
+			}
+            break;
+        }
+        }
+    }
+    else
+    {
+        if (dataCorrect)
+        {
+            m_lastRect3DResult.center.x = static_cast<track_t>(rect.center.x);
+            m_lastRect3DResult.center.y = static_cast<track_t>(rect.center.y);
+            m_lastRect3DResult.center.z = static_cast<track_t>(rect.center.z);
+            m_lastRect3DResult.size.width = static_cast<track_t>(rect.size.width);
+            m_lastRect3DResult.size.height = static_cast<track_t>(rect.size.height);
+            m_lastRect3DResult.size.length = static_cast<track_t>(rect.size.length);
+            m_lastRect3DResult.yaw = static_cast<track_t>(rect.yaw);
+        }
+    }
+    return m_lastRect3DResult;
+}
+
+//---------------------------------------------------------------------------
+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 > 5)
+                {
+                    indX = 4;
+                    indY = 5;
+                    if (m_linearKalman.statePre.rows > 8)
+                    {
+                        indX = 7;
+                        indY = 8;
+                    }
+                    res[0] = m_linearKalman.statePre.at<track_t>(indX);
+                    res[1] = m_linearKalman.statePre.at<track_t>(indY);
+                }
+            }
+            break;
+        }
+
+        }
+    }
+    return res;
+}

src/fusion/lidar_radar_fusion_cnn_1025/Tracker/Kalman.h

@@ -0,0 +1,120 @@
+#pragma once
+#include "defines.h"
+#include <memory>
+#include <deque>
+
+#include <opencv2/opencv.hpp>
+
+///
+/// \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;
+
+    cv::Point3f GetPointPrediction();
+    cv::Point3f Update(cv::Point3f pt, bool dataCorrect);
+
+    cv::Rect GetRectPrediction();
+    cv::Rect Update(cv::Rect rect, bool dataCorrect);
+
+    Rect3D GetRect3DPrediction();
+    Rect3D Update(Rect3D rect, bool dataCorrect);
+
+	cv::Vec<track_t, 2> GetVelocity() const;
+
+private:
+    tracking::KalmanType m_type = tracking::KalmanLinear;
+    cv::KalmanFilter m_linearKalman;
+    static const size_t MIN_INIT_VALS = 4;
+    std::deque<cv::Point3f> m_initialPoints;
+    cv::Point3f m_lastPointResult;
+
+    std::deque<cv::Rect> m_initialRects;
+    cv::Rect_<track_t> m_lastRectResult;
+    cv::Rect_<track_t> m_lastRect;
+
+    std::deque<Rect3D> m_initialRect3Ds;
+    Rect3D m_lastRect3DResult;
+    cv::Rect_<track_t> m_lastRect3D;
+
+    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(cv::Point3f xy0, cv::Point3f xyv0);
+    void CreateLinear(cv::Rect_<track_t> rect0, Point_t rectv0);
+    void CreateLinear3D(Rect3D rect0, cv::Point3f rectv0);
+
+	// Constant acceleration model
+	// https://www.mathworks.com/help/driving/ug/linear-kalman-filters.html
+    void CreateLinearAcceleration(cv::Point3f xy0, cv::Point3f xyv0);
+    void CreateLinearAcceleration(cv::Rect_<track_t> rect0, Point_t rectv0);
+    void CreateLinearAcceleration3D(Rect3D rect0, cv::Point3f rectv0);
+};
+
+//---------------------------------------------------------------------------
+///
+/// \brief sqr
+/// \param val
+/// \return
+///
+template<class T> inline
+T sqr(T val)
+{
+    return val * val;
+}
+
+///
+/// \brief get_lin_regress_params
+/// \param in_data
+/// \param start_pos
+/// \param in_data_size
+/// \param kx
+/// \param bx
+/// \param ky
+/// \param by
+///
+template<typename T, typename CONT>
+void get_lin_regress_params(
+        const CONT& in_data,
+        size_t start_pos,
+        size_t in_data_size,
+        T& kx, T& bx, T& ky, T& by)
+{
+    T m1(0.), m2(0.);
+    T m3_x(0.), m4_x(0.);
+    T m3_y(0.), m4_y(0.);
+
+    const T el_count = static_cast<T>(in_data_size - start_pos);
+    for (size_t i = start_pos; i < in_data_size; ++i)
+    {
+        m1 += i;
+        m2 += sqr(i);
+
+        m3_x += in_data[i].x;
+        m4_x += i * in_data[i].x;
+
+        m3_y += in_data[i].y;
+        m4_y += i * in_data[i].y;
+    }
+    T det_1 = 1 / (el_count * m2 - sqr(m1));
+
+    m1 *= -1;
+
+    kx = det_1 * (m1 * m3_x + el_count * m4_x);
+    bx = det_1 * (m2 * m3_x + m1 * m4_x);
+
+    ky = det_1 * (m1 * m3_y + el_count * m4_y);
+    by = det_1 * (m2 * m3_y + m1 * m4_y);
+}

src/fusion/lidar_radar_fusion_cnn_1025/Tracker/ShortPathCalculator.h

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

src/fusion/lidar_radar_fusion_cnn_1025/Tracker/Tracking.hpp

@@ -0,0 +1,178 @@
+#pragma once
+#include "fusionobjectarray.pb.h"
+#include "Ctracker.h"
+#include <iostream>
+#include <vector>
+#define FPS 8
+#define MIN_DETECTEDFRAMES 5
+///
+/// \brief 跟踪器参数设置
+///
+bool InitTracker(CTracker& tracker)
+{
+    TrackerSettings settings;
+    settings.SetDistance(tracking::DistRect3Ds); // 代价矩阵：两中心点之间的距离
+    settings.m_kalmanType = tracking::KalmanLinear; // 滤波器类型：卡尔曼线性滤波器
+    settings.m_filterGoal = tracking::FilterRect3D; // 滤波对象：Rect3D
+    settings.m_lostTrackType = tracking::TrackNone; // 丢失了的目标，不再追踪
+    settings.m_matchType = tracking::MatchHungrian; // 匹配算法：匈牙利
+    settings.m_dt = 1.f; // 卡尔曼滤波器的时间步长
+    settings.m_accelNoiseMag = 0.5f; // 卡尔曼的噪声放大器
+    settings.m_distThres = 30.f; // 匹配算法中的距离阈值
+    settings.m_minAreaRadiusPix = -1.f;//frame.rows / 20.f; // 目标的最小面积半径（像素）
+    settings.m_maximumAllowedSkippedFrames = 2; // 被跟踪目标允许未匹配到的最大次数，当超过这一数值，该目标的跟踪器将被移除
+    settings.m_maxTraceLength = 5; // 最大跟踪长度，即历史轨迹保留的最大长度
+
+    tracker.setSettings(settings);
+
+    return true;
+}
+
+
+///
+/// \brief 对融合后的目标进行跟踪，并以跟踪后的最优估计值更新融合目标的状态信息
+///
+iv::fusion::fusionobjectarray Tracking(iv::fusion::fusionobjectarray& fusionobjvec, CTracker& tracker)
+{
+#ifdef DEBUG_SHOW
+    std::cout<<"-------------------------------------------------"<<std::endl;
+#endif
+    iv::fusion::fusionobjectarray trackedobjvec;
+    trackedobjvec.clear_obj();
+    trackedobjvec.set_timestamp(fusionobjvec.timestamp());
+    regions_t regions;
+    cv::Point3f pointXYZ;
+    for(int i = 0;i<fusionobjvec.obj_size();i++)
+    {
+        pointXYZ.x = fusionobjvec.obj(i).centroid().x();
+        pointXYZ.y = fusionobjvec.obj(i).centroid().y();
+        pointXYZ.z = fusionobjvec.obj(i).centroid().z();
+        Rect3D rect;
+        rect.center = pointXYZ;
+        rect.size.width = fusionobjvec.obj(i).dimensions().x();
+        rect.size.height = fusionobjvec.obj(i).dimensions().y();
+        rect.size.length = fusionobjvec.obj(i).dimensions().z();
+        rect.yaw = fusionobjvec.obj(i).yaw();
+        CRegion region = CRegion(rect,fusionobjvec.obj(i).type(),fusionobjvec.obj(i).prob());
+        regions.push_back(region);
+#ifdef DEBUG_SHOW
+        std::cout<<"old id:"<<i<<std::endl;
+        std::cout<<"old type:"<<fusionobjvec.obj(i).type()<<std::endl;
+        std::cout<<"old x,y,z,w,h,l,yaw:"<<rect.center.x<<","<<rect.center.y<<","<<rect.center.z<<"  "<<rect.size.width<<","<<rect.size.height<<","<<rect.size.length<<"  "<<rect.yaw<<std::endl;
+#endif
+    }
+    tracker.Update(regions, cv::UMat(), 30);
+    auto tracks = tracker.GetTracks();
+#ifdef DEBUG_SHOW
+    std::cout<<"fusion size, tracker size:"<<regions.size()<<","<<tracks.size()<<std::endl;
+#endif
+    for (size_t i = 0; i < tracks.size(); i++)
+    {
+        const auto& track = tracks[i];
+        if(track.m_detectedFrames < MIN_DETECTEDFRAMES) continue;
+        int obj_id = track.m_regionID;
+        iv::fusion::fusionobject fusion_object;
+        if(obj_id != -1) // 当前融合目标成功匹配上跟踪器中的已有目标，则根据跟踪所得的最优估计值更新融合目标状态
+        {
+            fusion_object = fusionobjvec.obj(obj_id);
+            fusion_object.set_id(track.m_ID);
+
+            iv::fusion::PointXYZ centroid;
+            iv::fusion::PointXYZ *centerpoint;
+            centroid.set_x(track.m_rect.center.x);
+            centroid.set_y(track.m_rect.center.y);
+            centroid.set_z(track.m_rect.center.z);
+            centerpoint=fusion_object.mutable_centroid();
+            centerpoint->CopyFrom(centroid);
+/* not update */
+//            iv::fusion::Dimension dimension;
+//            iv::fusion::Dimension *obj_dimension;
+//            dimension.set_x(track.m_rect.size.width);
+//            dimension.set_y(track.m_rect.size.height);
+//            dimension.set_z(track.m_rect.size.length);
+//            obj_dimension=fusion_object.mutable_dimensions();
+//            obj_dimension->CopyFrom(dimension);
+
+//            fusion_object.set_yaw(track.m_rect.yaw);
+
+//            iv::fusion::VelXY vel_relative;
+//            iv::fusion::VelXY *velrelative;
+//            vel_relative.set_x(track.m_velocity[0]);
+//            vel_relative.set_y(track.m_velocity[1]);
+//            velrelative = fusion_object.mutable_vel_relative();
+//            velrelative->CopyFrom(vel_relative);
+
+            iv::fusion::PointXYZ point_historical;
+            for(int j=0;j<track.m_trace.size();j++)
+            {
+                point_historical.set_x(track.m_trace[j].x);
+                point_historical.set_y(track.m_trace[j].y);
+                point_historical.set_z(track.m_trace[j].z);
+                iv::fusion::PointXYZ *p = fusion_object.add_point_historical();
+                p->CopyFrom(point_historical);
+            }
+
+            iv::fusion::fusionobject *pe = trackedobjvec.add_obj();
+            pe->CopyFrom(fusion_object);
+        }else{ // 当前时刻没有融合目标与跟踪器中的已有目标匹配上，则将跟踪器中已有目标的预测结果增加到融合结果中
+            fusion_object.set_id(track.m_ID);
+            fusion_object.set_type(track.m_region.m_type);
+            fusion_object.set_prob(track.m_region.m_confidence);
+
+            iv::fusion::PointXYZ centroid;
+            iv::fusion::PointXYZ *centerpoint;
+            centroid.set_x(track.m_rect.center.x);
+            centroid.set_y(track.m_rect.center.y);
+            centroid.set_z(track.m_rect.center.z);
+            centerpoint=fusion_object.mutable_centroid();
+            centerpoint->CopyFrom(centroid);
+
+            iv::fusion::Dimension dimension;
+            iv::fusion::Dimension *obj_dimension;
+            dimension.set_x(track.m_region.m_rect.size.width);
+            dimension.set_y(track.m_region.m_rect.size.height);
+            dimension.set_z(track.m_region.m_rect.size.length);
+            obj_dimension=fusion_object.mutable_dimensions();
+            obj_dimension->CopyFrom(dimension);
+
+            fusion_object.set_yaw(track.m_region.m_rect.yaw);
+
+            iv::fusion::VelXY vel_relative;
+            iv::fusion::VelXY *velrelative;
+            vel_relative.set_x(track.m_velocity[0]*FPS);
+            vel_relative.set_y(track.m_velocity[1]*FPS);
+            velrelative = fusion_object.mutable_vel_relative();
+            velrelative->CopyFrom(vel_relative);
+
+            iv::fusion::PointXYZ point_historical;
+            for(int j=0;j<track.m_trace.size();j++)
+            {
+                point_historical.set_x(track.m_trace[j].x);
+                point_historical.set_y(track.m_trace[j].y);
+                point_historical.set_z(track.m_trace[j].z);
+                iv::fusion::PointXYZ *p = fusion_object.add_point_historical();
+                p->CopyFrom(point_historical);
+            }
+
+            iv::fusion::fusionobject *pe = trackedobjvec.add_obj();
+            pe->CopyFrom(fusion_object);
+        }
+#ifdef DEBUG_SHOW
+        std::cout<<"id:"<<fusion_object.id()<<"  "<<obj_id<<std::endl;
+        std::cout<<"type:"<<fusion_object.type()<<std::endl;
+        std::cout<<"update x,y,z,w,h,l,yaw,vx,vy:"<<fusion_object.centroid().x()<<","<<fusion_object.centroid().y()<<","<<fusion_object.centroid().z()<<"  "<<fusion_object.dimensions().x()<<","<<fusion_object.dimensions().y()<<","<<fusion_object.dimensions().z()<<"  "<<fusion_object.yaw()<<"  "<<fusion_object.vel_relative().x()<<","<<fusion_object.vel_relative().y()<<std::endl;
+#endif
+    }
+#ifdef DEBUG_SHOW
+    std::cout<<"trackedobjvec size:"<<trackedobjvec.obj_size()<<std::endl;
+#endif
+//    for (size_t i = 0; i < trackedobjvec.obj_size(); i++)
+//    {
+//        iv::fusion::fusionobject fusion_object;
+//        fusion_object = trackedobjvec.obj(i);
+//        std::cout<<"historical size:"<<fusion_object.point_historical_size()<<std::endl;
+//    }
+    regions.clear();
+    tracks.clear();
+    return trackedobjvec;
+}

src/fusion/lidar_radar_fusion_cnn_1025/Tracker/defines.h

@@ -0,0 +1,147 @@
+#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;
+typedef struct Size3D{
+    Size3D()
+    {
+    }
+    Size3D(float w, float h, float l)
+        :
+          width(w),
+          height(h),
+          length(l)
+    {
+    }
+    float length;
+    float width;
+    float height;
+} Size3D;
+typedef struct Rect3D{
+    Rect3D()
+    {
+    }
+    Rect3D(cv::Point3f c, Size3D s, float y)
+        :
+          center(c),
+          size(s),
+          yaw(y)
+    {
+    }
+    cv::Point3f center;
+    Size3D size;
+    float yaw;
+} Rect3D;
+///
+/// \brief config_t
+///
+typedef std::multimap<std::string, std::string> config_t;
+
+///
+/// \brief The CRegion class
+///
+class CRegion
+{
+public:
+    CRegion()
+        : m_type(-1), m_type_name(""), m_confidence(-1)
+    {
+    }
+
+    CRegion(const Rect3D& rect, const int& type, float confidence)
+        : m_rect(rect), m_type(type), m_confidence(confidence)
+    {
+        RBRect();
+    }
+
+    CRegion(const Rect3D& rect, const std::string& type, float confidence)
+        : m_rect(rect), m_type_name(type), m_confidence(confidence)
+    {
+        RBRect();
+    }
+    Rect3D m_rect;
+    cv::RotatedRect m_rrect;
+    cv::Rect m_brect;
+    int m_type = -1;
+    std::string m_type_name = "";
+    float m_confidence = -1;
+private:
+    ///
+    /// \brief B2RRect
+    /// \return
+    ///
+    cv::RotatedRect RBRect()
+    {
+        m_rrect = cv::RotatedRect(Point_t(m_rect.center.x,m_rect.center.y), cv::Size(m_rect.size.width,m_rect.size.height), 0);
+        m_brect = cv::Rect(m_rect.center.x-m_rect.size.width/2,m_rect.center.y-m_rect.size.height/2, m_rect.size.width,m_rect.size.height);
+        return m_rrect;
+    }
+};
+
+typedef std::vector<CRegion> regions_t;
+
+///
+///
+///
+namespace tracking
+{
+
+///
+/// \brief The DistType enum
+///
+enum DistType
+{
+    DistCenters,   // Euclidean distance between centers, pixels
+    DistRects,     // Euclidean distance between bounding rectangles, pixels
+    DistRect3Ds,     // Euclidean distance between bounding rectangles, pixels
+	DistsCount
+};
+
+///
+/// \brief The FilterGoal enum
+///
+enum FilterGoal
+{
+    FilterCenter, // x,y
+    FilterRect,   // x,y,w,h
+    FilterRect3D  // x,y,z,w,h,l,yaw
+};
+
+///
+/// \brief The KalmanType enum
+///
+enum KalmanType
+{
+    KalmanLinear
+};
+
+///
+/// \brief The MatchType enum
+///
+enum MatchType
+{
+    MatchHungrian
+};
+
+///
+/// \brief The LostTrackType enum
+///
+enum LostTrackType
+{
+    TrackNone
+};
+}

src/fusion/lidar_radar_fusion_cnn_1025/Tracker/track.cpp

@@ -0,0 +1,492 @@
+#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,
+        tracking::FilterGoal filterGoal,
+        tracking::LostTrackType externalTrackerForLost
+        )
+    :
+      m_trackID(trackID),
+      m_regionID(regionID),
+      m_skippedFrames(0),
+      m_lastRegion(region),
+      m_predictionPoint(region.m_rect.center),
+      m_predictionRect(region.m_rrect),
+      m_predictionRect3D(region.m_rect),
+      m_kalman(kalmanType, useAcceleration, deltaTime, accelNoiseMag),
+      m_filterGoal(filterGoal),
+      m_outOfTheFrame(false),
+      m_externalTrackerForLost(externalTrackerForLost)
+{
+    if (filterGoal == tracking::FilterRect)
+        m_kalman.Update(region.m_brect, true);
+    else if(filterGoal == tracking::FilterRect3D)
+        m_kalman.Update(region.m_rect, 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
+{
+    cv::Point3f diff = m_predictionPoint - reg.m_rect.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::CalcDistRect3D
+/// \param reg
+/// \return
+///
+track_t CTrack::CalcDistRect3D(const CRegion& reg) const
+{
+    std::array<track_t, 7> diff;
+    diff[0] = reg.m_rect.center.x - m_lastRegion.m_rect.center.x;
+    diff[1] = reg.m_rect.center.y - m_lastRegion.m_rect.center.y;
+    diff[2] = 0;//reg.m_rect.center.z - m_lastRegion.m_rect.center.z;
+    diff[3] = static_cast<track_t>(m_lastRegion.m_rect.size.width - reg.m_rect.size.width);
+    diff[4] = static_cast<track_t>(m_lastRegion.m_rect.size.height - reg.m_rect.size.height);
+    diff[5] = static_cast<track_t>(m_lastRegion.m_rect.size.length - reg.m_rect.size.length);
+    diff[6] = 0;//static_cast<track_t>(m_lastRegion.m_rect.yaw - reg.m_rect.yaw);
+
+    track_t dist = 0;
+    for (size_t i = 0; i < diff.size(); ++i)
+    {
+        dist += sqr(diff[i]);
+    }
+    return sqrtf(dist);
+}
+
+///
+/// \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_filterGoal == tracking::FilterRect) // Kalman filter for object coordinates and size
+        RectUpdate(region, dataCorrect, prevFrame, currFrame);
+    if (m_filterGoal == tracking::FilterRect3D) // Kalman filter for object coordinates and size
+        Rect3DUpdate(region, dataCorrect, prevFrame, currFrame);
+    else // Kalman filter only for object center
+        PointUpdate(region.m_rect.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_rect.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, fabs(d.x)), std::max(minRadius.height, 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_rect.size.width < reg.m_rect.size.width)
+        return m_lastRegion.m_rect.size.width / reg.m_rect.size.width;
+    else
+        return reg.m_rect.size.width / m_lastRegion.m_rect.size.width;
+}
+
+///
+/// \brief CTrack::HeightDist
+/// \param reg
+/// \return
+///
+track_t CTrack::HeightDist(const CRegion& reg) const
+{
+    if (m_lastRegion.m_rect.size.height < reg.m_rect.size.height)
+        return m_lastRegion.m_rect.size.height / reg.m_rect.size.height;
+    else
+        return reg.m_rect.size.height / m_lastRegion.m_rect.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_rect;
+            }
+
+            ++m_staticFrames;
+            m_isStatic = true;
+        }
+        else
+        {
+            m_isStatic = false;
+            m_staticFrames = 0;
+            m_staticFrame = cv::UMat();
+        }
+    }
+
+    return m_isStatic;
+}
+
+///
+/// \brief GetLastRect
+/// \return
+///
+Rect3D CTrack::GetLastRect() const
+{
+    if (m_filterGoal == tracking::FilterRect)
+        return Rect3D(cv::Point3f(m_predictionRect.center.x,m_predictionRect.center.y,0),Size3D(m_predictionRect.boundingRect().width,m_predictionRect.boundingRect().height,0),0);
+    else if(m_filterGoal == tracking::FilterRect3D)
+        return m_predictionRect3D;
+    else
+        return Rect3D(cv::Point3f(m_predictionPoint.x, m_predictionPoint.y,0),Size3D(0,0,0),0);
+
+}
+///
+/// \brief CTrack::LastRegion
+/// \return
+///
+const CRegion& CTrack::LastRegion() const
+{
+    return m_lastRegion;
+}
+
+///
+/// \brief CTrack::ConstructObject
+/// \return
+///
+TrackingObject CTrack::ConstructObject() const
+{
+    return TrackingObject(GetLastRect(), m_lastRegion, m_trackID, m_regionID, m_trace, IsStatic(), IsOutOfTheFrame(), m_kalman.GetVelocity(), m_detectedFrames);
+}
+
+///
+/// \brief CTrack::SkippedFrames
+/// \return
+///
+size_t CTrack::SkippedFrames() const
+{
+    return m_skippedFrames;
+}
+
+///
+/// \brief CTrack::SkippedFrames
+/// \return
+///
+size_t& CTrack::SkippedFrames()
+{
+    return m_skippedFrames;
+}
+///
+/// \brief CTrack::DetectedFrames
+/// \return
+///
+size_t& CTrack::DetectedFrames()
+{
+    return m_detectedFrames;
+}
+///
+/// \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;
+    }
+
+    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);
+
+    m_outOfTheFrame = (dx != 0) || (dy != 0) || (brect.width < 2) || (brect.height < 2);
+
+    m_predictionPoint = cv::Point3f(m_predictionRect.center.x,m_predictionRect.center.y,0);
+
+    //std::cout << "brect = " << brect << ", dx = " << dx << ", dy = " << dy << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
+}
+///
+/// \brief Rect3DUpdate
+/// \param region
+/// \param dataCorrect
+/// \param prevFrame
+/// \param currFrame
+///
+void CTrack::Rect3DUpdate(
+        const CRegion& region,
+        bool dataCorrect,
+        cv::UMat prevFrame,
+        cv::UMat currFrame
+        )
+{
+    m_kalman.GetRect3DPrediction();
+
+    switch (m_externalTrackerForLost)
+    {
+    case tracking::TrackNone:
+        break;
+    }
+
+    m_predictionRect3D = m_kalman.Update(region.m_rect, dataCorrect);
+
+    m_predictionPoint = m_predictionRect3D.center;
+
+    //std::cout << "brect = " << brect << ", dx = " << dx << ", dy = " << dy << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
+}
+
+///
+/// \brief PointUpdate
+/// \param pt
+/// \param dataCorrect
+///
+void CTrack::PointUpdate(
+        const cv::Point3f& pt,
+        const cv::Size& newObjSize,
+        bool dataCorrect,
+        const cv::Size& frameSize
+        )
+{
+    m_kalman.GetPointPrediction();
+
+    m_predictionPoint = m_kalman.Update(pt, dataCorrect);
+
+    if (dataCorrect)
+    {
+        const int a1 = 1;
+        const int a2 = 9;
+        m_predictionRect.size.width = (a1 * newObjSize.width + a2 * m_predictionRect.size.width) / (a1 + a2);
+        m_predictionRect.size.height = (a1 * newObjSize.height + a2 * m_predictionRect.size.height) / (a1 + a2);
+    }
+
+    auto Clamp = [](track_t& v, int hi) -> bool
+    {
+        if (v < 0)
+        {
+            v = 0;
+            return true;
+        }
+        else if (hi && v > hi - 1)
+        {
+            v = static_cast<track_t>(hi - 1);
+            return true;
+        }
+        return false;
+    };
+    auto p = m_predictionPoint;
+    m_outOfTheFrame = Clamp(p.x, frameSize.width) || Clamp(p.y, frameSize.height) || (m_predictionRect.size.width < 2) || (m_predictionRect.size.height < 2);
+
+    //std::cout << "predictionRect = " << m_predictionRect.boundingRect() << ", outOfTheFrame = " << m_outOfTheFrame << ", predictionPoint = " << m_predictionPoint << std::endl;
+}

src/fusion/lidar_radar_fusion_cnn_1025/Tracker/track.h

@@ -0,0 +1,303 @@
+#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"
+
+// --------------------------------------------------------------------------
+///
+/// \brief The TrajectoryPoint struct
+///
+struct TrajectoryPoint
+{
+    ///
+    /// \brief TrajectoryPoint
+    ///
+    TrajectoryPoint()
+        : m_hasRaw(false)
+    {
+    }
+
+    ///
+    /// \brief TrajectoryPoint
+    /// \param prediction
+    ///
+    TrajectoryPoint(const cv::Point3f& prediction)
+        :
+          m_hasRaw(false),
+          m_prediction(prediction)
+    {
+    }
+
+    ///
+    /// \brief TrajectoryPoint
+    /// \param prediction
+    /// \param raw
+    ///
+    TrajectoryPoint(const cv::Point3f& prediction, const cv::Point3f& raw)
+        :
+          m_hasRaw(true),
+          m_prediction(prediction),
+          m_raw(raw)
+    {
+    }
+
+    bool m_hasRaw = false;
+    cv::Point3f m_prediction;
+    cv::Point3f m_raw;
+};
+
+// --------------------------------------------------------------------------
+///
+/// \brief The Trace class
+///
+class Trace
+{
+public:
+    ///
+    /// \brief operator []
+    /// \param i
+    /// \return
+    ///
+    const cv::Point3f& operator[](size_t i) const
+    {
+        return m_trace[i].m_prediction;
+    }
+
+    ///
+    /// \brief operator []
+    /// \param i
+    /// \return
+    ///
+    cv::Point3f& 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 cv::Point3f& prediction)
+    {
+        m_trace.emplace_back(prediction);
+    }
+    void push_back(const cv::Point3f& prediction, const cv::Point3f& 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
+{
+    Rect3D m_rect;           // Coordinates
+    CRegion m_region;         // detect region
+    Trace m_trace;                     // Trajectory
+    size_t m_ID = 0;                   // Objects ID
+    size_t m_regionID = 0;                   // Objects ID
+    bool m_isStatic = false;           // Object is abandoned
+    bool m_outOfTheFrame = false;      // Is object out of freme
+    cv::Vec<track_t, 2> m_velocity;    // pixels/sec
+    size_t m_detectedFrames = 0;       // detected frames' count
+    ///
+    TrackingObject(const Rect3D& rect, const CRegion& region, size_t ID, size_t regionID, const Trace& trace,
+                   bool isStatic, bool outOfTheFrame, cv::Vec<track_t, 2> velocity, size_t detectedFrames)
+        :
+          m_rect(rect), m_region(region), m_ID(ID), m_regionID(regionID), m_isStatic(isStatic), m_outOfTheFrame(outOfTheFrame), m_velocity(velocity), m_detectedFrames(detectedFrames)
+    {
+        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_rect.size.width / m_rect.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,
+           tracking::FilterGoal filterObjectGoal,
+           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 CalcDist
+    /// Euclidean distance in pixels between object contours on two N and N+1 frames
+    /// \param reg
+    /// \return
+    ///
+    track_t CalcDistRect3D(const CRegion& reg) 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;
+
+    Rect3D GetLastRect() const;
+
+    const cv::Point3f& AveragePoint() const;
+    cv::Point3f& AveragePoint();
+    const CRegion& LastRegion() const;
+    size_t SkippedFrames() const;
+    size_t& SkippedFrames();
+    size_t& DetectedFrames();
+
+    TrackingObject ConstructObject() const;
+public:
+    size_t m_regionID = 0;
+private:
+    Trace m_trace;
+    size_t m_trackID = 0;
+    size_t m_skippedFrames = 0;
+    size_t m_detectedFrames = 0;
+    CRegion m_lastRegion;
+
+    cv::Point3f m_predictionPoint;
+    cv::RotatedRect m_predictionRect;
+    Rect3D m_predictionRect3D;
+    TKalmanFilter m_kalman;
+    tracking::FilterGoal m_filterGoal = tracking::FilterCenter;
+    bool m_outOfTheFrame = false;
+
+    tracking::LostTrackType m_externalTrackerForLost;
+
+    void RectUpdate(const CRegion& region, bool dataCorrect, cv::UMat prevFrame, cv::UMat currFrame);
+    void Rect3DUpdate(const CRegion& region, bool dataCorrect, cv::UMat prevFrame, cv::UMat currFrame);
+    void PointUpdate(const cv::Point3f& 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;
+    Rect3D m_staticRect;
+};
+
+typedef std::vector<std::unique_ptr<CTrack>> tracks_t;
+

src/fusion/lidar_radar_fusion_cnn_1025/fusion.hpp

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

src/fusion/lidar_radar_fusion_cnn_1025/fusion_probabilities.cpp

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

src/fusion/lidar_radar_fusion_cnn_1025/fusion_probabilities.h

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

src/fusion/lidar_radar_fusion_cnn_1025/imageBuffer.h

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

src/fusion/lidar_radar_fusion_cnn_1025/lidar_radar_fusion_cnn.pro

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

src/fusion/lidar_radar_fusion_cnn_1025/main.cpp

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

src/fusion/lidar_radar_fusion_cnn_1025/perceptionoutput.cpp

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

src/fusion/lidar_radar_fusion_cnn_1025/transformation.cpp

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

src/fusion/lidar_radar_fusion_cnn_1025/transformation.h

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