|
|
@@ -7,30 +7,40 @@ Error_manager DeviceTof3D::Init(const VzEtcMap &tp_tof3d, bool search) {
|
|
|
return {TOF3D_VZENSE_DEVICE_INIT_FAILED, MAJOR_ERROR, "VzInitialize failed status: %d", m_vz_status};
|
|
|
}
|
|
|
|
|
|
+ detector = new TensorrtWheelDetector(ETC_PATH PROJECT_NAME "/model.onnx", ETC_PATH PROJECT_NAME "/class.txt");
|
|
|
+
|
|
|
loadEtc(tp_tof3d);
|
|
|
|
|
|
if (search) {
|
|
|
SearchDevice(5);
|
|
|
}
|
|
|
|
|
|
+ cv::namedWindow("ret", cv::WINDOW_AUTOSIZE);
|
|
|
+
|
|
|
ConnectAllEtcDevices();
|
|
|
|
|
|
VZ_SetHotPlugStatusCallback(DeviceTof3D::HotPlugStateCallback, &mp_device_info);
|
|
|
|
|
|
//启动 线程。
|
|
|
for (auto &info: mp_device_info) {
|
|
|
- setTof3dParams(info.second.etc.ip());
|
|
|
- getTof3dParams(info.second.etc.ip());
|
|
|
+ if (!info.second->etc.enable_device()) {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ setTof3dParams(info.second->etc.ip());
|
|
|
+ getTof3dParams(info.second->etc.ip());
|
|
|
mp_thread_info.insert(std::pair<std::string, ThreadInfo>(
|
|
|
info.first, ThreadInfo(new std::thread(&DeviceTof3D::receiveFrameThread, this, info.first),
|
|
|
new Thread_condition()
|
|
|
- )
|
|
|
- ));
|
|
|
+ )
|
|
|
+ ));
|
|
|
}
|
|
|
|
|
|
for (auto &t: mp_thread_info) {
|
|
|
t.second.condit->notify_all(true);
|
|
|
}
|
|
|
+ detect_thread.t = new std::thread(&DeviceTof3D::detectThread, this);
|
|
|
+ detect_thread.condit = new Thread_condition();
|
|
|
+ detect_thread.condit->notify_all(true);
|
|
|
|
|
|
return {TOF3D_VZENSE_DEVICE_INIT_SUCCESS, NORMAL, "VzInitialize success."};
|
|
|
}
|
|
|
@@ -63,20 +73,17 @@ Error_manager DeviceTof3D::SearchDevice(const double &time) {
|
|
|
for (int i = 0; i < deviceCount; i++) {
|
|
|
auto iter = mp_device_info.find(pDeviceListInfo[i].ip);
|
|
|
if (iter == mp_device_info.end()) {
|
|
|
- mp_device_info.insert(std::pair<std::string, tof3dVzenseInfo>(pDeviceListInfo[i].ip, tof3dVzenseInfo()));
|
|
|
+ mp_device_info.insert(
|
|
|
+ std::pair<std::string, tof3dVzenseInfo *>(pDeviceListInfo[i].ip, new tof3dVzenseInfo()));
|
|
|
iter = mp_device_info.find(pDeviceListInfo[i].ip);
|
|
|
}
|
|
|
- iter->second.info = pDeviceListInfo[i];
|
|
|
+ iter->second->info = pDeviceListInfo[i];
|
|
|
LOG(INFO) << "Found device: " << pDeviceListInfo[i].ip;
|
|
|
}
|
|
|
}
|
|
|
return {};
|
|
|
}
|
|
|
|
|
|
-void DeviceTof3D::run() {
|
|
|
-
|
|
|
-}
|
|
|
-
|
|
|
Error_manager DeviceTof3D::ConnectDevice(const std::string &ip, bool open_stream) {
|
|
|
auto iter = mp_device_info.find(ip);
|
|
|
if (iter == mp_device_info.end()) {
|
|
|
@@ -84,24 +91,24 @@ Error_manager DeviceTof3D::ConnectDevice(const std::string &ip, bool open_stream
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
|
|
|
- m_vz_status = VZ_OpenDeviceByIP(iter->second.info.ip, &iter->second.handle);
|
|
|
+ m_vz_status = VZ_OpenDeviceByIP(iter->second->info.ip, &iter->second->handle);
|
|
|
if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
- iter->second.is_connect = false;
|
|
|
+ iter->second->is_connect = false;
|
|
|
LOG(WARNING) << "OpenDevice " << ip << " failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
} else {
|
|
|
- iter->second.is_connect = true;
|
|
|
+ iter->second->is_connect = true;
|
|
|
LOG(INFO) << "OpenDevice " << ip << " success status:" << m_vz_status;
|
|
|
}
|
|
|
|
|
|
if (open_stream) {
|
|
|
- m_vz_status = VZ_StartStream(iter->second.handle);
|
|
|
+ m_vz_status = VZ_StartStream(iter->second->handle);
|
|
|
if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
- iter->second.is_start_stream = false;
|
|
|
+ iter->second->is_start_stream = false;
|
|
|
LOG(WARNING) << "VZ_StartStream " << ip << " failed status:" << m_vz_status;
|
|
|
return {};
|
|
|
} else {
|
|
|
- iter->second.is_start_stream = true;
|
|
|
+ iter->second->is_start_stream = true;
|
|
|
LOG(INFO) << "VZ_StartStream " << ip << " success status:" << m_vz_status;
|
|
|
}
|
|
|
}
|
|
|
@@ -111,7 +118,7 @@ Error_manager DeviceTof3D::ConnectDevice(const std::string &ip, bool open_stream
|
|
|
|
|
|
Error_manager DeviceTof3D::ConnectAllEtcDevices(bool open_stream) {
|
|
|
for (auto &device: mp_device_info) {
|
|
|
- if (device.second.etc.enable_device()) {
|
|
|
+ if (device.second->etc.enable_device()) {
|
|
|
ConnectDevice(device.first, open_stream);
|
|
|
}
|
|
|
}
|
|
|
@@ -131,21 +138,21 @@ Error_manager DeviceTof3D::DisConnectDevice(const std::string &ip) {
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
|
|
|
- m_vz_status = VZ_StopStream(iter->second.handle);
|
|
|
+ m_vz_status = VZ_StopStream(iter->second->handle);
|
|
|
if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
- iter->second.is_start_stream = true;
|
|
|
+ iter->second->is_start_stream = true;
|
|
|
LOG(WARNING) << "VZ_StopStream failed status:" << m_vz_status;
|
|
|
} else {
|
|
|
- iter->second.is_start_stream = false;
|
|
|
+ iter->second->is_start_stream = false;
|
|
|
LOG(INFO) << "VZ_StopStream success status:" << m_vz_status;
|
|
|
}
|
|
|
|
|
|
- m_vz_status = VZ_CloseDevice(&iter->second.handle);
|
|
|
+ m_vz_status = VZ_CloseDevice(&iter->second->handle);
|
|
|
if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
- iter->second.is_connect = true;
|
|
|
+ iter->second->is_connect = true;
|
|
|
LOG(WARNING) << "VZ_CloseDevice failed status:" << m_vz_status;
|
|
|
} else {
|
|
|
- iter->second.is_connect = false;
|
|
|
+ iter->second->is_connect = false;
|
|
|
LOG(INFO) << "VZ_CloseDevice success status:" << m_vz_status;
|
|
|
}
|
|
|
|
|
|
@@ -154,7 +161,7 @@ Error_manager DeviceTof3D::DisConnectDevice(const std::string &ip) {
|
|
|
|
|
|
Error_manager DeviceTof3D::DisConnectAllEtcDevices() {
|
|
|
for (auto &device: mp_device_info) {
|
|
|
- if (device.second.etc.enable_device()) {
|
|
|
+ if (device.second->etc.enable_device()) {
|
|
|
DisConnectDevice(device.first);
|
|
|
}
|
|
|
}
|
|
|
@@ -174,22 +181,22 @@ Error_manager DeviceTof3D::getDepthFrame(const std::string &ip, VzFrame &depthFr
|
|
|
return {FAILED, NORMAL, "Device %s not in list, can\'t get depth frame.", ip.c_str()}; //TODO
|
|
|
}
|
|
|
|
|
|
- if (iter->second.handle == nullptr || !iter->second.is_connect) {
|
|
|
- if(ConnectDevice(ip, true) != SUCCESS) {
|
|
|
+ if (iter->second->handle == nullptr || !iter->second->is_connect) {
|
|
|
+ if (ConnectDevice(ip, true) != SUCCESS) {
|
|
|
return {FAILED, NORMAL, "Open device %s failed, stop get depth frame.", ip.c_str()};
|
|
|
}
|
|
|
}
|
|
|
|
|
|
VzFrameReady frameReady = {0};
|
|
|
- m_vz_status = VZ_GetFrameReady(iter->second.handle, 100, &frameReady);
|
|
|
+ m_vz_status = VZ_GetFrameReady(iter->second->handle, iter->second->etc.bip().exposure_time(), &frameReady);
|
|
|
|
|
|
if (m_vz_status != VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_GetFrameReady failed status:" << m_vz_status;
|
|
|
- return {}; //TODO
|
|
|
+ return {FAILED, MINOR_ERROR, "%s VZ_GetFrameReady failed status %d!", ip.c_str(), m_vz_status}; //TODO
|
|
|
}
|
|
|
|
|
|
if (1 == frameReady.depth) {
|
|
|
- m_vz_status = VZ_GetFrame(iter->second.handle, VzDepthFrame, &depthFrame);
|
|
|
+ m_vz_status = VZ_GetFrame(iter->second->handle, VzDepthFrame, &depthFrame);
|
|
|
|
|
|
if (m_vz_status == VzRetOK && depthFrame.pFrameData != nullptr) {
|
|
|
// LOG(INFO) << ip << " VZ_GetFrame VzDepthFrame success:" << m_vz_status;
|
|
|
@@ -207,14 +214,14 @@ Error_manager DeviceTof3D::getIrFrame(const std::string &ip, VzFrame &irFrame) {
|
|
|
return {FAILED, NORMAL, "Device %s not in list, can\'t get ir frame.", ip.c_str()}; //TODO
|
|
|
}
|
|
|
|
|
|
- if (iter->second.handle == nullptr || !iter->second.is_connect) {
|
|
|
- if(ConnectDevice(ip, true) != SUCCESS) {
|
|
|
+ if (iter->second->handle == nullptr || !iter->second->is_connect) {
|
|
|
+ if (ConnectDevice(ip, true) != SUCCESS) {
|
|
|
return {FAILED, NORMAL, "Open device %s failed, stop get ir frame.", ip.c_str()};
|
|
|
}
|
|
|
}
|
|
|
|
|
|
VzFrameReady frameReady = {0};
|
|
|
- m_vz_status = VZ_GetFrameReady(iter->second.handle, 100, &frameReady);
|
|
|
+ m_vz_status = VZ_GetFrameReady(iter->second->handle, 100, &frameReady);
|
|
|
|
|
|
if (m_vz_status != VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_GetFrameReady failed status:" << m_vz_status;
|
|
|
@@ -222,7 +229,7 @@ Error_manager DeviceTof3D::getIrFrame(const std::string &ip, VzFrame &irFrame) {
|
|
|
}
|
|
|
|
|
|
if (1 == frameReady.ir) {
|
|
|
- m_vz_status = VZ_GetFrame(iter->second.handle, VzIRFrame, &irFrame);
|
|
|
+ m_vz_status = VZ_GetFrame(iter->second->handle, VzIRFrame, &irFrame);
|
|
|
|
|
|
if (m_vz_status == VzRetOK && irFrame.pFrameData != nullptr) {
|
|
|
LOG(INFO) << ip << "VZ_GetFrame VzIrFrame success:" << m_vz_status;
|
|
|
@@ -240,25 +247,26 @@ Error_manager DeviceTof3D::getDepthAndIrPicture(const std::string &ip, VzFrame &
|
|
|
return {FAILED, NORMAL, "Device %s not in list, can\'t get depth and ir frame.", ip.c_str()}; //TODO
|
|
|
}
|
|
|
|
|
|
- if (iter->second.handle == nullptr || !iter->second.is_connect) {
|
|
|
- if(ConnectDevice(ip, true) != SUCCESS) {
|
|
|
+ if (iter->second->handle == nullptr || !iter->second->is_connect) {
|
|
|
+ if (ConnectDevice(ip, true) != SUCCESS) {
|
|
|
return {FAILED, NORMAL, "Open device %s failed, stop get depth and ir frame.", ip.c_str()};
|
|
|
}
|
|
|
}
|
|
|
|
|
|
Error_manager ret;
|
|
|
VzFrameReady frameReady = {0};
|
|
|
- m_vz_status = VZ_GetFrameReady(iter->second.handle, 100, &frameReady);
|
|
|
+ m_vz_status = VZ_GetFrameReady(iter->second->handle, 100, &frameReady);
|
|
|
|
|
|
if (m_vz_status != VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_GetFrameReady failed status:" << m_vz_status;
|
|
|
- ret.compare_and_merge_up({FAILED, MINOR_ERROR, "Device %s VZ_GetFrameReady failed status: %d.", ip.c_str(), m_vz_status}); //TODO
|
|
|
+ ret.compare_and_merge_up({FAILED, MINOR_ERROR, "Device %s VZ_GetFrameReady failed status: %d.", ip.c_str(),
|
|
|
+ m_vz_status}); //TODO
|
|
|
return ret;
|
|
|
}
|
|
|
|
|
|
//Get depth frame, depth frame only output in following data mode
|
|
|
if (1 == frameReady.depth) {
|
|
|
- m_vz_status = VZ_GetFrame(iter->second.handle, VzDepthFrame, &depthFrame);
|
|
|
+ m_vz_status = VZ_GetFrame(iter->second->handle, VzDepthFrame, &depthFrame);
|
|
|
|
|
|
if (m_vz_status == VzRetOK && depthFrame.pFrameData != nullptr) {
|
|
|
LOG(INFO) << ip << " frameIndex :" << depthFrame.frameIndex;
|
|
|
@@ -269,19 +277,23 @@ Error_manager DeviceTof3D::getDepthAndIrPicture(const std::string &ip, VzFrame &
|
|
|
LOG(INFO) << ip << "VZ_GetFrame VzIrFrame status:" << m_vz_status;
|
|
|
}
|
|
|
} else {
|
|
|
- ret.compare_and_merge_up({FAILED, MINOR_ERROR, "Device %s VZ_GetFrameReady depth not ready: %d.", ip.c_str(), frameReady.depth});
|
|
|
+ ret.compare_and_merge_up(
|
|
|
+ {FAILED, MINOR_ERROR, "Device %s VZ_GetFrameReady depth not ready: %d.", ip.c_str(), frameReady.depth});
|
|
|
}
|
|
|
|
|
|
if (1 == frameReady.ir) {
|
|
|
- m_vz_status = VZ_GetFrame(iter->second.handle, VzIRFrame, &irFrame);
|
|
|
+ m_vz_status = VZ_GetFrame(iter->second->handle, VzIRFrame, &irFrame);
|
|
|
if (m_vz_status == VzRetOK && irFrame.pFrameData != nullptr) {
|
|
|
LOG(INFO) << ip << " frameIndex :" << irFrame.frameIndex;
|
|
|
} else {
|
|
|
- ret.compare_and_merge_up({FAILED, MINOR_ERROR, "Device %s VZ_GetFrame VzIrFrame failed status: %d.", ip.c_str(), m_vz_status}); //TODO
|
|
|
+ ret.compare_and_merge_up(
|
|
|
+ {FAILED, MINOR_ERROR, "Device %s VZ_GetFrame VzIrFrame failed status: %d.", ip.c_str(),
|
|
|
+ m_vz_status}); //TODO
|
|
|
LOG(INFO) << ip << "VZ_GetFrame VzIrFrame status:" << m_vz_status;
|
|
|
}
|
|
|
} else {
|
|
|
- ret.compare_and_merge_up({FAILED, NORMAL, "Device %s VZ_GetFrameReady ir not ready: %d.", ip.c_str(), frameReady.depth});
|
|
|
+ ret.compare_and_merge_up(
|
|
|
+ {FAILED, NORMAL, "Device %s VZ_GetFrameReady ir not ready: %d.", ip.c_str(), frameReady.depth});
|
|
|
}
|
|
|
|
|
|
return ret;
|
|
|
@@ -293,14 +305,14 @@ Error_manager DeviceTof3D::getDepthPointCloud(const std::string &ip, pcl::PointC
|
|
|
return {FAILED, NORMAL, "Device %s not in list, can\'t get point cloud.", ip.c_str()}; //TODO
|
|
|
}
|
|
|
|
|
|
- if (iter->second.handle == nullptr || !iter->second.is_connect) {
|
|
|
- if(ConnectDevice(ip, true) != SUCCESS) {
|
|
|
+ if (iter->second->handle == nullptr || !iter->second->is_connect) {
|
|
|
+ if (ConnectDevice(ip, true) != SUCCESS) {
|
|
|
return {FAILED, NORMAL, "Open device %s failed, stop get point cloud.", ip.c_str()};
|
|
|
}
|
|
|
}
|
|
|
|
|
|
VzFrameReady frameReady = {0};
|
|
|
- m_vz_status = VZ_GetFrameReady(iter->second.handle, 100, &frameReady);
|
|
|
+ m_vz_status = VZ_GetFrameReady(iter->second->handle, 100, &frameReady);
|
|
|
|
|
|
if (m_vz_status != VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_GetFrameReady failed status:" << m_vz_status;
|
|
|
@@ -310,7 +322,7 @@ Error_manager DeviceTof3D::getDepthPointCloud(const std::string &ip, pcl::PointC
|
|
|
//Get depth frame, depth frame only output in following data mode
|
|
|
if (1 == frameReady.depth) {
|
|
|
VzFrame depthFrame = {0};
|
|
|
- m_vz_status = VZ_GetFrame(iter->second.handle, VzDepthFrame, &depthFrame);
|
|
|
+ m_vz_status = VZ_GetFrame(iter->second->handle, VzDepthFrame, &depthFrame);
|
|
|
|
|
|
if (m_vz_status == VzRetOK && depthFrame.pFrameData != nullptr) {
|
|
|
LOG(INFO) << ip << " frameIndex :" << depthFrame.frameIndex;
|
|
|
@@ -319,8 +331,8 @@ Error_manager DeviceTof3D::getDepthPointCloud(const std::string &ip, pcl::PointC
|
|
|
const int len = srcFrame.width * srcFrame.height;
|
|
|
VzVector3f *worldV = new VzVector3f[len];
|
|
|
|
|
|
- m_vz_status = VZ_ConvertDepthFrameToPointCloudVector(iter->second.handle, &srcFrame,
|
|
|
- worldV);
|
|
|
+ m_vz_status = VZ_ConvertDepthFrameToPointCloudVector(iter->second->handle, &srcFrame,
|
|
|
+ worldV);
|
|
|
|
|
|
if (m_vz_status == VzRetOK) {
|
|
|
cloud->clear();
|
|
|
@@ -342,7 +354,8 @@ Error_manager DeviceTof3D::getDepthPointCloud(const std::string &ip, pcl::PointC
|
|
|
return {};
|
|
|
}
|
|
|
|
|
|
-Error_manager DeviceTof3D::DepthFrame2PointCloud(const std::string &ip, VzFrame &depthFrame, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
|
|
|
+Error_manager DeviceTof3D::DepthFrame2PointCloud(const std::string &ip, VzFrame &depthFrame,
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
|
|
|
// if (depthFrame.frameType != VzDepthFrame) {
|
|
|
// return {};
|
|
|
// }
|
|
|
@@ -428,30 +441,63 @@ Error_manager DeviceTof3D::IrFrame2Mat(VzFrame &frame, cv::Mat &mat) {
|
|
|
}
|
|
|
|
|
|
#include <sys/stat.h>
|
|
|
+
|
|
|
void DeviceTof3D::receiveFrameThread(const std::string &ip) {
|
|
|
LOG(INFO) << ip << " in thread " << std::this_thread::get_id();
|
|
|
auto iter = mp_device_info.find(ip);
|
|
|
auto t_iter = mp_thread_info.find(ip);
|
|
|
- TensorrtWheelDetector detector(ETC_PATH PROJECT_NAME "/best.engine");
|
|
|
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr t_car_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr t_wheel_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ std::vector<TensorrtWheelDetector::Object> segment_results;
|
|
|
while (t_iter->second.condit->is_alive()) {
|
|
|
t_iter->second.condit->wait();
|
|
|
- if (t_iter->second.condit->is_alive() ) {
|
|
|
- Error_manager ret;
|
|
|
- VzFrame depthFrame;
|
|
|
- if (iter->second.handle) {
|
|
|
+ if (t_iter->second.condit->is_alive()) {
|
|
|
+ Error_manager ret;
|
|
|
+ if (iter->second->handle) {
|
|
|
+ t_car_cloud->clear();
|
|
|
+ t_wheel_cloud->clear();
|
|
|
VzFrame depthFrame = {0};
|
|
|
cv::Mat depthMat(480, 640, CV_16UC1);
|
|
|
- if(getDepthFrame(ip, depthFrame) == SUCCESS && DepthFrame2Mat(depthFrame, depthMat) == SUCCESS) {
|
|
|
+ if (getDepthFrame(ip, depthFrame) == SUCCESS) {
|
|
|
+ segment_results.clear();
|
|
|
+ if (Frame2Mat(depthFrame, depthMat) == SUCCESS) {
|
|
|
+ detector->detect(depthMat, segment_results);
|
|
|
+ cv::imshow("ret", depthMat);
|
|
|
+ cv::waitKey(100);
|
|
|
+ }
|
|
|
+
|
|
|
+ std::vector<cv::Point> mat_seg_ret;
|
|
|
+ for (auto &result: segment_results) {
|
|
|
+ if (result.prob > 0.5) {
|
|
|
+ mat_seg_ret = detector->getPointsFromObj(result);
|
|
|
+ for (auto &pt: mat_seg_ret) {
|
|
|
+ depthMat.at<uchar>(pt) = 255 - depthMat.at<uchar>(pt);
|
|
|
+ }
|
|
|
+ cv::imshow("ret", depthMat);
|
|
|
+ cv::waitKey(1);
|
|
|
+ }
|
|
|
+ }
|
|
|
|
|
|
+ if (!mat_seg_ret.empty()) {
|
|
|
+ segFrame2CarAndWheel(depthFrame, mat_seg_ret, t_car_cloud, t_wheel_cloud);
|
|
|
+ // viewer.removeAllPointClouds(view_port[0]);
|
|
|
+ // viewer.removeAllPointClouds(view_port[1]);
|
|
|
+ // viewer.addPointCloud(t_car_cloud, "car_cloud_", view_port[0]);
|
|
|
+ // viewer.addPointCloud(t_wheel_cloud, "wheel_cloud_", view_port[1]);
|
|
|
+ // viewer.spinOnce();
|
|
|
+ }
|
|
|
} else {
|
|
|
ret.compare_and_merge_up({FAILED, MINOR_ERROR, "Device %s VZ_GetFrame failed.", ip.c_str()});
|
|
|
}
|
|
|
- } else {
|
|
|
- ret.compare_and_merge_up({FAILED, MINOR_ERROR, "Device %s handle is null.", ip.c_str()});
|
|
|
- }
|
|
|
- LOG(INFO) << ret.get_error_description();
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds (1));
|
|
|
+ } else {
|
|
|
+ ret.compare_and_merge_up({FAILED, MINOR_ERROR, "Device %s handle is null.", ip.c_str()});
|
|
|
+ }
|
|
|
+
|
|
|
+ if (!ret.get_error_description().empty()) {
|
|
|
+ LOG(INFO) << ret.get_error_description();
|
|
|
+ }
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
|
}
|
|
|
}
|
|
|
|
|
|
@@ -459,8 +505,50 @@ void DeviceTof3D::receiveFrameThread(const std::string &ip) {
|
|
|
LOG(INFO) << ip << " thread end " << std::this_thread::get_id();
|
|
|
}
|
|
|
|
|
|
-void DeviceTof3D::HotPlugStateCallback(const VzDeviceInfo *pInfo, int status, void *contex)
|
|
|
-{
|
|
|
+#include <pcl/visualization/pcl_visualizer.h>
|
|
|
+
|
|
|
+void DeviceTof3D::detectThread() {
|
|
|
+ LOG(INFO) << "detect thread running in " << std::this_thread::get_id();
|
|
|
+
|
|
|
+ int view_port[2];
|
|
|
+ pcl::visualization::PCLVisualizer viewer("viewer_0");
|
|
|
+ viewer.createViewPort(0.0, 0.0, 0.5, 1, view_port[0]);
|
|
|
+ viewer.createViewPort(0.5, 0.0, 1, 1, view_port[1]);
|
|
|
+ viewer.addText("car_cloud", 10, 10, 20, 0, 1, 0, "car_cloud", view_port[0]);
|
|
|
+ viewer.addText("wheel_cloud", 10, 10, 20, 0, 1, 0, "wheel_cloud", view_port[1]);
|
|
|
+ viewer.addCoordinateSystem(1, "car_axis", view_port[0]);
|
|
|
+ viewer.addCoordinateSystem(1, "wheel_axis", view_port[1]);
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr t_car_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr t_wheel_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ auto t_start_time = std::chrono::steady_clock::now();
|
|
|
+ std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;
|
|
|
+
|
|
|
+ while (detect_thread.condit->is_alive()) {
|
|
|
+// detect_thread.condit->wait();
|
|
|
+ t_car_cloud->clear();
|
|
|
+ t_wheel_cloud->clear();
|
|
|
+ cost = std::chrono::steady_clock::now() - t_start_time;
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds((int) MAX(100 - cost.count() * 1000, 1)));
|
|
|
+ t_start_time = std::chrono::steady_clock::now();
|
|
|
+// LOG(INFO) << "last wheel cost time is " << cost.count() * 1000 << " ms";
|
|
|
+
|
|
|
+ if (detect_thread.condit->is_alive()) {
|
|
|
+
|
|
|
+
|
|
|
+// cost = std::chrono::steady_clock::now() - t_start_time;
|
|
|
+// LOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
|
|
|
+ if (!t_car_cloud->empty() && !t_wheel_cloud->empty()) {
|
|
|
+ CarPoseOptimize(t_car_cloud);
|
|
|
+ WheelCloudOptimize(t_wheel_cloud);
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ LOG(INFO) << "detect thread " << std::this_thread::get_id() << " closed.";
|
|
|
+}
|
|
|
+
|
|
|
+void DeviceTof3D::HotPlugStateCallback(const VzDeviceInfo *pInfo, int status, void *contex) {
|
|
|
LOG(WARNING) << "uri " << status << " " << pInfo->uri << " " << (status == 0 ? "add" : "remove");
|
|
|
LOG(WARNING) << "alia " << status << " " << pInfo->alias << " " << (status == 0 ? "add" : "remove");
|
|
|
|
|
|
@@ -468,22 +556,19 @@ void DeviceTof3D::HotPlugStateCallback(const VzDeviceInfo *pInfo, int status, vo
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
- tof3dVzenseInfoMap *mp = (tof3dVzenseInfoMap *)contex;
|
|
|
+ tof3dVzenseInfoMap *mp = (tof3dVzenseInfoMap *) contex;
|
|
|
auto iter = mp->find(pInfo->ip);
|
|
|
LOG(INFO) << iter->first;
|
|
|
- if (status == 0)
|
|
|
- {
|
|
|
- LOG(WARNING) << "VZ_OpenDevice " << VZ_OpenDeviceByUri(pInfo->uri, &iter->second.handle);
|
|
|
- LOG(WARNING) << "VZ_StartStream " << VZ_StartStream(iter->second.handle);
|
|
|
- iter->second.is_connect = true;
|
|
|
- iter->second.is_start_stream = true;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- LOG(WARNING) << "VZ_StopStream " << VZ_StopStream(iter->second.handle);
|
|
|
- LOG(WARNING) << "VZ_CloseDevice " << VZ_CloseDevice(&iter->second.handle);
|
|
|
- iter->second.is_connect = false;
|
|
|
- iter->second.is_start_stream = false;
|
|
|
+ if (status == 0) {
|
|
|
+ LOG(WARNING) << "VZ_OpenDevice " << VZ_OpenDeviceByUri(pInfo->uri, &iter->second->handle);
|
|
|
+ LOG(WARNING) << "VZ_StartStream " << VZ_StartStream(iter->second->handle);
|
|
|
+ iter->second->is_connect = true;
|
|
|
+ iter->second->is_start_stream = true;
|
|
|
+ } else {
|
|
|
+ LOG(WARNING) << "VZ_StopStream " << VZ_StopStream(iter->second->handle);
|
|
|
+ LOG(WARNING) << "VZ_CloseDevice " << VZ_CloseDevice(&iter->second->handle);
|
|
|
+ iter->second->is_connect = false;
|
|
|
+ iter->second->is_start_stream = false;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
@@ -495,13 +580,14 @@ bool DeviceTof3D::drawBox(cv::Mat &mat, cv::Rect &box, cv::Scalar &color, std::s
|
|
|
cv::Size textSize = cv::getTextSize(classString, cv::FONT_HERSHEY_DUPLEX, 1, 2, 0);
|
|
|
cv::Rect textBox(box.x, box.y - 40, textSize.width + 10, textSize.height + 20);
|
|
|
cv::rectangle(mat, textBox, color, cv::FILLED);
|
|
|
- cv::putText(mat, classString, cv::Point(box.x + 5, box.y - 10), cv::FONT_HERSHEY_DUPLEX, 1, cv::Scalar(0, 0, 0), 2, 0);
|
|
|
+ cv::putText(mat, classString, cv::Point(box.x + 5, box.y - 10), cv::FONT_HERSHEY_DUPLEX, 1, cv::Scalar(0, 0, 0), 2,
|
|
|
+ 0);
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
Error_manager DeviceTof3D::updateTof3dEtc() {
|
|
|
- for (auto &info : mp_device_info) {
|
|
|
- if (info.second.etc.enable_device()) {
|
|
|
+ for (auto &info: mp_device_info) {
|
|
|
+ if (info.second->etc.enable_device()) {
|
|
|
|
|
|
}
|
|
|
}
|
|
|
@@ -510,9 +596,9 @@ Error_manager DeviceTof3D::updateTof3dEtc() {
|
|
|
|
|
|
Error_manager DeviceTof3D::loadEtc(const DeviceTof3D::VzEtcMap &etc) {
|
|
|
for (auto &iter: etc) {
|
|
|
- tof3dVzenseInfo info;
|
|
|
- info.etc = iter.second;
|
|
|
- mp_device_info.insert(std::pair<std::string, tof3dVzenseInfo>(iter.first, info));
|
|
|
+ tof3dVzenseInfo *info = new tof3dVzenseInfo();
|
|
|
+ info->etc = iter.second;
|
|
|
+ mp_device_info.insert(std::pair<std::string, tof3dVzenseInfo *>(iter.first, info));
|
|
|
LOG(INFO) << "Get device " << iter.first << " etc: " << iter.second.DebugString();
|
|
|
}
|
|
|
return Error_manager();
|
|
|
@@ -520,7 +606,7 @@ Error_manager DeviceTof3D::loadEtc(const DeviceTof3D::VzEtcMap &etc) {
|
|
|
|
|
|
void DeviceTof3D::stopWorking() {
|
|
|
for (auto &info: mp_device_info) {
|
|
|
- auto iter = mp_thread_info.find(info.second.etc.ip());
|
|
|
+ auto iter = mp_thread_info.find(info.second->etc.ip());
|
|
|
if (iter != mp_thread_info.end()) {
|
|
|
iter->second.condit->kill_all();
|
|
|
iter->second.t->join();
|
|
|
@@ -547,77 +633,68 @@ Error_manager DeviceTof3D::setTof3dParams(const std::string &ip) {
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
|
|
|
- m_vz_status = VZ_SetWorkMode(iter->second.handle, (VzWorkMode)iter->second.etc.bip().work_mode());
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_SetWorkMode(iter->second->handle, (VzWorkMode) iter->second->etc.bip().work_mode());
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_SetWorkMode failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
|
|
|
- m_vz_status = VZ_SetIRGMMGain(iter->second.handle, iter->second.etc.bip().irgmmgain());
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_SetIRGMMGain(iter->second->handle, iter->second->etc.bip().irgmmgain());
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_SetIRGMMGain failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
|
|
|
- m_vz_status = VZ_SetFrameRate(iter->second.handle, iter->second.etc.bip().frame_rate());
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_SetFrameRate(iter->second->handle, iter->second->etc.bip().frame_rate());
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_SetFrameRate failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
|
|
|
VzExposureTimeParams exposure_param;
|
|
|
- exposure_param.mode = (VzExposureControlMode)iter->second.etc.bip().enable_manual_exposure_time();
|
|
|
- exposure_param.exposureTime = iter->second.etc.bip().exposure_time();
|
|
|
- m_vz_status = VZ_SetExposureTime(iter->second.handle, VzToFSensor, exposure_param);
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ exposure_param.mode = (VzExposureControlMode) iter->second->etc.bip().enable_manual_exposure_time();
|
|
|
+ exposure_param.exposureTime = iter->second->etc.bip().exposure_time();
|
|
|
+ m_vz_status = VZ_SetExposureTime(iter->second->handle, VzToFSensor, exposure_param);
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_SetExposureTime failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
|
|
|
- m_vz_status = VZ_SetFillHoleFilterEnabled(iter->second.handle, iter->second.etc.bip().enable_filter_fill_hole());
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_SetFillHoleFilterEnabled(iter->second->handle, iter->second->etc.bip().enable_filter_fill_hole());
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_SetFillHoleFilterEnabled failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
|
|
|
- m_vz_status = VZ_SetSpatialFilterEnabled(iter->second.handle, iter->second.etc.bip().enable_filter_spatial());
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_SetSpatialFilterEnabled(iter->second->handle, iter->second->etc.bip().enable_filter_spatial());
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_SetSpatialFilterEnabled failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
|
|
|
VzFlyingPixelFilterParams fly;
|
|
|
- fly.enable = iter->second.etc.bip().enable_flying_pixel_filter();
|
|
|
- fly.threshold = iter->second.etc.bip().flying_pixel_filter_value();
|
|
|
- m_vz_status = VZ_SetFlyingPixelFilterParams(iter->second.handle, fly);
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ fly.enable = iter->second->etc.bip().enable_flying_pixel_filter();
|
|
|
+ fly.threshold = iter->second->etc.bip().flying_pixel_filter_value();
|
|
|
+ m_vz_status = VZ_SetFlyingPixelFilterParams(iter->second->handle, fly);
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_SetFlyingPixelFilterParams failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
|
|
|
VzConfidenceFilterParams confidence;
|
|
|
- confidence.enable = iter->second.etc.bip().enable_confidence_filter();
|
|
|
- confidence.threshold = iter->second.etc.bip().confidence_filter_value();
|
|
|
- m_vz_status = VZ_SetConfidenceFilterParams(iter->second.handle, confidence);
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ confidence.enable = iter->second->etc.bip().enable_confidence_filter();
|
|
|
+ confidence.threshold = iter->second->etc.bip().confidence_filter_value();
|
|
|
+ m_vz_status = VZ_SetConfidenceFilterParams(iter->second->handle, confidence);
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_SetConfidenceFilterParams failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
|
|
|
VzTimeFilterParams time_filter;
|
|
|
- time_filter.enable = iter->second.etc.bip().enable_time_filter();
|
|
|
- time_filter.threshold = iter->second.etc.bip().time_filter_value();
|
|
|
- m_vz_status = VZ_SetTimeFilterParams(iter->second.handle, time_filter);
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ time_filter.enable = iter->second->etc.bip().enable_time_filter();
|
|
|
+ time_filter.threshold = iter->second->etc.bip().time_filter_value();
|
|
|
+ m_vz_status = VZ_SetTimeFilterParams(iter->second->handle, time_filter);
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_SetTimeFilterParams failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
@@ -634,88 +711,144 @@ Error_manager DeviceTof3D::getTof3dParams(const std::string &ip) {
|
|
|
|
|
|
// 获取参数
|
|
|
VzWorkMode mode;
|
|
|
- m_vz_status = VZ_GetWorkMode(iter->second.handle, &mode);
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_GetWorkMode(iter->second->handle, &mode);
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_GetWorkMode failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
- iter->second.etc.mutable_bip()->set_work_mode(mode);
|
|
|
+ iter->second->etc.mutable_bip()->set_work_mode(mode);
|
|
|
|
|
|
uint8_t irgmmgain;
|
|
|
- m_vz_status = VZ_GetIRGMMGain(iter->second.handle, &irgmmgain);
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_GetIRGMMGain(iter->second->handle, &irgmmgain);
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_GetIRGMMGain failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
- iter->second.etc.mutable_bip()->set_irgmmgain(irgmmgain);
|
|
|
+ iter->second->etc.mutable_bip()->set_irgmmgain(irgmmgain);
|
|
|
|
|
|
int frame_rate;
|
|
|
- m_vz_status = VZ_GetFrameRate(iter->second.handle, &frame_rate);
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_GetFrameRate(iter->second->handle, &frame_rate);
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_GetFrameRate failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
- iter->second.etc.mutable_bip()->set_frame_rate(frame_rate);
|
|
|
+ iter->second->etc.mutable_bip()->set_frame_rate(frame_rate);
|
|
|
|
|
|
VzExposureTimeParams exposure_param = {VzExposureControlMode_Manual, 0};
|
|
|
- m_vz_status = VZ_GetProperty(iter->second.handle, "Py_ToFExposureTimeMax", &exposure_param, sizeof(exposure_param));
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_GetProperty(iter->second->handle, "Py_ToFExposureTimeMax", &exposure_param,
|
|
|
+ sizeof(exposure_param));
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_GetExposureTime failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
- iter->second.etc.mutable_bip()->set_enable_manual_exposure_time(exposure_param.mode);
|
|
|
- iter->second.etc.mutable_bip()->set_exposure_time(exposure_param.exposureTime);
|
|
|
+ iter->second->etc.mutable_bip()->set_enable_manual_exposure_time(exposure_param.mode);
|
|
|
+ iter->second->etc.mutable_bip()->set_exposure_time(exposure_param.exposureTime);
|
|
|
|
|
|
bool boolret;
|
|
|
- m_vz_status = VZ_GetFillHoleFilterEnabled(iter->second.handle, &boolret);
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_GetFillHoleFilterEnabled(iter->second->handle, &boolret);
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_GetFillHoleFilterEnabled failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
- iter->second.etc.mutable_bip()->set_enable_filter_fill_hole(boolret);
|
|
|
+ iter->second->etc.mutable_bip()->set_enable_filter_fill_hole(boolret);
|
|
|
|
|
|
- m_vz_status = VZ_GetSpatialFilterEnabled(iter->second.handle, &boolret);
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_GetSpatialFilterEnabled(iter->second->handle, &boolret);
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_GetSpatialFilterEnabled failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
- iter->second.etc.mutable_bip()->set_enable_filter_spatial(boolret);
|
|
|
+ iter->second->etc.mutable_bip()->set_enable_filter_spatial(boolret);
|
|
|
|
|
|
VzFlyingPixelFilterParams fly;
|
|
|
- m_vz_status = VZ_GetFlyingPixelFilterParams(iter->second.handle, &fly);
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_GetFlyingPixelFilterParams(iter->second->handle, &fly);
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_GetFlyingPixelFilterParams failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
- iter->second.etc.mutable_bip()->set_enable_flying_pixel_filter(fly.enable);
|
|
|
- iter->second.etc.mutable_bip()->set_flying_pixel_filter_value(fly.threshold);
|
|
|
+ iter->second->etc.mutable_bip()->set_enable_flying_pixel_filter(fly.enable);
|
|
|
+ iter->second->etc.mutable_bip()->set_flying_pixel_filter_value(fly.threshold);
|
|
|
|
|
|
VzConfidenceFilterParams confidence;
|
|
|
- m_vz_status = VZ_GetConfidenceFilterParams(iter->second.handle, &confidence);
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_GetConfidenceFilterParams(iter->second->handle, &confidence);
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_GetConfidenceFilterParams failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
- iter->second.etc.mutable_bip()->set_enable_confidence_filter(confidence.enable);
|
|
|
- iter->second.etc.mutable_bip()->set_confidence_filter_value(confidence.threshold);
|
|
|
+ iter->second->etc.mutable_bip()->set_enable_confidence_filter(confidence.enable);
|
|
|
+ iter->second->etc.mutable_bip()->set_confidence_filter_value(confidence.threshold);
|
|
|
|
|
|
VzTimeFilterParams time_filter;
|
|
|
- m_vz_status = VZ_GetTimeFilterParams(iter->second.handle, &time_filter);
|
|
|
- if (m_vz_status != VzReturnStatus::VzRetOK)
|
|
|
- {
|
|
|
+ m_vz_status = VZ_GetTimeFilterParams(iter->second->handle, &time_filter);
|
|
|
+ if (m_vz_status != VzReturnStatus::VzRetOK) {
|
|
|
LOG(WARNING) << ip << " VZ_GetTimeFilterParams failed status:" << m_vz_status;
|
|
|
return {}; //TODO
|
|
|
}
|
|
|
- iter->second.etc.mutable_bip()->set_enable_time_filter(time_filter.enable);
|
|
|
- iter->second.etc.mutable_bip()->set_time_filter_value(time_filter.threshold);
|
|
|
+ iter->second->etc.mutable_bip()->set_enable_time_filter(time_filter.enable);
|
|
|
+ iter->second->etc.mutable_bip()->set_time_filter_value(time_filter.threshold);
|
|
|
+
|
|
|
+ return Error_manager();
|
|
|
+}
|
|
|
+
|
|
|
+Error_manager DeviceTof3D::segFrame2CarAndWheel(VzFrame &depth_frame, std::vector<cv::Point> &wheel_cv_cloud,
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud,
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud) {
|
|
|
+
|
|
|
+ auto iter = mp_device_info.find("192.168.2.102");
|
|
|
+ if (iter->second->handle != nullptr) {
|
|
|
+ VzSensorIntrinsicParameters cameraParam = {};
|
|
|
+ VZ_GetSensorIntrinsicParameters(iter->second->handle, VzToFSensor, &cameraParam);
|
|
|
+
|
|
|
+ const uint16_t *pDepthFrameData = (uint16_t *) depth_frame.pFrameData;
|
|
|
+ for (auto &pt: wheel_cv_cloud) {
|
|
|
+ VzDepthVector3 depthPoint = {pt.x, pt.y, pDepthFrameData[pt.y * depth_frame.width + pt.x]};
|
|
|
+ VzVector3f worldV = {};
|
|
|
+ VZ_ConvertDepthToPointCloud(iter->second->handle, &depthPoint, &worldV, 1, &cameraParam);
|
|
|
+ wheel_cloud->emplace_back(worldV.x, worldV.y, worldV.z);
|
|
|
+ }
|
|
|
+ LOG(INFO) << wheel_cloud->size();
|
|
|
+
|
|
|
+ VzFrame &srcFrame = depth_frame;
|
|
|
+ const int len = srcFrame.width * srcFrame.height;
|
|
|
+ VzVector3f *worldV = new VzVector3f[len];
|
|
|
+
|
|
|
+ m_vz_status = VZ_ConvertDepthFrameToPointCloudVector(iter->second->handle, &srcFrame,
|
|
|
+ worldV);
|
|
|
+
|
|
|
+ if (m_vz_status == VzRetOK) {
|
|
|
+ car_cloud->clear();
|
|
|
+ for (int i = 0; i < len; i++) {
|
|
|
+ if (worldV[i].x == 0 && worldV[i].y == 0 && worldV[i].z == 0) {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ car_cloud->points.emplace_back(worldV[i].x, worldV[i].y, worldV[i].z);
|
|
|
+ }
|
|
|
+ delete[] worldV;
|
|
|
+ worldV = nullptr;
|
|
|
+ LOG(INFO) << "Save point cloud successful to cloud: " << car_cloud->size();
|
|
|
+ }
|
|
|
+
|
|
|
+// for (int i = (depth_frame.height - WINDOW_SIZE)/2, offset = i * depth_frame.width; i < (depth_frame.height + WINDOW_SIZE)/2; i++)
|
|
|
+// {
|
|
|
+// for (int j = (depth_frame.width - WINDOW_SIZE)/2; j < (depth_frame.width + WINDOW_SIZE)/2; j++)
|
|
|
+// {
|
|
|
+// VzDepthVector3 depthPoint = {j, i, pDepthFrameData[offset + j]};
|
|
|
+// VzVector3f worldV = {};
|
|
|
+// VZ_ConvertDepthToPointCloud(iter->second->handle, &depthPoint, &worldV, 1, &cameraParam);
|
|
|
+// if (0 < worldV.z && worldV.z < 0xFFFF)
|
|
|
+// {
|
|
|
+// LOG(INFO) << worldV.x << "\t" << worldV.y << "\t" << worldV.z << std::endl;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// offset += depth_frame.width;
|
|
|
+// }
|
|
|
+ }
|
|
|
+ return Error_manager();
|
|
|
+}
|
|
|
+
|
|
|
+Error_manager DeviceTof3D::WheelCloudOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud) {
|
|
|
+ return Error_manager();
|
|
|
+}
|
|
|
|
|
|
+Error_manager DeviceTof3D::CarPoseOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud) {
|
|
|
return Error_manager();
|
|
|
}
|
LiuZe