Updates for ZED SDK 4.0.7 (#592)

* Updates for ZED SDK 4.0.7

* Remove build directories

* Add .gitignore

---------

Co-authored-by: Matthew Rousseau <[email protected]>

Author: mattrouss

.gitignore

@@ -111,7 +111,9 @@ int main(int argc, char **argv) {
 
     // Configure object detection runtime parameters
     BodyTrackingRuntimeParameters body_tracker_parameters_rt;
-    body_tracker_parameters_rt.detection_confidence_threshold = 40;
+    body_tracker_parameters_rt.detection_confidence_threshold = 60;
+    body_tracker_parameters_rt.skeleton_smoothing = 0.7;
+    
     // Create ZED Bodies filled in the main loop
     Bodies bodies;
 

body tracking/body tracking/cpp/src/main.cpp

@@ -298,6 +298,10 @@ void parseArgs(int argc, char** argv, sl::InitParameters& param) {
             param.camera_resolution = sl::RESOLUTION::HD2K;
             std::cout << "[Sample] Using Camera in resolution HD2K" << std::endl;
         }
+        else if (arg.find("HD1200") != std::string::npos) {
+            param.camera_resolution = sl::RESOLUTION::HD1200;
+            std::cout << "[Sample] Using Camera in resolution HD1200" << std::endl;
+        }
         else if (arg.find("HD1080") != std::string::npos) {
             param.camera_resolution = sl::RESOLUTION::HD1080;
             std::cout << "[Sample] Using Camera in resolution HD1080" << std::endl;
@@ -306,6 +310,10 @@ void parseArgs(int argc, char** argv, sl::InitParameters& param) {
             param.camera_resolution = sl::RESOLUTION::HD720;
             std::cout << "[Sample] Using Camera in resolution HD720" << std::endl;
         }
+        else if (arg.find("SVGA") != std::string::npos) {
+            param.camera_resolution = sl::RESOLUTION::SVGA;
+            std::cout << "[Sample] Using Camera in resolution SVGA" << std::endl;
+        }
         else if (arg.find("VGA") != std::string::npos) {
             param.camera_resolution = sl::RESOLUTION::VGA;
             std::cout << "[Sample] Using Camera in resolution VGA" << std::endl;

body tracking/export/fbx export/cpp/src/main.cpp

@@ -13,36 +13,16 @@ class ClientPublisher{
     ~ClientPublisher();
 
     bool open(sl::InputType);
-
     void start();
     void stop();
-
-    sl::Mat getViewRef(){
-        return view;
-    }
-
-    sl::Mat getPointCloufRef(){
-        return point_cloud;
-    }
-
-    CUstream getStream() {
-        return zed.getCUDAStream();
-    }
+    void setStartSVOPosition(unsigned pos);
 
     bool isRunning() {
         return running;
     }
 
-    int getSerial() {
-        return serial;
-    }
-
-
 private:
-    sl::Resolution low_resolution;
-    sl::Mat point_cloud, view;
     sl::Camera zed;
-    sl::InitParameters init_parameters;
     void work();
     std::thread runner;
     bool running;

body tracking/multi-camera/cpp/include/ClientPublisher.hpp

@@ -162,7 +162,7 @@ class PointCloud {
     void initialize(sl::Mat&, sl::float3 clr);
     // Push a new point cloud
     // Warning: can be called from any thread but the mutex "mutexData" must be locked
-    void pushNewPC(CUstream);
+    void pushNewPC();
     // Draw the point cloud
     // Warning: must be called in the Opengl thread
     void draw(const sl::Transform& vp, bool draw_clr);
@@ -192,7 +192,7 @@ class CameraViewer {
     // Initialize Opengl and Cuda buffers
     bool initialize(sl::Mat& image, sl::float3 clr);
     // Push a new Image + Z buffer and transform into a point cloud
-    void pushNewImage(CUstream);
+    void pushNewImage();
     // Draw the Image
     void draw(sl::Transform vpMatrix);
     // Close (disable update)
@@ -231,7 +231,7 @@ class GLViewer {
     bool isAvailable();
     void init(int argc, char **argv);
 
-    void setupNewCamera(int, sl::Mat, sl::Mat, CUstream );
+    void updateCamera(int, sl::Mat &, sl::Mat &);
 
     void updateBodies(sl::Bodies &objs,std::map<sl::CameraIdentifier, sl::Bodies>& singldata, sl::FusionMetrics& metrics);
 
@@ -301,7 +301,7 @@ class GLViewer {
     std::map<int, PointCloud> point_clouds;
     std::map<int, CameraViewer> viewers;
     std::map<int, sl::Transform> poses;
-    std::map<int, CUstream> streams;
+    
     std::map<int, Simple3DObject> skeletons_raw;
     std::map<int, sl::float3> colors;
     std::map<int, sl::float3> colors_sk;

body tracking/multi-camera/cpp/include/GLViewer.hpp

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <sl/Camera.hpp>
+
+/**
+* @brief Compute the start frame of each SVO for playback to be synced
+*
+* @param svo_files Map camera index to SVO file path
+* @return Map camera index to starting SVO frame for synced playback
+*/
+std::map<int, int> syncDATA(std::map<int, std::string> svo_files) {
+    std::map<int, int> output; // map of camera index and frame index of the starting point for each
+
+    // Open all SVO
+    std::map<int, std::shared_ptr<sl::Camera>> p_zeds;
+
+    for (auto &it : svo_files) {
+        auto p_zed = std::make_shared<sl::Camera>();
+
+        sl::InitParameters init_param;
+        init_param.depth_mode = sl::DEPTH_MODE::NONE;
+        init_param.camera_disable_self_calib = true;
+        init_param.input.setFromSVOFile(it.second.c_str());
+
+        auto error = p_zed->open(init_param);
+        if (error == sl::ERROR_CODE::SUCCESS)
+            p_zeds.insert(std::make_pair(it.first, p_zed));
+        else {
+            std::cerr << "Could not open file " << it.second.c_str() << ": " << sl::toString(error) << ". Skipping" << std::endl;
+        }
+    }
+
+    // Compute the starting point, we have to take the latest one
+    sl::Timestamp start_ts = 0;
+    for (auto &it : p_zeds) {
+        it.second->grab();
+        auto ts = it.second->getTimestamp(sl::TIME_REFERENCE::IMAGE);
+
+        if (ts > start_ts)
+            start_ts = ts;
+    }
+
+    std::cout << "Found SVOs common starting time: " << start_ts << std::endl;
+
+    // The starting point is now known, let's find the frame idx for all corresponding
+    for (auto &it : p_zeds) {
+        auto frame_position_at_ts = it.second->getSVOPositionAtTimestamp(start_ts);
+
+        if (frame_position_at_ts != -1)
+            output.insert(std::make_pair(it.first, frame_position_at_ts));
+    }
+
+    for (auto &it : p_zeds) it.second->close();
+
+    return output;
+}

body tracking/multi-camera/cpp/include/utils.hpp

@@ -2,7 +2,6 @@
 
 ClientPublisher::ClientPublisher() : running(false)
 {
-    init_parameters.depth_mode = sl::DEPTH_MODE::ULTRA;
 }
 
 ClientPublisher::~ClientPublisher()
@@ -15,7 +14,11 @@ bool ClientPublisher::open(sl::InputType input) {
     if (runner.joinable())
         return false;
 
+    sl::InitParameters init_parameters;
+    init_parameters.depth_mode = sl::DEPTH_MODE::ULTRA;
     init_parameters.input = input;
+    if (input.getType() == sl::InputType::INPUT_TYPE::SVO_FILE)
+        init_parameters.svo_real_time_mode = true;
     init_parameters.coordinate_units = sl::UNIT::METER;
     init_parameters.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP;
     auto state = zed.open(init_parameters);
@@ -39,7 +42,7 @@ bool ClientPublisher::open(sl::InputType input) {
     // define the body tracking parameters, as the fusion can does the tracking and fitting you don't need to enable them here, unless you need it for your app
     sl::BodyTrackingParameters body_tracking_parameters;
     body_tracking_parameters.detection_model = sl::BODY_TRACKING_MODEL::HUMAN_BODY_MEDIUM;
-    body_tracking_parameters.body_format = sl::BODY_FORMAT::BODY_38;
+    body_tracking_parameters.body_format = sl::BODY_FORMAT::BODY_18;
     body_tracking_parameters.enable_body_fitting = false;
     body_tracking_parameters.enable_tracking = false;
     state = zed.enableBodyTracking(body_tracking_parameters);
@@ -49,20 +52,6 @@ bool ClientPublisher::open(sl::InputType input) {
         return false;
     }
 
-    auto camera_infos = zed.getCameraInformation();
-
-    serial = camera_infos.serial_number;
-
-    auto resolution = camera_infos.camera_configuration.resolution;
-
-    // Define display resolution and check that it fit at least the image resolution
-    float image_aspect_ratio = resolution.width / (1.f * resolution.height);
-    int requested_low_res_w = std::min(640, (int)resolution.width);
-    low_resolution = sl::Resolution(requested_low_res_w, requested_low_res_w / image_aspect_ratio);
-    
-    view.alloc(low_resolution, sl::MAT_TYPE::U8_C4, sl::MEM::GPU);
-    point_cloud.alloc(low_resolution, sl::MAT_TYPE::F32_C4, sl::MEM::GPU);
-
     return true;
 }
 
@@ -91,22 +80,25 @@ void ClientPublisher::work()
     sl::BodyTrackingRuntimeParameters body_runtime_parameters;
     body_runtime_parameters.detection_confidence_threshold = 40;
 
-    sl::RuntimeParameters runtime_parameters;
-    runtime_parameters.confidence_threshold = 30;
 
     // in this sample we use a dummy thread to process the ZED data.
     // you can replace it by your own application and use the ZED like you use to, retrieve its images, depth, sensors data and so on.
-    // as long as you call the grab function and the retrieveBodies (wich run the detection) the camera will be able to seamlessly transmit the data to the fusion module.
+    // as long as you call the grab function and the retrieveBodies (which runs the detection) the camera will be able to seamlessly transmit the data to the fusion module.
     while (running) {
-        if (zed.grab(runtime_parameters) == sl::ERROR_CODE::SUCCESS) {
+        if (zed.grab() == sl::ERROR_CODE::SUCCESS) {
             /*
             Your App
-            */            
-            zed.retrieveImage(view, sl::VIEW::LEFT, sl::MEM::GPU, low_resolution);
-            zed.retrieveMeasure(point_cloud, sl::MEASURE::XYZBGRA, sl::MEM::GPU, low_resolution);
+
+            */
 
             // just be sure to run the bodies detection
             zed.retrieveBodies(bodies, body_runtime_parameters);
         }
     }
 }
+
+void ClientPublisher::setStartSVOPosition(unsigned pos) {
+    zed.setSVOPosition(pos);
+    zed.grab();
+}
+

body tracking/multi-camera/cpp/src/ClientPublisher.cpp

@@ -221,15 +221,19 @@ sl::float3 newColor(float hh) {
     return clr;
 }
 
-void GLViewer::setupNewCamera(int id, sl::Mat view, sl::Mat pc, CUstream strm){
+void GLViewer::updateCamera(int id, sl::Mat &view, sl::Mat &pc){
     mtx.lock();
     if (colors.find(id) == colors.end()) {
         float hh = uint_dist360(rng) / 60.f;
         colors[id] = newColor(hh);
     }
-    viewers[id].initialize(view, colors[id]);
-    point_clouds[id].initialize(pc, colors[id]);
-    streams[id] = strm;
+
+    if(view.isInit() && viewers.find(id) == viewers.end())
+        viewers[id].initialize(view, colors[id]);
+
+    if(pc.isInit() && point_clouds.find(id) == point_clouds.end())
+        point_clouds[id].initialize(pc, colors[id]);
+    
     mtx.unlock();
 }
 
@@ -281,7 +285,6 @@ void GLViewer::render() {
 void GLViewer::setCameraPose(int id, sl::Transform pose) {
     mtx.lock();
     poses[id] = pose;
-
     if (colors.find(id) == colors.end()) {
         float hh = uint_dist360(rng) / 60.f;      
         colors[id] = newColor(hh);
@@ -436,10 +439,10 @@ void GLViewer::update() {
         it.second.pushToGPU();
 
     for(auto &it: point_clouds)
-        it.second.pushNewPC(streams[it.first]);
+        it.second.pushNewPC();
 
     for(auto &it: viewers)
-        it.second.pushNewImage(streams[it.first]);
+        it.second.pushNewImage();
 
     mtx.unlock();
     clearInputs();
@@ -792,9 +795,9 @@ void PointCloud::initialize(sl::Mat &ref, sl::float3 clr_) {
     shDrawColor = glGetUniformLocation(shader_.getProgramId(), "u_drawFlat");
 }
 
-void PointCloud::pushNewPC(CUstream strm) {
+void PointCloud::pushNewPC() {
     if (refMat.isInit())
-        cudaMemcpyAsync(xyzrgbaMappedBuf_, refMat.getPtr<sl::float4>(sl::MEM::GPU), numBytes_, cudaMemcpyDeviceToDevice, strm);
+        cudaMemcpy(xyzrgbaMappedBuf_, refMat.getPtr<sl::float4>(sl::MEM::CPU), numBytes_, cudaMemcpyHostToDevice);
 }
 
 void PointCloud::draw(const sl::Transform& vp, bool draw_flat) {
@@ -949,9 +952,9 @@ bool CameraViewer::initialize(sl::Mat &im, sl::float3 clr) {
 	return (err == cudaSuccess);
 }
 
-void CameraViewer::pushNewImage(CUstream strm) {
+void CameraViewer::pushNewImage() {
 	if (!ref.isInit())  return;
-	auto err = cudaMemcpy2DToArrayAsync(ArrIm, 0, 0, ref.getPtr<sl::uchar1>(sl::MEM::GPU), ref.getStepBytes(sl::MEM::GPU), ref.getPixelBytes() * ref.getWidth(), ref.getHeight(), cudaMemcpyDeviceToDevice, strm);
+	auto err = cudaMemcpy2DToArray(ArrIm, 0, 0, ref.getPtr<sl::uchar1>(sl::MEM::CPU), ref.getStepBytes(sl::MEM::CPU), ref.getPixelBytes() * ref.getWidth(), ref.getHeight(), cudaMemcpyHostToDevice);
 	if (err) std::cout << "err 2 " << err << " " << cudaGetErrorString(err) << "\n";
 }