# zed tutorial
###### tags: `notes`
## basic
### create ZED object
``` cpp
Camera zed;
```
### Open the camera
``` cpp
// Open the camera
ERROR_CODE returned_state = zed.open();
if (returned_state != ERROR_CODE::SUCCESS) {
cout << "Error " << returned_state << ", exit program.\n";
return EXIT_FAILURE;
}
```
### close the camera
``` cpp
zed.close();
```
### Set configuration parameters
``` cpp
InitParameters init_parameters;
init_parameters.camera_resolution = RESOLUTION::HD1080; // Use HD1080 video mode
init_parameters.camera_fps = 30; // Set fps at 30
```
## capture image
the ZED SDK provides its own sl::Mat class to store image data
[【OpenCV】Mat的使用方法以及Mat中的参数和函数](https://blog.csdn.net/Gordon_Wei/article/details/85037684)
know that datat can be shred between ```sl::Mat``` and ```cv::MAT``` by having both pointer pointing to the samee address
```cpp
// Grab an image
if (zed.grab() == ERROR_CODE::SUCCESS) {
// A new image is available if grab() returns ERROR_CODE::SUCCESS
}
```
```cpp
zed.retrieveImage(image,VIEW_LEFT); // Get the left image
unsigned long long timestamp = zed.getCameraTimestamp(); // Get the timestamp of the image
printf("Image resolution: %d x %d || Image timestamp: %llu\n", image.getWidth(), image.getHeight(), timestamp);
```
## capture depth
We call retrieveMeasure() to get the depth map and the point cloud.
```cpp
// after an image is caprured
zed.retrieveImage(image, VIEW::LEFT); // Get the left imag
zed.retrieveMeasure(depth, MEASURE::DEPTH); // Retrieve depth Mat. Depth is aligned on the left image
zed.retrieveMeasure(point_cloud, MEASURE::XYZRGBA);
```
retrive the distance from the middle of the point cloud
```cpp
// Get and print distance value in mm at the center of the image
// We measure the distance camera - object using Euclidean distance
int x = image.getWidth() / 2;
int y = image.getHeight() / 2;
sl::float4 point_cloud_value;
point_cloud.getValue(x, y, &point_cloud_value);
float distance = sqrt(point_cloud_value.x*point_cloud_value.x + point_cloud_value.y*point_cloud_value.y + point_cloud_value.z*point_cloud_value.z);
printf("Distance to Camera at (%d, %d): %f mm\n", x, y, distance);
```
retrive the distance from depth map
```cpp
int x = image.getWidth() / 2;
int y = image.getHeight() / 2;
float depth_value;
depth.getValue(x,y, depth_value);
printf("Depth to Camera at (%d, %d): %f mm\n", x, y, depth_value);
```
## position tracking
enable the position tracking
```cpp
// Enable positional tracking with default parameters
sl::PositionalTrackingParameters tracking_parameters;
err = zed.enablePositionalTracking(tracking_parameters);
if (err != ERROR_CODE::SUCCESS)
exit(-1);
```
The camera position is given by the class ```sl::Pose```.
A pose is always linked to a reference frame. SDK provides two reference frame ```REFERENCE_FRAME::WORLD``` and ```REFERENCE_FRAME::CAMERA```.
```cpp
sl::Pose zed_pose;
if (zed.grab() == ERROR_CODE::SUCCESS) {
// Get the pose of the left eye of the camera with reference to the world frame
zed.getPosition(zed_pose, REFERENCE_FRAME::WORLD);
// Display the translation and timestamp
printf("Translation: Tx: %.3f, Ty: %.3f, Tz: %.3f, Timestamp: %llu\n", zed_pose.getTranslation().tx, zed_pose.getTranslation().ty, zed_pose.getTranslation().tz, zed_pose.timestamp);
// Display the orientation quaternion
printf("Orientation: Ox: %.3f, Oy: %.3f, Oz: %.3f, Ow: %.3f\n\n", zed_pose.getOrientation().ox, zed_pose.getOrientation().oy, zed_pose.getOrientation().oz, zed_pose.getOrientation().ow);
}
```
disable the position tracking before close
```cpp
zed.disablePositionalTracking();
```
## spatial mapping
Enable positional tracking
```cpp
sl::PositionalTrackingParameters tracking_parameters;
err = zed.enablePositionalTracking(tracking_parameters);
if (err != ERROR_CODE::SUCCESS)
exit(-1);
```
enable spatial mapping
```cpp
sl::SpatialMappingParameters mapping_parameters;
err = zed.enableSpatialMapping(mapping_parameters);
if (err != ERROR_CODE::SUCCESS)
exit(-1);
```
capture data
```cpp
sl::Mesh mesh; // Create a mesh object
if (zed.grab() == ERROR_CODE::SUCCESS) {
// In background, spatial mapping will use new images, depth and pose to create and update the mesh. No specific functions are required here
sl::SPATIAL_MAPPING_STATE mapping_state = zed.getSpatialMappingState();
// Print spatial mapping state
std::cout << "\rImages captured: " << i << " / 500 || Spatial mapping state: " << spatialMappingState2str(mapping_state) << " " << std::flush;
```
extract whole mesh
```cpp
zed.extractWholeMesh(mesh); // Extract the whole mesh
```
remove duplicate vertices and unneeded faces
```cpp
mesh.filter(sl::MeshFilterParameters::MESH_FILTER::LOW); // Filter the mesh (remove unnecessary vertices and faces)
```
save the mesh
```cpp
mesh.save("mesh.obj"); // Save the mesh in an obj file
```
disable the modules and close the camera before exiting the program.
## object detection
create zed with some initial parameters
``` cpp
Camera zed;
InitParameters initParameters;
initParameters.camera_resolution = RESOLUTION::HD720;
initParameters.depth_mode = DEPTH_MODE::PERFORMANCE;
initParameters.sdk_verbose = true;
```
enable object detection
```cpp
// Define the Objects detection module parameters
ObjectDetectionParameters detection_parameters;
detection_parameters.enable_tracking = false;
detection_parameters.enable_mask_output = false;
detection_parameters.image_sync = false;
// Object tracking requires the positional tracking module
if (detection_parameters.enable_tracking)
zed.enablePositionalTracking();
```
load the module
```cpp
std::cout << "Object Detection: Loading Module..." << std::endl;
zed_error = zed.enableObjectDetection(detection_parameters);
if (zed_error != ERROR_CODE::SUCCESS) {
std::cout << "Error " << zed_error << ", exit program.\n";
zed.close();
return 1;
}
```
The object confidence threshold can be adjusted at runtime to select only the revelant objects depending on the scene complexity.
```cpp
// Detection runtime parameters
ObjectDetectionRuntimeParameters detection_parameters_rt;
detection_parameters_rt.detection_confidence_threshold = 40;
// Detection output
Objects objects;
while (zed.grab() == ERROR_CODE::SUCCESS) {
zed_error = zed.retrieveObjects(objects, detection_parameters_rt);
if (objects.is_new) {
std::cout << objects.object_list.size() << " Object(s) detected ("
<< zed.getCurrentFPS() << " FPS)" << std::endl;
}
}
```
## trouble shooting
```libcuda.so.1 is not found:```
> https://github.com/stereolabs/zed-docker/blob/246cdd16523ee88682cfc4bfd1c5222e99774d4f/README.md
```failed to open display ':1' ```
>simply add this ```xhost local:root```