提交测试

BaseTracker/kf_gup/kalman_update_batch_online.cu

@@ -294,10 +294,70 @@ void map_kalman_update_batch( 	pybind11::array_t<float> Z,
 
 //PYBIND11_MODULE(juefx_kalman_multi_shared, m)
 //PYBIND11_MODULE(juefx_kalman_multi_1, m)
-PYBIND11_MODULE(juefx_kalman_batch, m)
-{
-  m.def("kalman_update_batch", &map_kalman_update_batch);
-}
+//PYBIND11_MODULE(juefx_kalman_batch, m)
+//{
+//  m.def("kalman_update_batch", &map_kalman_update_batch);
+//}
+
+/*
+	ns = dim_x = 10                          len of state
+	no = dim_z = 7                          len of observation
+
+	Z: measurement                                                              (bs, 7)
+	X: estimate                                                                 (bs, 10)
+	P: uncertainty covariance                                                   (bs, 100)
 
+  MAKE SURE ALL INPUTS ARE TWO-DIM NUMPY ARRAY
+  */
+void kalman_update_batch(pybind11::array_t<float> Z,
+	pybind11::array_t<float> X,     // in-place update
+	pybind11::array_t<float> P,     // in-place update
+	pybind11::array_t<float> HX,
+	const int bs,
+	const int ns,
+	const int no
+) {
+
+	pybind11::buffer_info ZZ = Z.request();
+	pybind11::buffer_info XX = X.request();
+	pybind11::buffer_info PP = P.request();
+	pybind11::buffer_info HXX = HX.request();
+
+	int size_ZZ = ZZ.shape[0] * ZZ.shape[1] * sizeof(float);
+	int size_XX = XX.shape[0] * XX.shape[1] * sizeof(float);
+	int size_PP = PP.shape[0] * PP.shape[1] * sizeof(float);
+	int size_HXX = HXX.shape[0] * HXX.shape[1] * sizeof(float);
+	// std::cout << "size_HXX: " << size_HXX <<"\n";
+
+	float* host_Z = reinterpret_cast<float*>(ZZ.ptr);
+	float* host_X = reinterpret_cast<float*>(XX.ptr);
+	float* host_P = reinterpret_cast<float*>(PP.ptr);
+	float* host_HX = reinterpret_cast<float*>(HXX.ptr);
 
+	float* device_Z;
+	float* device_X;
+	float* device_P;
+	float* device_HX;
+
+	GPU_CHECK(cudaMalloc(&device_Z, size_ZZ));
+	GPU_CHECK(cudaMalloc(&device_X, size_XX));
+	GPU_CHECK(cudaMalloc(&device_P, size_PP));
+	GPU_CHECK(cudaMalloc(&device_HX, size_HXX));
+
+	GPU_CHECK(cudaMemcpy(device_Z, host_Z, size_ZZ, cudaMemcpyHostToDevice));
+	GPU_CHECK(cudaMemcpy(device_X, host_X, size_XX, cudaMemcpyHostToDevice));
+	GPU_CHECK(cudaMemcpy(device_P, host_P, size_PP, cudaMemcpyHostToDevice));
+	GPU_CHECK(cudaMemcpy(device_HX, host_HX, size_HXX, cudaMemcpyHostToDevice));
+
+	kalmanUpdateLauncher_batch(device_Z, device_X, device_P, device_HX, bs, ns, no);
+
+	GPU_CHECK(cudaMemcpy(host_X, device_X, size_XX, cudaMemcpyDeviceToHost));
+	GPU_CHECK(cudaMemcpy(host_P, device_P, size_PP, cudaMemcpyDeviceToHost));
+
+	GPU_CHECK(cudaFree(device_Z));
+	GPU_CHECK(cudaFree(device_X));
+	GPU_CHECK(cudaFree(device_P));
+	GPU_CHECK(cudaFree(device_HX));
+
+}
 

TrackingRos.cpp

@@ -49,6 +49,17 @@ void TrackingRos::Init(ros::NodeHandle& nh)
 
     m_isTrackingRun = true;
     m_trackingThread = std::thread(&TrackingRos::ThreadTrackingProcess, this);
+
+    Eigen::MatrixXf mat(2,2);
+    mat(0, 0) = 0.1f;
+    mat(0, 1) = 0.2f;
+    mat(1, 0) = 0.3f;
+    mat(1, 1) = 0.4f;
+
+    float* f = mat.data();
+
+    SDK_LOG(SDK_INFO, "data = %f,%f,%f,%f",f[0],f[1],f[2],f[3]);
+
 }
 
 void TrackingRos::TrackingCallBackFunc(const jfx_common_msgs::det_tracking_arrayConstPtr& msg)//接受ros发过来的消息