modify JS_guide

src/decition/common/common/car_status.h

@@ -163,8 +163,8 @@ namespace iv {
         iv::roadmode_vel mroadmode_vel;
         int vehGroupId;
         double mfEPSOff = 0.0;
-        float socfDis=5;   //停障触发距离
-        float aocfDis=23;   //避障触发距离//20
+        float socfDis=8;   //停障触发距离
+        float aocfDis=25;   //避障触发距离//20
         float aocfTimes=3; //避障触发次数
         float aobzDis=5;   //避障保障距离
         /// traffice light : 0x90

src/decition/decition_brain_sf_jsguide/decition/brain.cpp

@@ -297,6 +297,8 @@ void iv::decition::BrainDecition::run() {
     ServiceCarStatus.msysparam.vehWidth = atof(xp.GetParam("vehWidth","2.1").data());
     ServiceCarStatus.msysparam.vehLenth = atof(xp.GetParam("vehLenth","4.6").data());
 
+    ServiceCarStatus.msysparam.mbSideDrive = xp.GetParam("SideDrive",false);
+    ServiceCarStatus.msysparam.mfSideDis = xp.GetParam("SideDis",0.3);
 
 
 
@@ -1338,7 +1340,10 @@ void iv::decition::BrainDecition::SideMove(iv::GPS_INS &x)
     double rel_x1,rel_y1;
     rel_x1 = rel_x + fmove * cos(fhdg + M_PI/2.0);
     rel_y1 = rel_y + fmove * sin(fhdg + M_PI/2.0);
+
     GaussProjInvCal(rel_x1,rel_y1,&x.gps_lng,&x.gps_lat);
+    x.gps_x = rel_x1;
+    x.gps_y = rel_y1;
 }
 void iv::decition::BrainDecition::UpdateMap(const char *mapdata, const int mapdatasize)
 {

src/decition/decition_brain_sf_jsguide/decition/decide_gps_00.cpp

@@ -2355,13 +2355,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
          minDecelerate=min(-1.0f,minDecelerate);
      }
 
-     if((ServiceCarStatus.mbUltraDis[1]>=1.0&&ServiceCarStatus.mbUltraDis[1]<5.0)||
-        (ServiceCarStatus.mbUltraDis[3]>=1.0&&ServiceCarStatus.mbUltraDis[3]<5.0))
+     if((ServiceCarStatus.mbUltraDis[1]>=0.5&&ServiceCarStatus.mbUltraDis[1]<1.0)||
+        (ServiceCarStatus.mbUltraDis[3]>=0.5&&ServiceCarStatus.mbUltraDis[3]<1.0))
      {
         dSpeed=min(dSpeed,3.0);
      }
-     else if((ServiceCarStatus.mbUltraDis[1]>=0.0&&ServiceCarStatus.mbUltraDis[1]<1.0)||
-             (ServiceCarStatus.mbUltraDis[3]>=0.0&&ServiceCarStatus.mbUltraDis[3]<1.0))
+     else if((ServiceCarStatus.mbUltraDis[1]>=0.0&&ServiceCarStatus.mbUltraDis[1]<0.5)||
+             (ServiceCarStatus.mbUltraDis[3]>=0.0&&ServiceCarStatus.mbUltraDis[3]<0.5))
      {
          minDecelerate=min(-0.5f,minDecelerate);
      }

src/detection/detection_lidar_PointPillars_MultiHead/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/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/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/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/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/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/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/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/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/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/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/roiaware_pool3d.h

@@ -0,0 +1,25 @@
+/*
+RoI-aware point cloud feature pooling
+Reference paper:  https://arxiv.org/abs/1907.03670
+Written by Shaoshuai Shi
+All Rights Reserved 2019-2020.
+*/
+
+
+#include <assert.h>
+
+
+//#define CHECK_CUDA(x) AT_CHECK(x.type().is_cuda(), #x, " must be a CUDAtensor ")
+//#define CHECK_CONTIGUOUS(x) AT_CHECK(x.is_contiguous(), #x, " must be contiguous ")
+//#define CHECK_INPUT(x) CHECK_CUDA(x);CHECK_CONTIGUOUS(x)
+
+
+void roiaware_pool3d_launcher(int boxes_num, int pts_num, int channels, int max_pts_each_voxel,
+    int out_x, int out_y, int out_z, const float *rois, const float *pts, const float *pts_feature,
+    int *argmax, int *pts_idx_of_voxels, float *pooled_features, int pool_method);
+
+void roiaware_pool3d_backward_launcher(int boxes_num, int out_x, int out_y, int out_z, int channels, int max_pts_each_voxel,
+    const int *pts_idx_of_voxels, const int *argmax, const float *grad_out, float *grad_in, int pool_method);
+
+void points_in_boxes_launcher(int batch_size, int boxes_num, int pts_num, const float *boxes,
+    const float *pts, int *box_idx_of_points);

src/detection/detection_lidar_PointPillars_MultiHead/roiaware_pool3d_kernel.cu

@@ -0,0 +1,361 @@
+/*
+RoI-aware point cloud feature pooling
+Written by Shaoshuai Shi
+All Rights Reserved 2019-2020.
+*/
+
+
+#include <math.h>
+#include <stdio.h>
+
+#include "roiaware_pool3d.h"
+
+#define THREADS_PER_BLOCK 256
+#define DIVUP(m,n) ((m) / (n) + ((m) % (n) > 0))
+// #define DEBUG
+
+
+__device__ inline void lidar_to_local_coords(float shift_x, float shift_y, float rot_angle, float &local_x, float &local_y){
+    float cosa = cos(-rot_angle), sina = sin(-rot_angle);
+    local_x = shift_x * cosa + shift_y * (-sina);
+    local_y = shift_x * sina + shift_y * cosa;
+}
+
+
+__device__ inline int check_pt_in_box3d(const float *pt, const float *box3d, float &local_x, float &local_y){
+    // param pt: (x, y, z)
+    // param box3d: [x, y, z, dx, dy, dz, heading] (x, y, z) is the box center
+
+    const float MARGIN = 1e-5;
+    float x = pt[0], y = pt[1], z = pt[2];
+    float cx = box3d[0], cy = box3d[1], cz = box3d[2];
+    float dx = box3d[3], dy = box3d[4], dz = box3d[5], rz = box3d[6];
+
+    if (fabsf(z - cz) > dz / 2.0) return 0;
+    lidar_to_local_coords(x - cx, y - cy, rz, local_x, local_y);
+    float in_flag = (fabs(local_x) < dx / 2.0 + MARGIN) & (fabs(local_y) < dy / 2.0 + MARGIN);
+    return in_flag;
+}
+
+
+__global__ void generate_pts_mask_for_box3d(int boxes_num, int pts_num, int out_x, int out_y, int out_z,
+    const float *rois, const float *pts, int *pts_mask){
+    // params rois: [x, y, z, dx, dy, dz, heading] (x, y, z) is the box center
+    // params pts: (npoints, 3) [x, y, z]
+    // params pts_mask: (N, npoints): -1 means point doesnot in this box, otherwise: encode (x_idxs, y_idxs, z_idxs) by binary bit
+    int pt_idx = blockIdx.x * blockDim.x + threadIdx.x;
+    int box_idx = blockIdx.y;
+    if (pt_idx >= pts_num || box_idx >= boxes_num) return;
+
+    pts += pt_idx * 3;
+    rois += box_idx * 7;
+    pts_mask += box_idx * pts_num + pt_idx;
+
+    float local_x = 0, local_y = 0;
+    int cur_in_flag = check_pt_in_box3d(pts, rois, local_x, local_y);
+
+    pts_mask[0] = -1;
+    if (cur_in_flag > 0){
+        float local_z = pts[2] - rois[2];
+        float dx = rois[3], dy = rois[4], dz = rois[5];
+
+        float x_res = dx / out_x;
+        float y_res = dy / out_y;
+        float z_res = dz / out_z;
+
+        unsigned int x_idx = int((local_x + dx / 2) / x_res);
+        unsigned int y_idx = int((local_y + dy / 2) / y_res);
+        unsigned int z_idx = int((local_z + dz / 2) / z_res);
+
+        x_idx = min(max(x_idx, 0), out_x - 1);
+        y_idx = min(max(y_idx, 0), out_y - 1);
+        z_idx = min(max(z_idx, 0), out_z - 1);
+
+        unsigned int idx_encoding = (x_idx << 16) + (y_idx << 8) + z_idx;
+        pts_mask[0] = idx_encoding;
+    }
+}
+
+
+__global__ void collect_inside_pts_for_box3d(int boxes_num, int pts_num, int max_pts_each_voxel,
+    int out_x, int out_y, int out_z, const int *pts_mask, int *pts_idx_of_voxels){
+    // params pts_mask: (N, npoints)  0 or 1
+    // params pts_idx_of_voxels: (N, out_x, out_y, out_z, max_pts_each_voxel)
+
+    int box_idx = blockIdx.x * blockDim.x + threadIdx.x;
+    if (box_idx >= boxes_num) return;
+
+    int max_num_pts = max_pts_each_voxel - 1;  // index 0 is the counter
+    pts_idx_of_voxels += box_idx * out_x * out_y * out_z * max_pts_each_voxel;
+
+    for (int k = 0; k < pts_num; k++){
+        if (pts_mask[box_idx * pts_num + k] != -1){
+            unsigned int idx_encoding = pts_mask[box_idx * pts_num + k];
+            unsigned int x_idx = (idx_encoding >> 16) & 0xFF;
+            unsigned int y_idx = (idx_encoding >> 8) & 0xFF;
+            unsigned int z_idx = idx_encoding & 0xFF;
+            unsigned int base_offset = x_idx * out_y * out_z * max_pts_each_voxel + y_idx * out_z * max_pts_each_voxel + z_idx * max_pts_each_voxel;
+            unsigned int cnt = pts_idx_of_voxels[base_offset];
+            if (cnt < max_num_pts){
+                pts_idx_of_voxels[base_offset + cnt + 1] = k;
+                pts_idx_of_voxels[base_offset]++;
+            }
+#ifdef DEBUG
+        printf("collect: pts_%d, idx(%d, %d, %d), idx_encoding=%x\n",
+            k, x_idx, y_idx, z_idx, idx_encoding);
+#endif
+
+        }
+    }
+}
+
+
+__global__ void roiaware_maxpool3d(int boxes_num, int pts_num, int channels, int max_pts_each_voxel, int out_x,
+    int out_y, int out_z, const float *pts_feature, const int *pts_idx_of_voxels, float *pooled_features, int *argmax){
+    // params pts_feature: (npoints, C)
+    // params pts_idx_of_voxels: (N, out_x, out_y, out_z, max_pts_each_voxel), index 0 is the counter
+    // params pooled_features: (N, out_x, out_y, out_z, C)
+    // params argmax: (N, out_x, out_y, out_z, C)
+
+    int box_idx = blockIdx.z;
+    int channel_idx = blockIdx.y;
+    int voxel_idx_flat = blockIdx.x * blockDim.x + threadIdx.x;
+
+    int x_idx = voxel_idx_flat / (out_y * out_z);
+    int y_idx = (voxel_idx_flat - x_idx * (out_y * out_z)) / out_z;
+    int z_idx = voxel_idx_flat % out_z;
+    if (box_idx >= boxes_num || channel_idx >= channels|| x_idx >= out_x || y_idx >= out_y || z_idx >= out_z) return;
+
+#ifdef DEBUG
+    printf("src pts_idx_of_voxels: (%p, ), argmax: %p\n", pts_idx_of_voxels, argmax);
+#endif
+
+    int offset_base = x_idx * out_y * out_z + y_idx * out_z + z_idx;
+    pts_idx_of_voxels += box_idx * out_x * out_y * out_z * max_pts_each_voxel + offset_base * max_pts_each_voxel;
+    pooled_features += box_idx * out_x * out_y * out_z * channels + offset_base * channels + channel_idx;
+    argmax += box_idx * out_x * out_y * out_z * channels + offset_base * channels + channel_idx;
+
+    int argmax_idx = -1;
+    float max_val = -1e50;
+
+    int total_pts = pts_idx_of_voxels[0];
+
+    for (int k = 1; k <= total_pts; k++){
+        if (pts_feature[pts_idx_of_voxels[k] * channels + channel_idx] > max_val){
+            max_val = pts_feature[pts_idx_of_voxels[k] * channels + channel_idx];
+            argmax_idx = pts_idx_of_voxels[k];
+        }
+    }
+
+    if (argmax_idx != -1){
+        pooled_features[0] = max_val;
+    }
+    argmax[0] = argmax_idx;
+
+#ifdef DEBUG
+    printf("channel_%d idx(%d, %d, %d), argmax_idx=(%d, %.3f), total=%d, after pts_idx: %p, argmax: (%p, %d)\n",
+        channel_idx, x_idx, y_idx, z_idx, argmax_idx, max_val, total_pts, pts_idx_of_voxels, argmax, argmax_idx);
+#endif
+}
+
+
+__global__ void roiaware_avgpool3d(int boxes_num, int pts_num, int channels, int max_pts_each_voxel, int out_x,
+    int out_y, int out_z, const float *pts_feature, const int *pts_idx_of_voxels, float *pooled_features){
+    // params pts_feature: (npoints, C)
+    // params pts_idx_of_voxels: (N, out_x, out_y, out_z, max_pts_each_voxel), index 0 is the counter
+    // params pooled_features: (N, out_x, out_y, out_z, C)
+    // params argmax: (N, out_x, out_y, out_z, C)
+
+    int box_idx = blockIdx.z;
+    int channel_idx = blockIdx.y;
+    int voxel_idx_flat = blockIdx.x * blockDim.x + threadIdx.x;
+
+    int x_idx = voxel_idx_flat / (out_y * out_z);
+    int y_idx = (voxel_idx_flat - x_idx * (out_y * out_z)) / out_z;
+    int z_idx = voxel_idx_flat % out_z;
+    if (box_idx >= boxes_num || channel_idx >= channels|| x_idx >= out_x || y_idx >= out_y || z_idx >= out_z) return;
+
+    int offset_base = x_idx * out_y * out_z + y_idx * out_z + z_idx;
+    pts_idx_of_voxels += box_idx * out_x * out_y * out_z * max_pts_each_voxel + offset_base * max_pts_each_voxel;
+    pooled_features += box_idx * out_x * out_y * out_z * channels + offset_base * channels + channel_idx;
+
+    float sum_val = 0;
+    int total_pts = pts_idx_of_voxels[0];
+
+    for (int k = 1; k <= total_pts; k++){
+        sum_val += pts_feature[pts_idx_of_voxels[k] * channels + channel_idx];
+    }
+
+    if (total_pts > 0){
+        pooled_features[0] = sum_val / total_pts;
+    }
+}
+
+
+void roiaware_pool3d_launcher(int boxes_num, int pts_num, int channels, int max_pts_each_voxel, int out_x, int out_y, int out_z,
+    const float *rois, const float *pts, const float *pts_feature, int *argmax, int *pts_idx_of_voxels, float *pooled_features, int pool_method){
+    // params rois: (N, 7) [x, y, z, dx, dy, dz, heading] (x, y, z) is the box center
+    // params pts: (npoints, 3) [x, y, z]
+    // params pts_feature: (npoints, C)
+    // params argmax: (N, out_x, out_y, out_z, C)
+    // params pts_idx_of_voxels: (N, out_x, out_y, out_z, max_pts_each_voxel)
+    // params pooled_features: (N, out_x, out_y, out_z, C)
+    // params pool_method: 0: max_pool 1: avg_pool
+
+    int *pts_mask = NULL;
+    cudaMalloc(&pts_mask, boxes_num * pts_num * sizeof(int));  // (N, M)
+    cudaMemset(pts_mask, -1, boxes_num * pts_num * sizeof(int));
+
+    dim3 blocks_mask(DIVUP(pts_num, THREADS_PER_BLOCK), boxes_num);
+    dim3 threads(THREADS_PER_BLOCK);
+    generate_pts_mask_for_box3d<<<blocks_mask, threads>>>(boxes_num, pts_num, out_x, out_y, out_z, rois, pts, pts_mask);
+
+    // TODO: Merge the collect and pool functions, SS
+
+    dim3 blocks_collect(DIVUP(boxes_num, THREADS_PER_BLOCK));
+    collect_inside_pts_for_box3d<<<blocks_collect, threads>>>(boxes_num, pts_num, max_pts_each_voxel,
+        out_x, out_y, out_z, pts_mask, pts_idx_of_voxels);
+
+    dim3 blocks_pool(DIVUP(out_x * out_y * out_z, THREADS_PER_BLOCK), channels, boxes_num);
+    if (pool_method == 0){
+        roiaware_maxpool3d<<<blocks_pool, threads>>>(boxes_num, pts_num, channels, max_pts_each_voxel, out_x, out_y, out_z,
+            pts_feature, pts_idx_of_voxels, pooled_features, argmax);
+    }
+    else if (pool_method == 1){
+        roiaware_avgpool3d<<<blocks_pool, threads>>>(boxes_num, pts_num, channels, max_pts_each_voxel, out_x, out_y, out_z,
+            pts_feature, pts_idx_of_voxels, pooled_features);
+    }
+
+
+    cudaFree(pts_mask);
+
+#ifdef DEBUG
+    cudaDeviceSynchronize();  // for using printf in kernel function
+#endif
+}
+
+
+__global__ void roiaware_maxpool3d_backward(int boxes_num, int channels, int out_x, int out_y, int out_z,
+    const int *argmax, const float *grad_out, float *grad_in){
+    // params argmax: (N, out_x, out_y, out_z, C)
+    // params grad_out: (N, out_x, out_y, out_z, C)
+    // params grad_in: (npoints, C), return value
+
+    int box_idx = blockIdx.z;
+    int channel_idx = blockIdx.y;
+    int voxel_idx_flat = blockIdx.x * blockDim.x + threadIdx.x;
+
+    int x_idx = voxel_idx_flat / (out_y * out_z);
+    int y_idx = (voxel_idx_flat - x_idx * (out_y * out_z)) / out_z;
+    int z_idx = voxel_idx_flat % out_z;
+    if (box_idx >= boxes_num || channel_idx >= channels|| x_idx >= out_x || y_idx >= out_y || z_idx >= out_z) return;
+
+    int offset_base = x_idx * out_y * out_z + y_idx * out_z + z_idx;
+    argmax += box_idx * out_x * out_y * out_z * channels + offset_base * channels + channel_idx;
+    grad_out += box_idx * out_x * out_y * out_z * channels + offset_base * channels + channel_idx;
+
+    if (argmax[0] == -1) return;
+
+    atomicAdd(grad_in + argmax[0] * channels + channel_idx, grad_out[0] * 1);
+}
+
+
+__global__ void roiaware_avgpool3d_backward(int boxes_num, int channels, int out_x, int out_y, int out_z,
+    int max_pts_each_voxel, const int *pts_idx_of_voxels, const float *grad_out, float *grad_in){
+    // params pts_idx_of_voxels: (N, out_x, out_y, out_z, max_pts_each_voxel)
+    // params grad_out: (N, out_x, out_y, out_z, C)
+    // params grad_in: (npoints, C), return value
+
+    int box_idx = blockIdx.z;
+    int channel_idx = blockIdx.y;
+    int voxel_idx_flat = blockIdx.x * blockDim.x + threadIdx.x;
+
+    int x_idx = voxel_idx_flat / (out_y * out_z);
+    int y_idx = (voxel_idx_flat - x_idx * (out_y * out_z)) / out_z;
+    int z_idx = voxel_idx_flat % out_z;
+    if (box_idx >= boxes_num || channel_idx >= channels|| x_idx >= out_x || y_idx >= out_y || z_idx >= out_z) return;
+
+    int offset_base = x_idx * out_y * out_z + y_idx * out_z + z_idx;
+    pts_idx_of_voxels += box_idx * out_x * out_y * out_z * max_pts_each_voxel + offset_base * max_pts_each_voxel;
+    grad_out += box_idx * out_x * out_y * out_z * channels + offset_base * channels + channel_idx;
+
+
+    int total_pts = pts_idx_of_voxels[0];
+    float cur_grad = 1 / fmaxf(float(total_pts), 1.0);
+    for (int k = 1; k <= total_pts; k++){
+        atomicAdd(grad_in + pts_idx_of_voxels[k] * channels + channel_idx, grad_out[0] * cur_grad);
+    }
+}
+
+
+void roiaware_pool3d_backward_launcher(int boxes_num, int out_x, int out_y, int out_z, int channels, int max_pts_each_voxel,
+    const int *pts_idx_of_voxels, const int *argmax, const float *grad_out, float *grad_in, int pool_method){
+    // params pts_idx_of_voxels: (N, out_x, out_y, out_z, max_pts_each_voxel)
+    // params argmax: (N, out_x, out_y, out_z, C)
+    // params grad_out: (N, out_x, out_y, out_z, C)
+    // params grad_in: (npoints, C), return value
+    // params pool_method: 0: max_pool, 1: avg_pool
+
+    dim3 blocks(DIVUP(out_x * out_y * out_z, THREADS_PER_BLOCK), channels, boxes_num);
+    dim3 threads(THREADS_PER_BLOCK);
+    if (pool_method == 0){
+        roiaware_maxpool3d_backward<<<blocks, threads>>>(
+            boxes_num, channels, out_x, out_y, out_z, argmax, grad_out, grad_in
+        );
+    }
+    else if (pool_method == 1){
+        roiaware_avgpool3d_backward<<<blocks, threads>>>(
+            boxes_num, channels, out_x, out_y, out_z, max_pts_each_voxel, pts_idx_of_voxels, grad_out, grad_in
+        );
+    }
+
+}
+
+
+__global__ void points_in_boxes_kernel(int batch_size, int boxes_num, int pts_num, const float *boxes,
+    const float *pts, int *box_idx_of_points){
+    // params boxes: (B, N, 7) [x, y, z, dx, dy, dz, heading] (x, y, z) is the box center
+    // params pts: (B, npoints, 3) [x, y, z] in LiDAR coordinate
+    // params boxes_idx_of_points: (B, npoints), default -1
+
+    int bs_idx = blockIdx.y;
+    int pt_idx = blockIdx.x * blockDim.x + threadIdx.x;
+    if (bs_idx >= batch_size || pt_idx >= pts_num) return;
+
+    boxes += bs_idx * boxes_num * 7;
+    pts += bs_idx * pts_num * 5 + pt_idx * 5;
+    box_idx_of_points += bs_idx * pts_num + pt_idx;
+
+    float local_x = 0, local_y = 0;
+    int cur_in_flag = 0;
+    for (int k = 0; k < boxes_num; k++){
+        cur_in_flag = check_pt_in_box3d(pts, boxes + k * 7, local_x, local_y);
+        if (cur_in_flag){
+            box_idx_of_points[0] = k;
+            break;
+        }
+    }
+}
+
+
+void points_in_boxes_launcher(int batch_size, int boxes_num, int pts_num, const float *boxes,
+    const float *pts, int *box_idx_of_points){
+    // params boxes: (B, N, 7) [x, y, z, dx, dy, dz, heading] (x, y, z) is the box center
+    // params pts: (B, npoints, 3) [x, y, z]
+    // params boxes_idx_of_points: (B, npoints), default -1
+    cudaError_t err;
+
+    dim3 blocks(DIVUP(pts_num, THREADS_PER_BLOCK), batch_size);
+    dim3 threads(THREADS_PER_BLOCK);
+    points_in_boxes_kernel<<<blocks, threads>>>(batch_size, boxes_num, pts_num, boxes, pts, box_idx_of_points);
+
+    err = cudaGetLastError();
+    if (cudaSuccess != err) {
+        fprintf(stderr, "CUDA kernel failed : %s\n", cudaGetErrorString(err));
+        exit(-1);
+    }
+
+#ifdef DEBUG
+    cudaDeviceSynchronize();  // for using printf in kernel function
+#endif
+}

src/driver/driver_map_xodrload/main.cpp

@@ -35,6 +35,7 @@
 #include <signal.h>
 
 #include "service_roi_xodr.h"
+#include "ivservice.h"
 
 OpenDrive mxodr;
 xodrdijkstra * gpxd;
@@ -46,6 +47,8 @@ double gvehiclewidth = 2.0;
 
 bool gbExtendMap = true;
 
+static bool gbSideEnable = false;
+
 void * gpa;
 void * gpasrc;
 void * gpmap;
@@ -59,6 +62,8 @@ iv::Ivlog *givlog = nullptr;
 
 static QCoreApplication * gApp;
 
+service_roi_xodr gsrx;
+
 
 namespace iv {
 struct simpletrace
@@ -141,8 +146,7 @@ bool LoadXODR(std::string strpath)
     givlog->info("road name is %s",proad1->GetRoadName().data());
     std::cout<<" road name is "<<proad1->GetRoadName()<<std::endl;
 
-    service_roi_xodr * psrx = new service_roi_xodr();
-    psrx->SetXODR(&mxodr);
+    gsrx.SetXODR(&mxodr);
 }
 
 class roadx
@@ -468,6 +472,49 @@ inline bool isboringroad(int nroadid)
     return brtn;
 }
 
+
+void CalcSide(std::vector<PlanPoint> & xPlan)
+{
+    const double fsidedis = 0.3;
+    const int nChangePoint = 150;
+    const int nStopPoint = 50;
+    if(xPlan.size()<nChangePoint)return;
+    bool bChange = true;
+    int i;
+    int nsize = xPlan.size();
+    for(i=(nsize-1);i>=(nsize - nChangePoint);i--)
+    {
+        if(xPlan[i].mWidth<(2.0*(gvehiclewidth/2.0+fsidedis)))
+        {
+            std::cout<<" Because Lane is narrow, not change."<<std::endl;
+            bChange = false;
+            break;
+        }
+    }
+
+    if(bChange == false)return;
+
+    for(i=(nsize-1);i>=(nsize - nStopPoint);i--)
+    {
+        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        xPlan[i].x = xold + fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fMove*sin(xPlan[i].hdg - M_PI/2.0);
+    }
+
+    for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
+    {
+        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        double fRatio = 1.0 - ((nsize-nStopPoint) -i )*1.0/(nChangePoint-nStopPoint);
+        xPlan[i].x = xold + fRatio*fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fRatio*fMove*sin(xPlan[i].hdg - M_PI/2.0);
+    }
+    return;
+}
+
 void SetPlan(xodrobj xo)
 {
 
@@ -494,6 +541,11 @@ void SetPlan(xodrobj xo)
     int i;
     int nSize = xPlan.size();
 
+    if(gbSideEnable)
+    {
+        CalcSide(xPlan);
+    }
+
     if(nSize<1)
     {
         qDebug("plan fail.");
@@ -986,6 +1038,35 @@ void signal_handler(int sig)
     }
 }
 
+void ProcROIReq(std::shared_ptr<char> pstr_req,const int nreqsize,std::shared_ptr<char> & pstr_res,int & nressize)
+{
+    (void)pstr_req;
+    (void)nreqsize;
+    std::shared_ptr<iv::roi::request> pstr_roireq = std::shared_ptr<iv::roi::request>(new iv::roi::request);
+    if(pstr_roireq->ParseFromArray(pstr_req.get(),nreqsize))
+    {
+        std::shared_ptr<iv::roi::resultarray> pstr_roires;
+        gsrx.GetROI(pstr_roireq,pstr_roires);
+        int nbytesize = pstr_roires->ByteSize();
+        pstr_res = std::shared_ptr<char>(new char[nbytesize]);
+        if(pstr_roires->SerializeToArray(pstr_res.get(),nbytesize))
+        {
+            nressize = nbytesize;
+            std::cout<<"return res."<<std::endl;
+            return;
+        }
+        else
+        {
+            std::cout<<" ProcROIReq fail serizlizetoarray"<<std::endl;
+        }
+    }
+    else
+    {
+        std::cout<<" ProcROIReq parse pstr_req fail."<<std::endl;
+        return;
+    }
+}
+
 int main(int argc, char *argv[])
 {
     showversion("driver_map_xodrload");
@@ -1027,6 +1108,8 @@ int main(int argc, char *argv[])
 
     std::string strextendmap = xp.GetParam("extendmap","false");
 
+    std::string strsideenable = xp.GetParam("sideenable","true");
+
 
     glat0 = atof(strlat0.data());
     glon0 = atof(strlon0.data());
@@ -1043,9 +1126,17 @@ int main(int argc, char *argv[])
         gbExtendMap = false;
     }
 
+    if(strsideenable == "true")
+    {
+        gbSideEnable = true;
+    }
+
 
     LoadXODR(strmapth);
 
+    iv::service::Server serviceroi("ServiceROI",ProcROIReq);
+    (void)serviceroi;
+
     gpmap = iv::modulecomm::RegisterSend("tracemap",20000000,1);
     gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);
     gpanewtrace = iv::modulecomm::RegisterSend("newtracemap",20000000,1);

src/driver/driver_map_xodrload/planpoint.h

@@ -16,7 +16,7 @@ public:
     double hdg;
     double dis;
     double mS;
-    double mWidth;
+    double mWidth;  //Current Lane Width
     double mLeftWidth[5];
     double mRightWidth[5];
 

src/driver/driver_map_xodrload/service_roi_xodr.cpp

@@ -15,9 +15,18 @@ void service_roi_xodr::SetXODR(OpenDrive *pxodr)
     updateroadarea();
 }
 
-int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::shared_ptr<iv::roi::resultarray> &xres_prt)
+
+#include <QTime>
+int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::shared_ptr<iv::roi::resultarray> &xres_ptr)
 {
 
+    xres_ptr = std::shared_ptr<iv::roi::resultarray>(new iv::roi::resultarray);
+    xres_ptr->set_lat(xreq_ptr->lat());
+    xres_ptr->set_lon(xreq_ptr->lon());
+    xres_ptr->set_heading(xreq_ptr->heading());
+    xres_ptr->set_range(xreq_ptr->range());
+    xres_ptr->set_gridsize(xreq_ptr->gridsize());
+
     double x0,y0;
     double x,y;
     GaussProjCal(xreq_ptr->lon(),xreq_ptr->lat(),&x,&y);
@@ -26,7 +35,13 @@ int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::sh
     y = y - y0;
     unsigned int nroadsize = mvectorarea.size();
     unsigned int i;
+    unsigned int j;
     std::vector<iv::roi::area> xvectorarea;
+
+    QTime xTime;
+    xTime.start();
+
+    //Find Near ROI area
     for(i=0;i<nroadsize;i++)
     {
         double fdis1;
@@ -37,7 +52,7 @@ int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::sh
             continue;
         }
         unsigned int nareasize = proadarea->mvectorarea.size();
-        unsigned int j;
+
         for(j=0;j<nareasize;j++)
         {
             iv::roi::area * parea = &proadarea->mvectorarea[j];
@@ -67,6 +82,96 @@ int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::sh
             }
         }
     }
+
+    std::cout<<"xTime: "<<xTime.elapsed()<<std::endl;
+
+    if(xreq_ptr->gridsize() <= 0.0)
+    {
+        return -1;  //grid size is error
+    }
+
+    double fgridsize = xreq_ptr->gridsize()/2.0;
+
+    std::vector<iv::roi::Point> xvectorpoint;
+    unsigned int nareasize = xvectorarea.size();
+    double fgpsyaw = (90-xreq_ptr->heading())*M_PI/180.0 -3.0*M_PI/2.0;
+    for(i=0;i<nareasize;i++)
+    {
+        int ix,iy;
+        int ny = xvectorarea[i].fsteplen/fgridsize;
+        int nxl = 0;
+        int nxr = 0;
+        if(xvectorarea[i].fleftwidth>0)nxl = xvectorarea[i].fleftwidth/fgridsize;
+        if(xvectorarea[i].frightwidth>0)nxr = xvectorarea[i].frightwidth/fgridsize;
+        double fyaw = xvectorarea[i].fhdg - fgpsyaw;
+ //       double fyaw = xvectorarea[i].fhdg - fgpsyaw;
+        double flaneoff = xvectorarea[i].flaneoff;
+        if(fyaw < 0)fyaw = fyaw + 2.0*M_PI;
+        double frelxraw = xvectorarea[i].refPoint.x - x;
+        double frelyraw = xvectorarea[i].refPoint.y - y;
+        double fxrel,fyrel;
+//        fgpsyaw = 0;
+        fxrel = frelxraw * cos(-fgpsyaw) - frelyraw * sin(-fgpsyaw);
+        fyrel = frelxraw * sin(-fgpsyaw) + frelyraw * cos(-fgpsyaw);
+        double fxbase = fxrel + flaneoff * cos(fyaw + M_PI/2.0);
+        double fybase = fyrel + flaneoff * sin(fyaw + M_PI/2.0);
+//        iv::roi::Point xpoint;
+//        xpoint.x = fxbase;
+//        xpoint.y = fybase;
+//        xvectorpoint.push_back(xpoint);
+//        continue;
+
+        fyaw = fyaw + 3.0*M_PI/2.0;
+        if((nxl == 0)&&(nxr== 0)&&(ny <= 1))
+        {
+            iv::roi::Point xpoint;
+            xpoint.x = fxbase;
+            xpoint.y = fybase;
+            xvectorpoint.push_back(xpoint);
+        }
+        else
+        {
+            if((nxl == 0)&&(nxr== 0))
+            {
+                for(iy=(-ny);iy<=0;iy++)
+                {
+                    iv::roi::Point xpoint;
+                    xpoint.x = fxbase +(iy*fgridsize)*cos(fyaw);
+                    xpoint.y = fybase +(iy*fgridsize)*sin(fyaw);
+                    xvectorpoint.push_back(xpoint);
+                }
+            }
+            else
+            {
+                for(ix=(-nxl);ix<=nxr;ix++)
+                {
+                    for(iy=(-ny);iy<=0;iy++)
+                    {
+                        double fxlocal,fylocal;
+                        fxlocal = ix*fgridsize;
+                        fylocal = iy*fgridsize;
+                        iv::roi::Point xpoint;
+
+                        xpoint.x = fxbase +fxlocal*cos(fyaw) - fylocal * sin(fyaw);
+                        xpoint.y = fybase +fxlocal*sin(fyaw) + fylocal * cos(fyaw);
+                        xvectorpoint.push_back(xpoint);
+                    }
+                }
+            }
+        }
+    }
+
+
+    std::cout<<"xTime: "<<xTime.elapsed()<<" size : "<<xvectorpoint.size()<< std::endl;
+
+    for(i=0;i<xvectorpoint.size();i++)
+    {
+        iv::roi::resultunit * padd = xres_ptr->add_res();
+//        std::cout<<" x: "<<xvectorpoint[i].x<<" y: "<<xvectorpoint[i].y<<std::endl;
+        padd->set_x(xvectorpoint[i].x);
+        padd->set_y(xvectorpoint[i].y);
+    }
+
     return 0;
 }
 
@@ -76,7 +181,7 @@ void service_roi_xodr::updateroadarea()
     mvectorarea.clear();
     unsigned int nroadcount = pxodr->GetRoadCount();
     unsigned int i;
-    const double fstep = 0.1;
+    double fstep = GEOSAMPLESTEP;
     for(i=0;i<nroadcount;i++)
     {
         Road * pRoad = pxodr->GetRoad(i);
@@ -155,6 +260,12 @@ iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
             xarea.refPoint.x = x;
             xarea.refPoint.y = y;
             xarea.s = snow;
+            xarea.fmaxlen = fleftwidth + frigthwidth;
+            xarea.fhdg = yaw;
+            xarea.fleftwidth = fleftwidth;
+            xarea.frightwidth = frigthwidth;
+            xarea.flaneoff = flaneoff;
+            xarea.fsteplen = fstep;
             xroadarea.mvectorarea.push_back(xarea);
 //            double fleftwidth = pRoad->GetRoadLeftWidth(snow);
 //            double frigthwidth = pRoad->GetRoadRightWidth(snow);
@@ -165,8 +276,9 @@ iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
             snow = snow + fstep;
 
         }
-        if(snow < (endS - 0.000001))
+        if((snow-fstep) < (endS - 0.000001))
         {
+            double foldsnow = snow - fstep;
             snow = endS - 0.000001;
             fleftwidth = pRoad->GetRoadLeftWidth(snow);
             frigthwidth = pRoad->GetRoadRightWidth(snow);
@@ -184,9 +296,47 @@ iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
             xarea.refPoint.x = x;
             xarea.refPoint.y = y;
             xarea.s = snow;
+            xarea.fhdg = yaw;
+            xarea.fleftwidth = fleftwidth;
+            xarea.frightwidth = frigthwidth;
+            xarea.flaneoff = flaneoff;
+            xarea.fsteplen = endS - foldsnow;
             xroadarea.mvectorarea.push_back(xarea);
         }
     }
     return xroadarea;
 }
 
+bool service_roi_xodr::CheckPointInArea(double x, double y, iv::roi::area &xarea)
+{
+    double a = (xarea.P2.x - xarea.P1.x)*(y - xarea.P1.y) - (xarea.P2.y-xarea.P1.y)*(x-xarea.P1.x);
+    double b = (xarea.P3.x - xarea.P2.x)*(y - xarea.P2.y) - (xarea.P3.y-xarea.P2.y)*(x-xarea.P2.x);
+    double c = (xarea.P4.x - xarea.P3.x)*(y - xarea.P3.y) - (xarea.P4.y-xarea.P3.y)*(x-xarea.P3.x);
+    double d = (xarea.P1.x - xarea.P4.x)*(y - xarea.P4.y) - (xarea.P1.y-xarea.P4.y)*(x-xarea.P4.x);
+
+    if((a > 0 && b > 0 && c > 0 && d > 0) || (a < 0 && b < 0 && c < 0 && d < 0)) {
+        return true;
+    }
+    return false;
+}
+
+bool service_roi_xodr::CheckPointInROI(double x, double y, std::vector<iv::roi::area> &xvectorroi)
+{
+    unsigned int i;
+    unsigned nsize = xvectorroi.size();
+    for(i=0;i<nsize;i++)
+    {
+        iv::roi::area xarea = xvectorroi[i];
+        double fdis = sqrt(pow(xarea.refPoint.x-x,2)+pow(xarea.refPoint.y-y,2));
+        if(fdis>(xarea.fmaxlen+0.1))
+        {
+            continue;
+        }
+        if(CheckPointInArea(x,y,xarea))
+        {
+            return true;
+        }
+    }
+    return false;
+}
+

src/driver/driver_map_xodrload/service_roi_xodr.h

@@ -26,6 +26,12 @@ struct area
     Point P2;
     Point P3;
     Point P4;
+    double fhdg;
+    double fleftwidth;
+    double frightwidth;
+    double fsteplen;
+    double flaneoff;
+    double fmaxlen;
 
 };
 
@@ -58,17 +64,22 @@ public:
      * @param xres_prt  result
      * @return 0 Succeed -1 No Map  -2 Not in Map
      */
-    int GetROI(std::shared_ptr<iv::roi::request> xreq_ptr,std::shared_ptr<iv::roi::resultarray> & xres_prt );
+    int GetROI(std::shared_ptr<iv::roi::request> xreq_ptr,std::shared_ptr<iv::roi::resultarray> & xres_ptr );
 
 private:
     void updateroadarea();
     iv::roi::roadarea GetRoadArea(Road * pRoad,const double fstep);
 
+    bool CheckPointInROI(double x,double y, std::vector<iv::roi::area> & xvectorroi);
+
+    bool CheckPointInArea(double x, double y,iv::roi::area & xarea);
+
 private:
     OpenDrive * mpxodr = NULL;
     std::vector<iv::roi::roadarea> mvectorarea;
     double mlon0;
     double mlat0;
+    const double GEOSAMPLESTEP = 0.5;  // Get Road Unit at 0.5m
 
 };
 

src/driver/driver_ultrasonic_dauxi_KS136A/canrecv_consumer.cpp

@@ -156,6 +156,10 @@ void CANRecv_Consumer::run()
                     {
                         std::cout<<"sensor: "<<(int)(decodeSensorID + 1)<<" may be disturbed."<<std::endl;
                     }
+                    else
+                    {
+                        std::cout<<"sensor: "<<(int)(decodeSensorID + 1)<<" dist data: "<<(int)tempDist<<std::endl;
+                    }
 #endif
                 }
             }

src/include/proto/ultraarea.proto

@@ -0,0 +1,30 @@
+/*
+		      1  
+ 	        o/--o---o--\o
+		|           |
+ 	       o|	    |o 
+ 		|	    |
+		|	    |
+	     4  |	    |  2
+		|	    |
+               o|	    |o 
+		|	    |
+		o\--o---o--/o
+		      3 	
+ */
+
+syntax = "proto2";
+
+package iv.ultrasonic;
+
+message Area {
+    optional int32 id = 1;   // id of area.
+    optional float dist = 2 [default = 50.0]; // min dist in the area. (m)
+    optional bool valid = 3 [default = true]; // true -> working   |   false -> all ultras in the area don't work.
+};
+
+message ultraarea
+{
+    repeated Area area = 1;	
+    required int64 timestamp = 2;	
+}