Skip to content

J
jfx_kalman_filter_src
Commit 2c1d0c39 authored by oscar's avatar oscar
Browse files
Options

提交修改

parent 27ae4f1a
Hide whitespace changes
Inline Side-by-side
Showing with 41 additions and 38 deletions
BaseTracker/BaseTracker.h
View file @ 2c1d0c39
......@@ -90,49 +90,52 @@ int BaseTracker<T>::Run(const std::vector<std::vector<float> >& detections, std:
else
{
int bs = matched.size();
int ns = 0;
int no = detections.size() > 0 ? detections[0].size():0;
std::shared_ptr<float> Z = std::shared_ptr<float>(new float[bs * no], [](float* p) {if (p) delete[] p; p = nullptr; });
std::shared_ptr<float*> X = std::shared_ptr<float*>(new float* [bs], [](float** p) {if (p) delete[] p; p = nullptr; });
std::shared_ptr<float*> P = std::shared_ptr<float*>(new float* [bs], [](float** p) {if (p) delete[] p; p = nullptr; });
std::shared_ptr<float> R = std::shared_ptr<float>(new float[no * no], [](float* p) {if (p) delete[] p; p = nullptr; });
std::shared_ptr<float> HX = std::shared_ptr<float>(new float[bs * no], [](float* p) {if (p) delete[] p; p = nullptr; });
int bs_i = 0;
for (const auto& match : matched)
if (bs > 0)
{
const auto& id = match.first;
std::vector<float> cre_det;
m_tracker[id]->UpdateDataCheck(detections[match.second], cre_det);
float* ptr_Z = Z.get() + bs_i * no;
for (int i = 0; i < no; i++)
{
ptr_Z[i] = cre_det[i];
}
X.get()[bs_i] = m_tracker[id]->GetStatesXPtr();
P.get()[bs_i] = m_tracker[id]->GetPredictPtr();
float* ptr_HX = HX.get() + bs_i * no;
memcpy(ptr_HX, m_tracker[id]->GetStatesXPtr(), no*sizeof(float));
if (ns == 0)
int ns = 0;
int no = detections.size() > 0 ? detections[0].size() : 0;
std::shared_ptr<float> Z = std::shared_ptr<float>(new float[bs * no], [](float* p) {if (p) delete[] p; p = nullptr; });
std::shared_ptr<float*> X = std::shared_ptr<float*>(new float* [bs], [](float** p) {if (p) delete[] p; p = nullptr; });
std::shared_ptr<float*> P = std::shared_ptr<float*>(new float* [bs], [](float** p) {if (p) delete[] p; p = nullptr; });
std::shared_ptr<float> R = std::shared_ptr<float>(new float[no * no], [](float* p) {if (p) delete[] p; p = nullptr; });
std::shared_ptr<float> HX = std::shared_ptr<float>(new float[bs * no], [](float* p) {if (p) delete[] p; p = nullptr; });
int bs_i = 0;
for (const auto& match : matched)
{
ns = m_tracker[id]->GetStatesNum();
float* _r = m_tracker[id]->GetRPtr();
memcpy(R.get(), _r, no*no*sizeof(float));
const auto& id = match.first;
std::vector<float> cre_det;
m_tracker[id]->UpdateDataCheck(detections[match.second], cre_det);
float* ptr_Z = Z.get() + bs_i * no;
for (int i = 0; i < no; i++)
{
ptr_Z[i] = cre_det[i];
}
X.get()[bs_i] = m_tracker[id]->GetStatesXPtr();
P.get()[bs_i] = m_tracker[id]->GetPredictPtr();
float* ptr_HX = HX.get() + bs_i * no;
memcpy(ptr_HX, m_tracker[id]->GetStatesXPtr(), no * sizeof(float));
if (ns == 0)
{
ns = m_tracker[id]->GetStatesNum();
float* _r = m_tracker[id]->GetRPtr();
memcpy(R.get(), _r, no * no * sizeof(float));
}
bs_i++;
m_tracker[id]->UpdateHit();//gpu时需要update里面的变量
detectionsId[match.second] = id;
updateId[id] = match.second;
}
bs_i++;
m_tracker[id]->UpdateHit();//gpu时需要update里面的变量
detectionsId[match.second] = id;
updateId[id] = match.second;
}
//SDK_LOG(SDK_INFO, "Z = [%s]", GetMatrixStr(Z.get(), no, bs).c_str());
//SDK_LOG(SDK_INFO, "X = [%s]", GetMatrixStr(X.get(), ns, bs).c_str());
//SDK_LOG(SDK_INFO, "P = [%s]", GetMatrixStr(P.get(), ns * ns, bs).c_str());
SDK_LOG(SDK_INFO, "R = [%s]", GetMatrixStr(R.get(), no, no).c_str());
//SDK_LOG(SDK_INFO, "HX = [%s]", GetMatrixStr(HX.get(),no,bs).c_str());
//SDK_LOG(SDK_INFO, "Z = [%s]", GetMatrixStr(Z.get(), no, bs).c_str());
//SDK_LOG(SDK_INFO, "X = [%s]", GetMatrixStr(X.get(), ns, bs).c_str());
//SDK_LOG(SDK_INFO, "P = [%s]", GetMatrixStr(P.get(), ns * ns, bs).c_str());
SDK_LOG(SDK_INFO, "R = [%s]", GetMatrixStr(R.get(), no, no).c_str());
//SDK_LOG(SDK_INFO, "HX = [%s]", GetMatrixStr(HX.get(),no,bs).c_str());
#ifdef _KF_IOU_CUDA_
kalman_update_batch(Z.get(), X.get(), P.get(), R.get(), HX.get(), bs, ns, no);
kalman_update_batch(Z.get(), X.get(), P.get(), R.get(), HX.get(), bs, ns, no);
#endif
//SDK_LOG(SDK_INFO, "after X = [%s]", GetMatrixStr(X.get(), ns, bs).c_str());
//SDK_LOG(SDK_INFO, "after P = [%s]", GetMatrixStr(P.get(), ns * ns, bs).c_str());
//SDK_LOG(SDK_INFO, "after X = [%s]", GetMatrixStr(X.get(), ns, bs).c_str());
//SDK_LOG(SDK_INFO, "after P = [%s]", GetMatrixStr(P.get(), ns * ns, bs).c_str());
}
}
/*** Create new tracks for unmatched detections ***/
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment