├── .github ├── ISSUE_TEMPLATE │ ├── 1_feature_request.yml │ ├── 2_bug_report.yml │ └── config.yml └── workflows │ └── stale_issues.yml ├── LICENSE ├── README.md ├── ZED_Camera.png ├── ZED_PointCloud.png ├── ZED_Tracking.png ├── matlab ├── ZED_BodyTracking.m ├── ZED_Camera.m ├── ZED_CameraControl.m ├── ZED_DepthSensing.m ├── ZED_Mapping.m ├── ZED_ObjectDetection.m ├── ZED_PointCloud.m ├── ZED_Recording.m ├── ZED_Sensors.m └── ZED_Tracking.m └── src ├── CMakeLists.txt ├── Matlabdef.def ├── cmake └── FindMatlab.cmake ├── include ├── iter.h ├── mc_convert.h └── types.h └── mex ├── CMakeLists.txt └── mexZED.cpp /.github/ISSUE_TEMPLATE/1_feature_request.yml: -------------------------------------------------------------------------------- 1 | name: Feature request 🧭 2 | description: Suggest an idea for this project. 3 | labels: "feature request" 4 | body: 5 | - type: markdown 6 | attributes: 7 | value: | 8 | # Welcome 👋 9 | 10 | Thanks for taking the time to fill out this feature request module. 11 | Please fill out each section below. This info allows Stereolabs developers to correctly evaluate your request. 12 | 13 | Useful Links: 14 | - Documentation: https://www.stereolabs.com/docs/ 15 | - Stereolabs support: https://support.stereolabs.com/hc/en-us/ 16 | - type: checkboxes 17 | attributes: 18 | label: Preliminary Checks 19 | description: Please make sure that you verify each checkbox and follow the instructions for them. 20 | options: 21 | - label: "This issue is not a duplicate. Before opening a new issue, please search existing issues." 22 | required: true 23 | - label: "This issue is not a question, bug report, or anything other than a feature request directly related to this project." 24 | required: true 25 | - type: textarea 26 | attributes: 27 | label: Proposal 28 | description: "What would you like to have as a new feature?" 29 | placeholder: "A clear and concise description of what you want to happen." 30 | validations: 31 | required: true 32 | - type: textarea 33 | attributes: 34 | label: Use-Case 35 | description: "How would this help you?" 36 | placeholder: "Tell us more what you'd like to achieve." 37 | validations: 38 | required: false 39 | - type: textarea 40 | id: anything-else 41 | attributes: 42 | label: Anything else? 43 | description: "Let us know if you have anything else to share" 44 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/2_bug_report.yml: -------------------------------------------------------------------------------- 1 | name: Bug Report 🐛 2 | description: Something isn't working as expected? Report your bugs here. 3 | labels: "bug" 4 | body: 5 | - type: markdown 6 | attributes: 7 | value: | 8 | # Welcome 👋 9 | 10 | Thanks for taking the time to fill out this bug report. 11 | Please fill out each section below. This info allows Stereolabs developers to diagnose (and fix!) your issue as quickly as possible. Otherwise we might need to close the issue without e.g. clear reproduction steps. 12 | 13 | Bug reports also shoulnd't be used for generic questions, please use the [Stereolabs Community forum](https://community.stereolabs.com/) instead. 14 | 15 | Useful Links: 16 | - Documentation: https://www.stereolabs.com/docs/ 17 | - Stereolabs support: https://support.stereolabs.com/hc/en-us/ 18 | - type: checkboxes 19 | attributes: 20 | label: Preliminary Checks 21 | description: Please make sure that you verify each checkbox and follow the instructions for them. 22 | options: 23 | - label: "This issue is not a duplicate. Before opening a new issue, please search existing issues." 24 | required: true 25 | - label: "This issue is not a question, feature request, or anything other than a bug report directly related to this project." 26 | required: true 27 | - type: textarea 28 | attributes: 29 | label: Description 30 | description: Describe the issue that you're seeing. 31 | placeholder: Be as precise as you can. Feel free to share screenshots, videos, or data. The more information you provide the easier will be to provide you with a fast solution. 32 | validations: 33 | required: true 34 | - type: textarea 35 | attributes: 36 | label: Steps to Reproduce 37 | description: Clear steps describing how to reproduce the issue. 38 | value: | 39 | 1. 40 | 2. 41 | 3. 42 | ... 43 | validations: 44 | required: true 45 | - type: textarea 46 | attributes: 47 | label: Expected Result 48 | description: Describe what you expected to happen. 49 | validations: 50 | required: true 51 | - type: textarea 52 | attributes: 53 | label: Actual Result 54 | description: Describe what actually happened. 55 | validations: 56 | required: true 57 | - type: dropdown 58 | attributes: 59 | label: ZED Camera model 60 | description: What model of ZED camera are you using? 61 | options: 62 | - "ZED" 63 | - "ZED Mini" 64 | - "ZED2" 65 | - "ZED2i" 66 | validations: 67 | required: true 68 | - type: textarea 69 | attributes: 70 | label: Environment 71 | render: shell 72 | description: Useful information about your system. 73 | placeholder: | 74 | OS: Operating System 75 | CPU: e.g. ARM 76 | GPU: Nvidia Jetson Xavier NX 77 | ZED SDK version: e.g. v3.5.3 78 | Other info: e.g. ROS Melodic 79 | validations: 80 | required: true 81 | - type: textarea 82 | attributes: 83 | label: Anything else? 84 | description: Please add any other information or comment that you think may be useful for solving the problem 85 | placeholder: 86 | validations: 87 | required: false 88 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/config.yml: -------------------------------------------------------------------------------- 1 | blank_issues_enabled: false 2 | contact_links: 3 | - name: Online Documentation 4 | url: https://www.stereolabs.com/docs/ 5 | about: Check out the Stereolabs documentation for answers to common questions. 6 | - name: Stereolabs Community 7 | url: https://community.stereolabs.com/ 8 | about: Ask questions, request features & discuss with other users and developers. 9 | - name: Stereolabs Twitter 10 | url: https://twitter.com/Stereolabs3D 11 | about: The official Stereolabs Twitter account to ask questions, comment our products and share your projects with the ZED community. 12 | 13 | -------------------------------------------------------------------------------- /.github/workflows/stale_issues.yml: -------------------------------------------------------------------------------- 1 | name: 'Stale issue handler' 2 | on: 3 | workflow_dispatch: 4 | schedule: 5 | - cron: '00 00 * * *' 6 | 7 | jobs: 8 | stale: 9 | runs-on: ubuntu-latest 10 | steps: 11 | - uses: actions/stale@main 12 | id: stale 13 | with: 14 | stale-issue-message: 'This issue is stale because it has been open 30 days with no activity. Remove stale label or comment otherwise it will be automatically closed in 5 days' 15 | stale-pr-message: 'This PR is stale because it has been open 30 days with no activity. Remove stale label or comment otherwise it will be automatically closed in 5 days' 16 | days-before-stale: 30 17 | days-before-close: 5 18 | operations-per-run: 1500 19 | exempt-issue-labels: 'feature_request' 20 | exempt-pr-labels: 'feature_request' 21 | enable-statistics: 'true' 22 | close-issue-label: 'closed_for_stale' 23 | close-pr-label: 'closed_for_stale' 24 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Stereolabs 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Stereolabs ZED - Matlab Integration 2 | 3 | This sample shows how to use the ZED SDK functionalities within Matlab. 4 | 5 | ## Getting started 6 | 7 | - First, download the latest version of the ZED SDK on [stereolabs.com](https://www.stereolabs.com/developers) 8 | - For more information, read the ZED [Documentation](https://www.stereolabs.com/docs/) and [API documentation](https://www.stereolabs.com/docs/api/python/) or our [Community page](https://community.stereolabs.com) 9 | 10 | ### Prerequisites 11 | 12 | - Windows 10 64bits or later, Ubuntu 16.04 or higher. 13 | - Matlab with MEX compiler installed 14 | - [ZED SDK](https://www.stereolabs.com/developers/release/) and its dependency ([CUDA](https://developer.nvidia.com/cuda-downloads)) 15 | 16 | 17 | ## Build the program 18 | 19 | #### Build for Windows 20 | 21 | For detailed installation instructions, check out our [documentation](https://www.stereolabs.com/docs/matlab#using-the-zed-sdk). 22 | 23 | #### Build for Linux 24 | 25 | Download the sample and execute the following command in a terminal: 26 | 27 | export MATLAB_ROOT=/usr/local/MATLAB/R2012b # Put your actual Matlab path here 28 | mkdir build 29 | cd build 30 | cmake ../src 31 | make 32 | make install 33 | 34 | ## Run the program 35 | In the Matlab directory, open the file `ZED_DepthSensing.m` with Matlab and press run. Press any key to exit the program. 36 | 37 | ## Features 38 | 39 | This sample is split into 6 scripts, each of them shows a specific feature of the ZED SDK in Matlab. 40 | 41 | 1. `ZED_DepthSensing.m` : 42 | 43 | In this part, we first initialize the ZED Camera and then retrieve both stereo images (Left and Right) as well as the depth data (32-bits float buffer) and the depth image normalized to 8-bits (grayscale). Then, with Matlab, we compute the depth histogram from the depth data values and display it. 44 | 45 | Drawing 46 | 47 | 2. `ZED_Tracking.m` : 48 | 49 | This part shows how to get the positional tracking information. First, we initialize the ZED camera and the tracking, then we display the current left image as well as all the positions of the camera. 50 | 51 | Drawing 52 | 53 | 3. `ZED_PointCloud.m` : 54 | 55 | This part shows how to initialize the ZED camera and retrieve for each frame the current point cloud (X,Y,Z) of the scene. Then we use the mesh function of Matlab to display it. 56 | 57 | Drawing 58 | 59 | 4. `ZED_Recording.m` : 60 | 61 | This script shows how to record a video sequence into 'svo' format which can be playback with the ZED SDK. 62 | 63 | 5. `ZED_ObjectDetection.m` : 64 | 65 | This script use the ability of the ZED2 to detect objects, a video window shows the results. 66 | 67 | 6. `ZED_CameraControl.m` : 68 | 69 | This script shows how to control the video settings of the ZED camera. 70 | 71 | ### Limitations 72 | This sample application is not designed to run in real time 73 | 74 | 75 | ### Troubleshooting 76 | On Linux, If you get the following error : 77 | 78 | /usr/local/MATLAB/R2014a/bin/glnxa64/../../sys/os/glnxa64/libstdc++.so.6: version `GLIBCXX_3.4.21` not found 79 | 80 | Launch matlab with `LD_PRELOAD`: 81 | 82 | LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.21:/usr/lib/x86_64-linux-gnu/libfreetype.so.6 matlab& 83 | 84 | You can also create an alias in a bash terminal (just once) to simplify the launch: 85 | 86 | echo "alias matlab=LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.21:/usr/lib/x86_64-linux-gnu/libfreetype.so.6 matlab&" >> ~/.bashrc 87 | 88 | Then just launch the application by typing: 89 | 90 | matlab 91 | 92 | ## Support 93 | If you need assistance go to our Community site at https://community.stereolabs.com/ 94 | -------------------------------------------------------------------------------- /ZED_Camera.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stereolabs/zed-matlab/5c94f9e120ddd6eb2869577c3b04fb4b36e74a57/ZED_Camera.png -------------------------------------------------------------------------------- /ZED_PointCloud.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stereolabs/zed-matlab/5c94f9e120ddd6eb2869577c3b04fb4b36e74a57/ZED_PointCloud.png -------------------------------------------------------------------------------- /ZED_Tracking.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stereolabs/zed-matlab/5c94f9e120ddd6eb2869577c3b04fb4b36e74a57/ZED_Tracking.png -------------------------------------------------------------------------------- /matlab/ZED_BodyTracking.m: -------------------------------------------------------------------------------- 1 | clc; 2 | disp('========= ZED SDK PLUGIN ========='); 3 | disp('-- Body Tracking --'); 4 | close all; 5 | clear mex; clear functions; clear all; 6 | 7 | 8 | % initial parameter structure, the same as sl::InitParameters 9 | % values as enum number, defines in : sl/defines.hpp 10 | % or from https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html 11 | 12 | InitParameters.camera_resolution = 6; %AUTO 13 | result = mexZED('open', InitParameters); 14 | 15 | clrs = jet(6); 16 | 17 | if(strcmp(result,'SUCCESS')) % the Camera is open 18 | % basic informations 19 | camInfo = mexZED('getCameraInformation'); 20 | if(camInfo.model ==2) % Need a ZED2 21 | 22 | mexZED('enablePositionalTracking'); 23 | 24 | BodyTrackingParameters.detection_model = 0; % HUMAN_BODY_FAST 25 | BodyTrackingParameters.body_format = 2; % BODY_38 26 | BodyTrackingParameters.enable_tracking = 1; % enable tracking 27 | BodyTrackingParameters.enable_body_fitting = 1; %enable fiting 28 | mexZED('enableBodyTracking', BodyTrackingParameters); 29 | BodyTrackingRuntimeParameters.detection_confidence_threshold = 50; 30 | BodyTrackingRuntimeParameters.minimum_keypoints_threshold = 5; 31 | 32 | % Create Figure and wait for keyboard interruption to quit 33 | f = figure('name','ZED SDK: Detection','NumberTitle','off', 'keypressfcn',@(obj,evt) 0); 34 | 35 | display_size = [720 404]; 36 | ratio = [display_size(1) / camInfo.left_cam.width display_size(2) / camInfo.left_cam.height]; 37 | 38 | key = 1; 39 | % loop over frames, till Esc is pressed 40 | while (key ~= 27) 41 | % grab the current image and compute the depth 42 | result = mexZED('grab'); 43 | if(strcmp(result,'SUCCESS')) 44 | % retrieve letf image 45 | image_left = mexZED('retrieveImage', 0, display_size(1), display_size(2)); %left 46 | 47 | bodies = mexZED('retrieveBodies', BodyTrackingRuntimeParameters); 48 | 49 | % display 50 | imshow(image_left); 51 | 52 | for b = 1: length(bodies.body_list) 53 | 54 | clr = clrs(mod(max(0, bodies.body_list(b).id), 6)+1, :); 55 | 56 | keypoints2d = bodies.body_list(b).keypoint_2d; 57 | for kp = 1: length(keypoints2d) 58 | kp2d = keypoints2d(kp); 59 | if(kp2d.u >= 0) % discard undetected keypoints 60 | rectangle('Position', [(kp2d.u * ratio(1) - 2), (kp2d.v * ratio(2) - 2), 4, 4]... 61 | ,'Curvature',[1,1], 'LineWidth',3, 'EdgeColor', clr); 62 | end 63 | end 64 | 65 | bb2d = bodies.body_list(b).bounding_box_2d; 66 | rectangle('Position', [bb2d(1).u * ratio(1), bb2d(1).v * ratio(2), (bb2d(3).u - bb2d(1).u) * ratio(1), (bb2d(3).v - bb2d(1).v) * ratio(2)]... 67 | ,'Curvature',0.05, 'LineWidth',3, 'EdgeColor', clr); 68 | 69 | ['Id: ' num2str(bodies.body_list(b).id) ' Conf: ', num2str(bodies.body_list(b).confidence)] 70 | end 71 | 72 | drawnow; %this checks for interrupts 73 | key = uint8(get(f,'CurrentCharacter')); 74 | if(~length(key)) 75 | key=0; 76 | end 77 | end 78 | end 79 | close(f) 80 | end 81 | mexZED('close') 82 | disp('========= END ========='); 83 | clear mex; 84 | end 85 | -------------------------------------------------------------------------------- /matlab/ZED_Camera.m: -------------------------------------------------------------------------------- 1 | clc; 2 | disp('========= ZED SDK PLUGIN ========='); 3 | disp('-- Retrieve Images and Depth --'); 4 | close all; 5 | clear mex; clear functions; clear all; 6 | 7 | % initial parameter structure, the same as sl::InitParameters 8 | % values as enum number, defines in : sl/defines.hpp 9 | % or from https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html 10 | 11 | InitParameters.camera_resolution = 6; %AUTO 12 | InitParameters.camera_fps = 60; 13 | InitParameters.coordinate_units = 2; %METER 14 | InitParameters.depth_mode = 3; %ULTRA 15 | %InitParameters.svo_input_filename = '../mySVOfile.svo'; % Enable SVO playback 16 | InitParameters.depth_maximum_distance = 7;% Define maximum depth (in METER) 17 | result = mexZED('open', InitParameters); 18 | 19 | if(strcmp(result,'SUCCESS')) % the Camera is open 20 | 21 | % basic informations 22 | camInfo = mexZED('getCameraInformation'); 23 | image_size = [camInfo.left_cam.width camInfo.left_cam.height] 24 | 25 | requested_depth_size = [720 404]; 26 | 27 | % init depth histogram 28 | binranges = 0.5:0.25:InitParameters.depth_maximum_distance; 29 | 30 | % (optional) Get number of frames (if SVO) 31 | nbFrame = mexZED('getSVONumberOfFrames'); 32 | 33 | % Create Figure and wait for keyboard interruption to quit 34 | f = figure('name','ZED SDK : Images and Depth','NumberTitle','off','keypressfcn',@(obj,evt) 0); 35 | 36 | % Setup runtime parameters 37 | RuntimeParameters.sensing_mode = 0; % STANDARD sensing mode 38 | 39 | key = 1; 40 | % loop over frames, till Esc is pressed 41 | while (key ~= 27) 42 | % grab the current image and compute the depth 43 | result = mexZED('grab', RuntimeParameters); 44 | if(strcmp(result,'SUCCESS')) 45 | % retrieve letf image 46 | image_left = mexZED('retrieveImage', 2); %left 47 | % retrieve right image 48 | image_right = mexZED('retrieveImage', 1); %right 49 | 50 | % image timestamp 51 | im_ts = mexZED('getTimestamp', 0) 52 | 53 | % retrieve depth as a normalized image 54 | image_depth = mexZED('retrieveImage', 9); %depth 55 | % retrieve the real depth, resized 56 | depth = mexZED('retrieveMeasure', 1, requested_depth_size(1), requested_depth_size(2)); %depth 57 | 58 | % display 59 | subplot(2,2,1) 60 | imshow(image_left); 61 | title('Image Left') 62 | subplot(2,2,2) 63 | imshow(image_right); 64 | title('Image Right') 65 | subplot(2,2,3) 66 | imshow(image_depth); 67 | title('Depth') 68 | subplot(2,2,4) 69 | % Compute the depth histogram 70 | val_ = find(isfinite(depth(:))); % handle wrong depth values 71 | depth_v = depth(val_); 72 | [bincounts] = histc(depth_v(:),binranges); 73 | bar(binranges,bincounts,'histc') 74 | title('Depth histogram') 75 | xlabel('meters') 76 | 77 | drawnow; %this checks for interrupts 78 | key = uint8(get(f,'CurrentCharacter')); 79 | if(~length(key)) 80 | key=0; 81 | end 82 | end 83 | end 84 | close(f) 85 | end 86 | 87 | % Make sure to call this function to free the memory before use this again 88 | mexZED('close') 89 | disp('========= END ========='); 90 | clear mex; -------------------------------------------------------------------------------- /matlab/ZED_CameraControl.m: -------------------------------------------------------------------------------- 1 | clc; 2 | disp('========= ZED SDK PLUGIN ========='); 3 | disp('-- Retrieve Images and change Video Settings --'); 4 | close all; 5 | clear mex; clear functions; clear all; 6 | 7 | % initial parameter structure, the same as sl::InitParameters 8 | % values as enum number, defines in : sl/defines.hpp 9 | % or from https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html 10 | 11 | InitParameters.camera_resolution = 6; %AUTO 12 | InitParameters.camera_fps = 60; 13 | InitParameters.depth_mode = 0; %NONE % no depth computation needed here 14 | result = mexZED('open', InitParameters); 15 | 16 | if(strcmp(result,'SUCCESS')) % the Camera is open 17 | 18 | % basic informations 19 | camInfo = mexZED('getCameraInformation'); 20 | image_size = [camInfo.left_cam.width camInfo.left_cam.height] 21 | 22 | roi = [0,0,image_size(1), image_size(2)]; 23 | 24 | % Create Figure and wait for keyboard interruption to quit 25 | f = figure('name','ZED SDK : Images and Depth','NumberTitle','off','KeyPressFcn',@(obj,evt) 0); 26 | 27 | key = 1; 28 | % loop over frames, till Esc is pressed 29 | while (key ~= 27) 30 | % grab the current image and compute the depth 31 | result = mexZED('grab'); 32 | if(strcmp(result,'SUCCESS')) 33 | % retrieve letf image 34 | image_left = mexZED('retrieveImage', 0); %left 35 | % retrieve right image 36 | image_right = mexZED('retrieveImage', 1); %right 37 | 38 | % image timestamp 39 | im_ts = mexZED('getTimestamp', 0); 40 | 41 | % display 42 | subplot(1,2,1) 43 | imshow(image_left); 44 | if(roi(3) < image_size(1)) % display Gain/Exposure ROI if neeeded 45 | rectangle('Position',roi,'EdgeColor', 'r','LineWidth', 1,'LineStyle','-'); 46 | end 47 | title('Image Left') 48 | subplot(1,2,2) 49 | imshow(image_right); 50 | if(roi(3) < image_size(1)) % display Gain/Exposure ROI if neeeded 51 | rectangle('Position',roi,'EdgeColor', 'r','LineWidth', 1,'LineStyle','-'); 52 | end 53 | title('Image Right') 54 | 55 | % redraw figure 56 | drawnow; 57 | % check for interrupts 58 | key = uint8(get(f,'CurrentCharacter')); 59 | if(~length(key)) 60 | key=0; 61 | else 62 | ask_plus = key == 45; % press '+' 63 | ask_minus = key == 43; % press '-' 64 | if(ask_plus | ask_minus) 65 | % get current Camera brightness value 66 | brightness = mexZED('getCameraSettings','brightness'); 67 | if(ask_plus & (brightness>0)) % decrease value 68 | brightness = brightness - 1; 69 | end 70 | if(ask_minus & (brightness<8)) % increase value 71 | brightness = brightness + 1; 72 | end 73 | brightness 74 | % set the new value 75 | mexZED('setCameraSettings', 'brightness', brightness); 76 | end 77 | 78 | if(key == 100) % press 'd' to reset to the default value 79 | disp('reset to default'); 80 | mexZED('setCameraSettings', 'brightness', -1); % set auto value 81 | roi = [0,0,image_size(1), image_size(2)]; 82 | mexZED('setCameraSettings', 'aec_agc_roi', roi, 2, 1); % set auto Gain/Exposure on full image 83 | end 84 | 85 | if(key == 114) % press 'r' to use the auto Gain/Exposure on a defineded ROI 86 | roi = [350, 250, 250, 125]; 87 | mexZED('setCameraSettings', 'aec_agc_roi', roi); 88 | end 89 | end 90 | set(f,'CurrentCharacter','0'); % reset pressed key 91 | end 92 | end 93 | end 94 | 95 | close(f) 96 | % Make sure to call this function to free the memory before use this again 97 | mexZED('close') 98 | disp('========= END ========='); 99 | clear mex; 100 | -------------------------------------------------------------------------------- /matlab/ZED_DepthSensing.m: -------------------------------------------------------------------------------- 1 | clc; 2 | disp('========= ZED SDK PLUGIN ========='); 3 | disp('-- Retrieve Images and Depth and compute Depth Histogram --'); 4 | close all; 5 | clear mex; clear functions; clear all; 6 | 7 | % initial parameter structure, the same as sl::InitParameters 8 | % values as enum number, defines qin : sl/defines.hpp 9 | % or from https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html 10 | 11 | InitParameters.camera_resolution = 6; %AUTO 12 | InitParameters.coordinate_units = 2; %METER 13 | InitParameters.depth_mode = 3; %ULTRA 14 | %InitParameters.svo_input_filename = '../mySVOfile.svo'; % Enable SVO playback 15 | InitParameters.depth_minimum_distance = 0.7;% Define maximum depth (in METER) 16 | InitParameters.depth_maximum_distance = 7;% Define maximum depth (in METER) 17 | result = mexZED('open', InitParameters); 18 | 19 | if(strcmp(result,'SUCCESS')) % the Camera is open 20 | requested_depth_size = [720 404]; 21 | 22 | % init depth histogram 23 | binranges = 0.5:0.25:InitParameters.depth_maximum_distance; 24 | 25 | % Create Figure and wait for keyboard interruption to quit 26 | f = figure('name','ZED SDK : Images and Depth','NumberTitle','off','KeyPressFcn',@(obj,evt) 0); 27 | % Setup runtime parameters 28 | RuntimeParameters.sensing_mode = 0; % STANDARD sensing mode 29 | 30 | key = 1; 31 | % loop over frames, till Esc is pressed 32 | while (key ~= 27) 33 | % grab the current image and compute the depth 34 | result = mexZED('grab', RuntimeParameters); 35 | if(strcmp(result,'SUCCESS')) 36 | % retrieve letf image 37 | image_left = mexZED('retrieveImage', 0); %left 38 | 39 | % retrieve depth as a normalized image 40 | image_depth = mexZED('retrieveImage', 9); %depth 41 | % retrieve the real depth, resized 42 | depth = mexZED('retrieveMeasure', 1, requested_depth_size(1), requested_depth_size(2)); %depth 43 | 44 | % display 45 | subplot(2,2,1) 46 | imshow(image_left); 47 | title('Image Left') 48 | subplot(2,2,2) 49 | imshow(image_depth); 50 | title('Depth') 51 | subplot(2,2,3:4) 52 | % Compute the depth histogram 53 | val_ = find(isfinite(depth(:))); % handle wrong depth values 54 | depth_v = depth(val_); 55 | [bincounts] = histc(depth_v(:),binranges); 56 | bar(binranges,bincounts,'histc') 57 | title('Depth histogram') 58 | xlabel('meters') 59 | 60 | % redraw figure 61 | drawnow; 62 | % check for interrupts 63 | key = uint8(get(f,'CurrentCharacter')); 64 | if(~length(key)) 65 | key=0; 66 | end 67 | end 68 | end 69 | close(f) 70 | end 71 | 72 | % Make sure to call this function to free the memory before use this again 73 | mexZED('close') 74 | disp('========= END ========='); 75 | clear mex; 76 | -------------------------------------------------------------------------------- /matlab/ZED_Mapping.m: -------------------------------------------------------------------------------- 1 | clc; 2 | disp('========= ZED SDK PLUGIN ========='); 3 | disp('-- ZED Camera Spatial Mapping --'); 4 | close all; 5 | clear mex; clear functions; clear all; 6 | 7 | % initial parameter structure, the same as sl::InitParameters 8 | % values as enum number, defines in : sl/defines.hpp 9 | % or from https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html 10 | 11 | InitParameters.camera_resolution = 6; %AUTO 12 | InitParameters.camera_fps = 60; 13 | InitParameters.coordinate_units = 2; %METER 14 | InitParameters.depth_mode = 1; %PERFORMANCE 15 | InitParameters.coordinate_system = 3; %COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP 16 | %InitParameters.svo_input_filename = '../mySVOfile.svo'; % Enable SVO playback 17 | result = mexZED('open', InitParameters); 18 | 19 | if(strcmp(result,'SUCCESS')) 20 | 21 | %enable Tracking 22 | PositionalTrackingParameters.enable_spatial_memory = 1; 23 | mexZED('enablePositionalTracking', PositionalTrackingParameters); 24 | 25 | %enable Spatial Mapping 26 | SpatialMappingParameters.map_type = 0; 27 | SpatialMappingParameters.range_meter = 5.; 28 | SpatialMappingParameters.resolution_meter = 0.08; 29 | mexZED('enableSpatialMapping', SpatialMappingParameters); 30 | 31 | f = figure('name','ZED SDK : Spatial Mapping','NumberTitle','off','keypressfcn',@(obj,evt) 0); 32 | 33 | key = 1; 34 | % loop over frames, till Esc is pressed 35 | while (key ~= 27) 36 | % grab the current image and compute the positional tracking 37 | result = mexZED('grab'); 38 | if(strcmp(result,'SUCCESS')) 39 | % retrieve letf image 40 | image_left = mexZED('retrieveImage', 0); %left 41 | %displays it 42 | imshow(image_left); 43 | 44 | drawnow; %this checks for interrupts 45 | key = uint8(get(f,'CurrentCharacter')); 46 | if(~length(key)) 47 | key=0; 48 | end 49 | end 50 | end 51 | 52 | if(SpatialMappingParameters.map_type == 0) % Mesh 53 | [vertices, faces] = mexZED('extractWholeSpatialMap'); 54 | else % Fused Point Cloud 55 | [vertices, colors] = mexZED('extractWholeSpatialMap'); 56 | end 57 | close(f) 58 | end 59 | 60 | % Make sure to call this function to free the memory before use this again 61 | mexZED('close') 62 | disp('========= END ========='); 63 | clear mex; -------------------------------------------------------------------------------- /matlab/ZED_ObjectDetection.m: -------------------------------------------------------------------------------- 1 | clc; 2 | disp('========= ZED SDK PLUGIN ========='); 3 | disp('-- Object Detection --'); 4 | close all; 5 | clear mex; clear functions; clear all; 6 | 7 | 8 | % initial parameter structure, the same as sl::InitParameters 9 | % values as enum number, defines in : sl/defines.hpp 10 | % or from https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html 11 | 12 | InitParameters.camera_resolution = 6; %AUTO 13 | result = mexZED('open', InitParameters); 14 | 15 | clrs = jet(6); 16 | 17 | if(strcmp(result,'SUCCESS')) % the Camera is open 18 | % basic informations 19 | camInfo = mexZED('getCameraInformation'); 20 | if(camInfo.model ==2) % Need a ZED2 21 | 22 | mexZED('enablePositionalTracking'); 23 | 24 | ObjectDetectionParameters.detection_model = 0; % MULTI_CLASS_BOX_FAST 25 | ObjectDetectionParameters.enable_tracking = 1; % enable Object tracking 26 | mexZED('enableObjectDetection', ObjectDetectionParameters); 27 | ObjectDetectionRuntimeParameters.detection_confidence_threshold = 35 28 | 29 | % Create Figure and wait for keyboard interruption to quit 30 | f = figure('name','ZED SDK: Detection','NumberTitle','off', 'keypressfcn',@(obj,evt) 0); 31 | 32 | display_size = [720 404]; 33 | ratio = [display_size(1) / camInfo.left_cam.width display_size(2) / camInfo.left_cam.height]; 34 | 35 | key = 1; 36 | % loop over frames, till Esc is pressed 37 | while (key ~= 27) 38 | % grab the current image and compute the depth 39 | result = mexZED('grab'); 40 | if(strcmp(result,'SUCCESS')) 41 | % retrieve letf image 42 | image_left = mexZED('retrieveImage', 0, display_size(1), display_size(2)); %left 43 | 44 | objs = mexZED('retrieveObjects', ObjectDetectionRuntimeParameters); 45 | 46 | % display 47 | imshow(image_left); 48 | 49 | for o = 1: length(objs.object_list) 50 | bb2d = objs.object_list(o).bounding_box_2d; 51 | clr = clrs(mod(max(0, objs.object_list(o).id), 6)+1, :); 52 | rectangle('Position', [bb2d(1).u * ratio(1), bb2d(1).v * ratio(2), (bb2d(3).u - bb2d(1).u) * ratio(1), (bb2d(3).v - bb2d(1).v) * ratio(2)]... 53 | ,'Curvature',0.05, 'LineWidth',3, 'EdgeColor', clr); 54 | 55 | ['Id: ' num2str(objs.object_list(o).id) ' Conf: ', num2str(objs.object_list(o).confidence)] 56 | end 57 | 58 | drawnow; %this checks for interrupts 59 | key = uint8(get(f,'CurrentCharacter')); 60 | if(~length(key)) 61 | key=0; 62 | end 63 | end 64 | end 65 | close(f) 66 | end 67 | mexZED('close') 68 | disp('========= END ========='); 69 | clear mex; 70 | end 71 | -------------------------------------------------------------------------------- /matlab/ZED_PointCloud.m: -------------------------------------------------------------------------------- 1 | clc; 2 | disp('========= ZED SDK PLUGIN ========='); 3 | disp('-- Get 3D Point Cloud --'); 4 | close all; 5 | clear mex; clear functions; clear all; 6 | 7 | % initial parameter structure, the same as sl::InitParameters 8 | % values as enum number, defines in : sl/defines.hpp 9 | % or from https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html 10 | 11 | InitParameters.camera_resolution = 6; %AUTO 12 | InitParameters.coordinate_units = 2; %METER 13 | InitParameters.depth_mode = 1; %PERFORMANCE 14 | InitParameters.coordinate_system = 3; %COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP 15 | %InitParameters.svo_input_filename = '../mySVOfile.svo'; % Enable SVO playback 16 | 17 | % DepthClamp value, maximum depth (in METER) 18 | depth_max = 5; 19 | InitParameters.depth_maximum_distance = depth_max; 20 | result = mexZED('open', InitParameters); 21 | 22 | if(strcmp(result,'SUCCESS')) 23 | % Create Figure 24 | f = figure('name','ZED SDK : Point Cloud','NumberTitle','off','keypressfcn',@(obj,evt) 0); 25 | %create 2 sub figure 26 | ha1 = axes('Position',[0.05,0.7,0.9,0.25]); 27 | ha2 = axes('Position',[0.05,0.05,0.9,0.6]); 28 | axis([-depth_max, depth_max, 0, depth_max, -depth_max ,depth_max]) 29 | xlabel('X'); 30 | ylabel('Z'); 31 | zlabel('Y'); 32 | grid on; 33 | hold on; 34 | 35 | % init point cloud data 36 | requested_size = [128 72]; 37 | nb_elem = requested_size(1) * requested_size(2); 38 | pt_X = zeros(requested_size); 39 | pt_Y = zeros(requested_size); 40 | pt_Z = zeros(requested_size); 41 | 42 | % Setup runtime parameters 43 | RuntimeParameters.sensing_mode = 0; % STANDARD sensing mode 44 | 45 | h = plot3(reshape(pt_X, 1,nb_elem), reshape( pt_Y, 1,nb_elem), reshape( pt_Z, 1,nb_elem), '.'); 46 | 47 | key = 1; 48 | % loop over frames, till Esc is pressed 49 | while (key ~= 27) 50 | % grab the current image and compute the depth 51 | result = mexZED('grab', RuntimeParameters); 52 | if(strcmp(result,'SUCCESS')) 53 | % retrieve letf image 54 | image_left = mexZED('retrieveImage', 0); %left 55 | %displays it 56 | axes(ha1); 57 | imshow(image_left); 58 | 59 | % retrieve the point cloud, resized 60 | [pt_X, pt_Y, pt_Z] = mexZED('retrieveMeasure', 3, requested_size(1), requested_size(2)); %XYZ pointcloud 61 | 62 | %displays it 63 | axes(ha2); 64 | set(h,'XData',reshape(pt_X, 1,nb_elem)) 65 | set(h,'YData',reshape(pt_Y, 1,nb_elem)) 66 | set(h,'ZData',reshape(pt_Z, 1,nb_elem)) 67 | 68 | drawnow; %this checks for interrupts 69 | key = uint8(get(f,'CurrentCharacter')); 70 | if(~length(key)) 71 | key=0; 72 | end 73 | end 74 | end 75 | close(f) 76 | end 77 | 78 | % Make sure to call this function to free the memory before use this again 79 | mexZED('close') 80 | disp('========= END ========='); 81 | clear mex; -------------------------------------------------------------------------------- /matlab/ZED_Recording.m: -------------------------------------------------------------------------------- 1 | clc; 2 | disp('========= ZED SDK PLUGIN ========='); 3 | disp('-- Record SVO File --'); 4 | close all; 5 | clear mex; clear functions; clear all; 6 | 7 | % initial parameter structure, the same as sl::InitParameters 8 | % values as enum number, defines in : sl/defines.hpp 9 | % or from https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html 10 | 11 | InitParameters.camera_resolution = 6; %AUTO 12 | InitParameters.camera_fps = 60; 13 | InitParameters.system_units = 2; %METER 14 | InitParameters.depth_mode = 0; %DEPTH_MODE_NONE 15 | result = mexZED('open', InitParameters); 16 | 17 | nb_frame_to_save = 1000; 18 | if(strcmp(result,'SUCCESS')) 19 | RecordingParameters.video_filename = 'MySVO.svo'; 20 | RecordingParameters.compression_mode = 2; 21 | result = mexZED('enableRecording', RecordingParameters); 22 | if(strcmp(result,'SUCCESS')) 23 | disp('Start Recording'); 24 | f = 0; 25 | while f < nb_frame_to_save 26 | % grab the current image 27 | result = mexZED('grab'); 28 | if(strcmp(result,'SUCCESS')) 29 | % record the grabbed image 30 | f = f+1; 31 | end 32 | end 33 | disp('End of Recording'); 34 | end 35 | mexZED('disableRecording'); 36 | end 37 | mexZED('close') 38 | disp('========= END ========='); 39 | clear mex; -------------------------------------------------------------------------------- /matlab/ZED_Sensors.m: -------------------------------------------------------------------------------- 1 | clc; 2 | disp('========= ZED SDK PLUGIN ========='); 3 | disp('-- Sensors --'); 4 | close all; 5 | clear mex; clear functions; clear all; 6 | 7 | % initial parameter structure, the same as sl::InitParameters 8 | % values as enum number, defines in : sl/defines.hpp 9 | % or from https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html 10 | 11 | InitParameters.camera_resolution = 6; %AUTO 12 | InitParameters.depth_mode = 0; % No depth required 13 | result = mexZED('open', InitParameters); 14 | 15 | if(strcmp(result,'SUCCESS')) % the Camera is open 16 | % basic informations 17 | camInfo = mexZED('getCameraInformation'); 18 | if(camInfo.model ~= 0) % ZED has no other sensors 19 | 20 | % Create Figure and wait for keyboard interruption to quit 21 | f = figure('name','ZED SDK: Sensors','NumberTitle','off', 'keypressfcn',@(obj,evt) 0); 22 | ha1 = axes('Position',[0.1,0.6,0.89,0.35]); 23 | hold on; 24 | title('Linear Acceleration') 25 | xlabel('frame') 26 | ylabel('Acceleration') 27 | ha2 = axes('Position',[0.1,0.05,0.89,0.35]); 28 | hold on; 29 | title('Angular Velocity') 30 | xlabel('frame') 31 | ylabel('Velocity') 32 | sample =0; 33 | key = 1; 34 | % loop over frames, till Esc is pressed 35 | while (key ~= 27) 36 | 37 | sensors_data = mexZED('getSensorsData', 1); % ask CURRENT sensors data 38 | 39 | % Baro 40 | if(sensors_data.BarometerData.is_available) 41 | baro_pressure = sensors_data.BarometerData.pressure 42 | baro_rate = sensors_data.BarometerData.effective_rate 43 | end 44 | 45 | % IMU 46 | if(sensors_data.IMUData.is_available) 47 | imu_pose = sensors_data.IMUData.pose 48 | imu_angular_v = sensors_data.IMUData.angular_velocity 49 | imu_linear_a = sensors_data.IMUData.linear_acceleration 50 | imu_rate = sensors_data.IMUData.effective_rate 51 | % Display 52 | axes(ha1); 53 | plot(sample, imu_linear_a(1), 'r+') 54 | plot(sample, imu_linear_a(2), 'b+') 55 | plot(sample, imu_linear_a(3), 'c+') 56 | axes(ha2); 57 | plot(sample, imu_angular_v(1), 'r+') 58 | plot(sample, imu_angular_v(2), 'b+') 59 | plot(sample, imu_angular_v(3), 'c+') 60 | end 61 | 62 | % Magnetometer 63 | if(sensors_data.MagnetometerData.is_available) 64 | mag_field_uncalib = sensors_data.MagnetometerData.magnetic_field_uncalibrated 65 | mag_field = sensors_data.MagnetometerData.magnetic_field_calibrated 66 | mag_rate = sensors_data.MagnetometerData.effective_rate 67 | end 68 | 69 | % Temperature 70 | temp_imu = sensors_data.TemperatureData.IMU 71 | temp_baro = sensors_data.TemperatureData.BAROMETER 72 | temp_left = sensors_data.TemperatureData.ONBOARD_LEFT 73 | temp_right = sensors_data.TemperatureData.ONBOARD_RIGHT 74 | 75 | sample = sample+1; 76 | drawnow; %this checks for interrupts 77 | key = uint8(get(f,'CurrentCharacter')); 78 | if(~length(key)) 79 | key=0; 80 | end 81 | end 82 | close(f) 83 | end 84 | mexZED('close') 85 | disp('========= END ========='); 86 | clear mex; 87 | end 88 | -------------------------------------------------------------------------------- /matlab/ZED_Tracking.m: -------------------------------------------------------------------------------- 1 | clc; 2 | disp('========= ZED SDK PLUGIN ========='); 3 | disp('-- Get ZED Camera Position --'); 4 | close all; 5 | clear mex; clear functions; clear all; 6 | 7 | % initial parameter structure, the same as sl::InitParameters 8 | % values as enum number, defines in : sl/defines.hpp 9 | % or from https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html 10 | 11 | InitParameters.camera_resolution = 6; %AUTO 12 | InitParameters.camera_fps = 60; 13 | InitParameters.coordinate_units = 2; %METER 14 | InitParameters.depth_mode = 1; %PERFORMANCE 15 | InitParameters.coordinate_system = 3; %COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP 16 | %InitParameters.svo_input_filename = '../mySVOfile.svo'; % Enable SVO playback 17 | result = mexZED('open', InitParameters); 18 | 19 | if(strcmp(result,'SUCCESS')) 20 | 21 | %enable Tracking 22 | PositionalTrackingParameters.enable_spatial_memory = 1; 23 | %TrackingParameters.initial_world_transform = [1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1]; 24 | mexZED('enablePositionalTracking', PositionalTrackingParameters); 25 | 26 | % for tracking informations storage 27 | PositionArray = []; 28 | 29 | % Create Figure and wait for keyboard interruption to quit 30 | f = figure('name','ZED SDK : Positional Tracking','NumberTitle','off','keypressfcn',@(obj,evt) 0); 31 | %create 2 sub figure 32 | ha1 = axes('Position',[0.05,0.7,0.9,0.25]); 33 | ha2 = axes('Position',[0.05,0.05,0.9,0.6]); 34 | xlabel('Tx (M)'); 35 | ylabel('Tz (M)'); 36 | zlabel('Ty (M)'); 37 | xlim(ha2, [-2 2]); 38 | ylim(ha2, [-2 2]); 39 | axis equal, grid on; 40 | hold on; 41 | % init 3d display 42 | h = plot3(0,0,0, 'r'); 43 | 44 | key = 1; 45 | % loop over frames, till Esc is pressed 46 | while (key ~= 27) 47 | % grab the current image and compute the positional tracking 48 | result = mexZED('grab'); 49 | if(strcmp(result,'SUCCESS')) 50 | % retrieve letf image 51 | image_left = mexZED('retrieveImage', 0); %left 52 | %displays it 53 | axes(ha1); 54 | imshow(image_left); 55 | 56 | % retrieve camera Path 57 | [position, state] = mexZED('getPosition'); 58 | %stack positions 59 | PositionArray = [PositionArray; position(1,4) position(2,4) position(3,4)]; 60 | 61 | axes(ha2); 62 | set(h,'XData',PositionArray(:,1)) 63 | set(h,'YData',PositionArray(:,2)) 64 | set(h,'ZData',PositionArray(:,3)) 65 | 66 | drawnow; %this checks for interrupts 67 | key = uint8(get(f,'CurrentCharacter')); 68 | if(~length(key)) 69 | key=0; 70 | end 71 | end 72 | end 73 | mexZED('disablePositionalTracking'); 74 | close(f) 75 | end 76 | 77 | % Make sure to call this function to free the memory before use this again 78 | mexZED('close') 79 | disp('========= END ========='); 80 | clear mex; -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(MEX) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | 7 | IF(WIN32 AND NOT (${CMAKE_SIZEOF_VOID_P} EQUAL 8)) 8 | message(FATAL_ERROR "You've selected the 32bit version of ${CMAKE_GENERATOR}. \n Please delete the cache (file->Delete Cache) and use the 64bit version. (${CMAKE_GENERATOR} Win64)") 9 | ENDIF() 10 | 11 | SET(MATLAB_ROOT $ENV{MATLAB_ROOT} CACHE FILEPATH "directory") 12 | 13 | set(CMAKE_INSTALL_PREFIX ${CMAKE_SOURCE_DIR}) 14 | set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake) 15 | 16 | add_definitions(/DMATLAB_MEX_FILE) 17 | add_definitions(/DMX_COMPAT_32) 18 | 19 | find_package(Matlab REQUIRED) 20 | 21 | IF(MATLAB_FOUND) 22 | message(STATUS "MATLAB Found, MATLAB MEX will be compiled.") 23 | 24 | find_package(ZED 3 REQUIRED) 25 | find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) 26 | find_package(OpenCV REQUIRED) 27 | 28 | include_directories(include) 29 | 30 | add_subdirectory(mex) 31 | ELSE(MATLAB_FOUND) 32 | message( FATAL_ERROR "MATLAB not found...nothing will be built." ) 33 | ENDIF(MATLAB_FOUND) 34 | -------------------------------------------------------------------------------- /src/Matlabdef.def: -------------------------------------------------------------------------------- 1 | EXPORTS mexFunction -------------------------------------------------------------------------------- /src/cmake/FindMatlab.cmake: -------------------------------------------------------------------------------- 1 | # - this module looks for Matlab 2 | # Defines: 3 | # MATLAB_INCLUDE_DIR: include path for mex.h 4 | # MATLAB_LIBRARIES: required libraries: libmex, libmx 5 | # MATLAB_MEX_LIBRARY: path to libmex 6 | # MATLAB_MX_LIBRARY: path to libmx 7 | 8 | SET(MATLAB_FOUND 0) 9 | IF( "${MATLAB_ROOT}" STREQUAL "" ) 10 | MESSAGE(STATUS "MATLAB_ROOT environment variable not set." ) 11 | MESSAGE(STATUS "In Linux this can be done in your user .bashrc file by appending the corresponding line, e.g:" ) 12 | MESSAGE(STATUS "export MATLAB_ROOT=/usr/local/MATLAB/R2014b" ) 13 | MESSAGE(STATUS "In Windows this can be done by adding system variable, e.g:" ) 14 | MESSAGE(STATUS "MATLAB_ROOT=C:/Program Files/MATLAB/R2014b" ) 15 | ELSE("${MATLAB_ROOT}" STREQUAL "" ) 16 | 17 | FIND_PATH(MATLAB_INCLUDE_DIR mex.h 18 | ${MATLAB_ROOT}/extern/include) 19 | 20 | INCLUDE_DIRECTORIES(${MATLAB_INCLUDE_DIR}) 21 | 22 | FIND_LIBRARY( MATLAB_MEX_LIBRARY 23 | NAMES libmex mex 24 | PATHS ${MATLAB_ROOT}/bin ${MATLAB_ROOT}/extern/lib 25 | PATH_SUFFIXES glnxa64 glnx86 win64/microsoft win32/microsoft) 26 | 27 | FIND_LIBRARY( MATLAB_MX_LIBRARY 28 | NAMES libmx mx 29 | PATHS ${MATLAB_ROOT}/bin ${MATLAB_ROOT}/extern/lib 30 | PATH_SUFFIXES glnxa64 glnx86 win64/microsoft win32/microsoft) 31 | 32 | MESSAGE (STATUS "MATLAB_ROOT: ${MATLAB_ROOT}") 33 | 34 | ENDIF("${MATLAB_ROOT}" STREQUAL "" ) 35 | 36 | # This is common to UNIX and Win32: 37 | SET(MATLAB_LIBRARIES 38 | ${MATLAB_MEX_LIBRARY} 39 | ${MATLAB_MX_LIBRARY} 40 | ) 41 | 42 | IF(MATLAB_INCLUDE_DIR AND MATLAB_LIBRARIES) 43 | SET(MATLAB_FOUND 1) 44 | MESSAGE(STATUS "Matlab libraries will be used") 45 | ENDIF(MATLAB_INCLUDE_DIR AND MATLAB_LIBRARIES) 46 | 47 | MARK_AS_ADVANCED( 48 | MATLAB_LIBRARIES 49 | MATLAB_MEX_LIBRARY 50 | MATLAB_MX_LIBRARY 51 | MATLAB_INCLUDE_DIR 52 | MATLAB_FOUND 53 | MATLAB_ROOT 54 | ) 55 | -------------------------------------------------------------------------------- /src/include/iter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * iter.h 3 | * iterator for data conversion 4 | */ 5 | 6 | #ifndef iter_h__ 7 | #define iter_h__ 8 | 9 | #include 10 | 11 | /** 12 | * Image Storage type. 13 | * Generally Maltab is column wise, while C++ row wise 14 | */ 15 | enum eStorType { 16 | eColWise, eRowWise 17 | }; 18 | 19 | /// Pixel iterator for 2D image, i.e. mono-color image 20 | 21 | template 22 | class pix_iterator_2d : public std::iterator { 23 | public: 24 | 25 | /** 26 | * Constructor 27 | * @param beg Beginning of the image 28 | * @param width width of image in pixels 29 | * @param height Height of image in pixels 30 | * @param pitch Width step of aligned image in BYTES 31 | */ 32 | pix_iterator_2d(data_t* beg, int width, int height, int pitch) 33 | : pBeg_(beg), pCurPix_(beg), pCurRowBeg_(beg), 34 | curCol_(0), curRow_(0), width_(width), height_(height) { 35 | size_t s = sizeof (data_t); 36 | if (st == eRowWise) pitch_ = ((unsigned) pitch < width * s) ? (width * s) : (pitch); 37 | else if (st == eColWise) pitch_ = (pitch < height * s) ? (height * s) : (pitch); 38 | #if 0 39 | mexEvalString("pcc = [];"); 40 | mexEvalString("pcr = [];"); 41 | #endif // _DEBUG 42 | } 43 | 44 | /// Constructor 45 | 46 | pix_iterator_2d(data_t* beg, int width, int height) 47 | : pBeg_(beg), pCurPix_(beg), pCurRowBeg_(beg), 48 | curCol_(0), curRow_(0), width_(width), height_(height) { 49 | if (st == eRowWise) pitch_ = width * sizeof (data_t); 50 | else if (st == eColWise) pitch_ = height * sizeof (data_t); 51 | #if 0 52 | mexEvalString("pcc = [];"); 53 | mexEvalString("pcr = [];"); 54 | #endif // _DEBUG 55 | } 56 | 57 | /// reset 58 | 59 | void reset(data_t* beg, int width, int height, int pitch) { 60 | pBeg_ = beg; 61 | pCurPix_ = beg; 62 | pCurRowBeg_ = beg; 63 | curCol_ = 0; 64 | curRow_ = 0; 65 | width_ = width; 66 | height_ = height; 67 | 68 | size_t s = sizeof (data_t); 69 | if (st == eRowWise) pitch_ = (pitch < width * s) ? (width * s) : (pitch); 70 | else if (st == eColWise) pitch_ = (pitch < height * s) ? (height * s) : (pitch); 71 | 72 | } 73 | 74 | /// reset 75 | 76 | void reset(data_t* beg, int width, int height) { 77 | pBeg_ = beg; 78 | pCurPix_ = beg; 79 | pCurRowBeg_ = beg; 80 | curCol_ = 0; 81 | curRow_ = 0; 82 | width_ = width; 83 | height_ = height; 84 | 85 | if (st == eRowWise) pitch_ = width * sizeof (data_t); 86 | else if (st == eColWise) pitch_ = height * sizeof (data_t); 87 | #if 0 88 | mexEvalString("pcc = [];"); 89 | mexEvalString("pcr = [];"); 90 | #endif // _DEBUG 91 | } 92 | 93 | /// Destructor 94 | 95 | ~pix_iterator_2d() { 96 | } 97 | /// operator != 98 | 99 | bool operator!=(const pix_iterator_2d& rhs) { 100 | return pCurPix_ != rhs.pCurPix_; 101 | } 102 | 103 | /// pre-increment 104 | 105 | pix_iterator_2d& operator++() { 106 | if (curCol_ < width_ - 1) { 107 | ++curCol_; 108 | if (st == eRowWise) ++pCurPix_; 109 | else if (st == eColWise) { 110 | char* tmp = (char*) pCurPix_; 111 | tmp += pitch_; 112 | pCurPix_ = (data_t*) tmp; 113 | } 114 | 115 | #if 0 116 | mxArray* pcc = MexUtil::createArrayFromScalar(curCol_); 117 | mexPutVariable("base", "tmp", pcc); 118 | mexEvalString("pcc(end+1) = tmp;"); 119 | #endif 120 | } else { // next row 121 | curCol_ = 0; 122 | ++curRow_; 123 | #if 0 124 | mexEvalString("pcc = 0;"); 125 | 126 | mxArray* pcr = MexUtil::createArrayFromScalar(curRow_); 127 | mexPutVariable("base", "tmp", pcr); 128 | mexEvalString("pcr(end+1) = tmp;"); 129 | #endif // _DEBUG 130 | if (st == eRowWise) { 131 | char* tmp = (char*) pCurRowBeg_; 132 | tmp += pitch_; 133 | pCurRowBeg_ = (data_t*) tmp; 134 | pCurPix_ = pCurRowBeg_; 135 | } else if (st == eColWise) { 136 | ++pCurRowBeg_; 137 | if (curRow_ < height_) pCurPix_ = pCurRowBeg_; 138 | else pCurPix_ = (data_t*) ((char*) pBeg_ + pitch_ * width_); 139 | } 140 | 141 | } 142 | return *this; 143 | } 144 | 145 | /// dereference 146 | 147 | data_t& operator*() { 148 | return *pCurPix_; 149 | } 150 | /// pass-end. STL semantic 151 | 152 | pix_iterator_2d& end() { 153 | curCol_ = 0; 154 | if (st == eRowWise) 155 | pCurRowBeg_ = (data_t*) ((char*) pBeg_ + pitch_ * height_); 156 | else if (st == eColWise) 157 | pCurRowBeg_ = (data_t*) ((char*) pBeg_ + pitch_ * width_); 158 | pCurPix_ = pCurRowBeg_; 159 | return *this; 160 | } 161 | protected: 162 | private: 163 | data_t *pCurPix_, *pCurRowBeg_; // ��ǰ���أ��е�ָ�� 164 | data_t* pBeg_; 165 | int curCol_, curRow_; 166 | int width_, height_; // in pixels 167 | int pitch_; // in bytes 168 | }; 169 | 170 | 171 | 172 | // mxArray_iter_3d 173 | // iterate among pages 174 | // see MATLAB on-line help: 175 | // MATLAB->Programming->Multidemensional Arrays->Overview 176 | // for the details about the concept of "row", "column" and "page" 177 | 178 | template 179 | class mxArray_iter_3d 180 | : public std::iterator { 181 | public: 182 | 183 | mxArray_iter_3d(data_t* start, 184 | unsigned int width, unsigned int height, unsigned int npage) 185 | : cur_page_start_(start), start_(start), elemcount_(0), 186 | width_(width), height_(height), pagesize_(width*height), npage_(npage), 187 | it_(start, width, height) { 188 | } 189 | 190 | // 191 | 192 | mxArray_iter_3d& operator++() { 193 | ++elemcount_; 194 | ++it_; 195 | if (!(elemcount_ < pagesize_)) { // next page 196 | cur_page_start_ += pagesize_; 197 | it_.reset(cur_page_start_, width_, height_); 198 | elemcount_ = 0; 199 | } 200 | return *this; 201 | } 202 | 203 | // 204 | 205 | bool operator==(const mxArray_iter_3d& rhs) { 206 | return it_ == rhs.it_; 207 | } 208 | 209 | // 210 | 211 | bool operator!=(const mxArray_iter_3d& rhs) { 212 | return it_ != rhs.it_; 213 | } 214 | 215 | // 216 | 217 | data_t& operator*() { 218 | return *it_; 219 | } 220 | 221 | // 222 | 223 | mxArray_iter_3d& end() { 224 | cur_page_start_ = start_ + (npage_ - 1) * pagesize_; 225 | it_.reset(cur_page_start_, width_, height_); 226 | it_.end(); 227 | return *this; 228 | } 229 | protected: 230 | private: 231 | const unsigned int width_, height_, pagesize_, npage_; 232 | unsigned int elemcount_; 233 | data_t *cur_page_start_, *start_; 234 | pix_iterator_2d it_; 235 | }; 236 | 237 | /** 238 | * Pixel iterator for 3-channel interleaved image 239 | * Data is stored as b0,g0,r0,...bn,gn,rn and 240 | * fetched as r0,...,rn,g0,...,gn,b0,...,bn 241 | */ 242 | template 243 | class pix_iter_rgb 244 | : public std::iterator { 245 | public: 246 | /// Constructor 247 | 248 | pix_iter_rgb(data_t* beg, int width, int height, int step) 249 | : pBeg_(beg), 250 | w_(0), h_(0), c_(0), width_(width), height_(height) { 251 | this->reset_ptr_channel(); 252 | size_t s = sizeof (data_t); 253 | step_ = (step < width * s) ? (width * s) : (step); 254 | } 255 | /// reset 256 | 257 | void reset(data_t* beg, int width, int height, int step) { 258 | w_ = h_ = c_ = 0; 259 | width_ = width; 260 | height_ = height; 261 | pBeg_ = beg; 262 | this->reset_ptr_channel(); 263 | size_t s = sizeof (data_t); 264 | step_ = (step < width * s) ? (width * s) : (step); 265 | } 266 | /// Destructor 267 | 268 | ~pix_iter_rgb() { 269 | } 270 | 271 | /// operator 1= 272 | 273 | bool operator!=(const pix_iter_rgb& rhs) { 274 | return p_ != rhs.p_; 275 | } 276 | /// pre-increment 277 | 278 | pix_iter_rgb& operator++() { 279 | if (w_ < width_ - 1) { // next pixel 280 | ++w_; 281 | p_ += 3; 282 | } else { 283 | if (h_ < height_ - 1) { // next row 284 | w_ = 0; 285 | ++h_; 286 | this->reset_ptr_next_row(); 287 | } else { 288 | if (c_ < 2) { // next channel 289 | ++c_; 290 | w_ = h_ = 0; 291 | this->reset_ptr_channel(); 292 | } else { // pass-end 293 | this->end(); 294 | } 295 | } 296 | } 297 | return *this; 298 | } 299 | /// dereference 300 | 301 | data_t& operator*() { 302 | return *p_; 303 | } 304 | /// pass-end iterator. STL style. 305 | 306 | pix_iter_rgb& end() { 307 | w_ = 0; 308 | h_ = height_; 309 | p_ = pRow_ = (data_t*) ((char*) pBeg_ + step_ * height_); 310 | return *this; 311 | } 312 | protected: 313 | private: 314 | data_t *p_, *pRow_; 315 | data_t *pBeg_; // image origin 316 | int w_, h_, c_; // current width, height and channel 317 | int width_, height_; // in pixels 318 | int step_; // in bytes 319 | // reset p_ and pRow_ to the origin of current channel 320 | 321 | void reset_ptr_channel() { 322 | pRow_ = pBeg_ + (2 - c_); 323 | p_ = pRow_; 324 | } 325 | // reset p_ and pRow_ to the beginning of next row 326 | 327 | void reset_ptr_next_row() { 328 | // NOTE: step in BYTES!!! 329 | char *tmp = (char*) pRow_; 330 | tmp += step_; 331 | pRow_ = (data_t*) tmp; 332 | p_ = pRow_; 333 | } 334 | }; 335 | #endif // iter_h__ -------------------------------------------------------------------------------- /src/include/mc_convert.h: -------------------------------------------------------------------------------- 1 | /** 2 | * mc_convert.h 3 | * converter between Matlab and C/C++ 4 | * 5 | */ 6 | 7 | 8 | #ifndef mc_convert_h__ 9 | #define mc_convert_h__ 10 | 11 | 12 | 13 | #include "types.h" 14 | #include 15 | #include "mex.h" 16 | 17 | /** 18 | * Convert values in range [beg,end) to a new mxArray. 19 | * The Matlab will free returned mxArray automatically. 20 | * @param beg iterator pointing to the beginning 21 | * @param end iterator pointing to the end (past-end) 22 | * @return pointer to the created mxArray r. r is N-by-1 where N = std::diff(beg,end); 23 | */ 24 | template 25 | mxArray* values_to_new_mxArr(iter_t beg, iter_t end) { 26 | using namespace std; 27 | typedef typename iterator_traits::value_type T; 28 | typedef typename iterator_traits::pointer ptr; 29 | 30 | const int ndims = 1; 31 | mwSize dims[ndims]; 32 | dims[0] = distance(beg, end); 33 | mxClassID id; 34 | id = cm_traits::CID; 35 | 36 | mxArray* p = mxCreateNumericArray(1, dims, id, mxREAL); 37 | ptr pp = (ptr) mxGetData(p); 38 | copy(beg, end, pp); 39 | return p; 40 | } 41 | 42 | /** 43 | * Copy values in mxArray mat(:) to the range beginning with beg. 44 | * The caller should ensure the range is large enough for mat. 45 | * @param mat pointer to the mxArray to be copied 46 | * @param beg iterator pointing to the beginning 47 | */ 48 | template 49 | void mxArr_to_values(const mxArray* mat, iter_t beg) { 50 | using namespace std; 51 | 52 | void* pp = mxGetData(mat); 53 | mxClassID id = mxGetClassID(mat); 54 | mwSize n = mxGetNumberOfElements(mat); 55 | 56 | if (mxDOUBLE_CLASS == id) { 57 | typedef mc_traits::CT T; 58 | T* pbeg = static_cast (pp); 59 | T* pend = pbeg + n; 60 | copy(pbeg, pend, beg); 61 | } 62 | 63 | } 64 | 65 | /** 66 | * Convert scalar value *it to a new mxArray. 67 | * The Matlab will free returned mxArray automatically. 68 | * @param it iterator pointing to the scalar to be converted 69 | * @return pointer to the created mxArray r. r is 1-by-1 70 | */ 71 | template 72 | mxArray* scalar_to_new_mxArr(iter_t it) { 73 | // TODO 74 | return 0; 75 | } 76 | 77 | /** 78 | * Copy the first value in mxArray mat(:) to *it 79 | * @param mat pointer to the mxArray to be copied 80 | * @param it iterator to the copied scalar 81 | */ 82 | template 83 | void mxArr_to_scalar(const mxArray* mat, iter_t it) { 84 | using namespace std; 85 | 86 | void* pp = mxGetData(mat); 87 | mxClassID id = mxGetClassID(mat); 88 | 89 | if (mxDOUBLE_CLASS == id) { 90 | typedef mc_traits::CT T; 91 | T* p = static_cast (pp); 92 | *it = *p; 93 | } 94 | } 95 | 96 | 97 | 98 | #ifdef HAS_OPENCV 99 | 100 | #include 101 | 102 | /** 103 | * Convert values in IplImage *img to a new mxArray. 104 | * The Matlab will free returned mxArray automatically. 105 | * @param img pointer to the IplImage to be converted 106 | * @return pointer to the created mxArray r. r is the same dimensions and data type as *img. 107 | */ 108 | mxArray* IplImage_to_new_mxArr(const IplImage* img); 109 | 110 | /** 111 | * Convert values in mxArray mat(:) to a new IplImage. 112 | * The caller is responsible to free returned pointer to IplImage. 113 | * @param mat pointer to the mxArray to be converted 114 | * @return pointer to the created IplImage r. r is the same dimensions and data type as *mat. 115 | */ 116 | IplImage* mxArr_to_new_IplImage(const mxArray* mat); 117 | 118 | /** 119 | * Convert values in CvMat *mat to a new mxArray. 120 | * The Matlab will free returned mxArray automatically. 121 | * @param mat pointer to the CvMat to be converted 122 | * @return pointer to the created mxArray r. r is the same dimensions and data type as *mat. 123 | */ 124 | mxArray* CvMat_to_new_mxArr(const CvMat* mat); 125 | /** 126 | * Convert values in mxArray mat(:) to a new CvMat. 127 | * The caller is responsible to free returned pointer to CvMat. 128 | * @param mat pointer to the mxArray to be converted 129 | * @return pointer to the created CvMat r. r is the same dimensions and data type as *mat. 130 | */ 131 | CvMat* mxArr_to_new_CvMat(const mxArray* mat); 132 | 133 | #endif // HAS_OPENCV 134 | 135 | 136 | 137 | 138 | 139 | /* 140 | // Add support to other data type conversion here... 141 | #ifdef HAS_SOME_LIBARARY 142 | 143 | #include "lib_header.h" 144 | 145 | mxArray* datatype_to_new_mat (datatype* d); 146 | 147 | datatype* mat_to_new_datatype (mxArray* mat); 148 | 149 | #endif // HAS_SOME_LIBARARY 150 | */ 151 | 152 | /** 153 | * Deprecated. Use values_to_new_mxArr instead. 154 | */ 155 | template 156 | mxArray* values_to_new_mat(iter_t beg, iter_t end) { 157 | return values_to_new_mxArr(beg, end); 158 | } 159 | 160 | /** 161 | * Deprecated. Use mxArr_to_values instead. 162 | */ 163 | template 164 | void mat_to_values(const mxArray* mat, iter_t beg) { 165 | mxArr_to_values(mat, beg); 166 | } 167 | 168 | /** 169 | * Deprecated. Use scalar_to_new_mxArr instead. 170 | */ 171 | template 172 | mxArray* scalar_to_new_mat(iter_t it) { 173 | return scalar_to_new_mxArr(it); 174 | } 175 | 176 | /** 177 | * Deprecated. Use mxArr_to_scalar instead. 178 | */ 179 | template 180 | void mat_to_scalar(const mxArray* mat, iter_t it) { 181 | mxArr_to_scalar(mat, it); 182 | } 183 | 184 | 185 | #ifdef HAS_OPENCV 186 | 187 | #include 188 | 189 | /** 190 | * Deprecated. Use image_to_new_mxArr instead. 191 | */ 192 | mxArray* image_to_new_mat(const IplImage* img); 193 | 194 | /** 195 | * Deprecated. Use mxArr_to_new_image instead. 196 | */ 197 | IplImage* mat_to_new_image(const mxArray* mat); 198 | 199 | #endif // HAS_OPENCV 200 | #endif // mc_convert_h__ -------------------------------------------------------------------------------- /src/include/types.h: -------------------------------------------------------------------------------- 1 | /** 2 | * types.h 3 | * types and traits 4 | */ 5 | 6 | #ifndef types_h__ 7 | #define types_h__ 8 | 9 | #include 10 | // Uncomment below #define if your Matlab is prior to R2008a 11 | //#ifndef mwSize 12 | //#define mwSize size_t 13 | //#endif 14 | 15 | /* 16 | * Bi-directional mapping between C/C++ native types and Matlab class id 17 | */ 18 | 19 | // C/C++ native type -> matlab class id 20 | 21 | template 22 | struct cm_traits { 23 | }; 24 | 25 | template<> 26 | struct cm_traits { 27 | static const mxClassID CID = mxDOUBLE_CLASS; 28 | }; 29 | 30 | template<> 31 | struct cm_traits { 32 | static const mxClassID CID = mxDOUBLE_CLASS; 33 | }; 34 | 35 | template<> 36 | struct cm_traits { 37 | static const mxClassID CID = mxSINGLE_CLASS; 38 | }; 39 | 40 | template<> 41 | struct cm_traits { 42 | static const mxClassID CID = mxSINGLE_CLASS; 43 | }; 44 | 45 | template<> 46 | struct cm_traits { 47 | static const mxClassID CID = mxUINT64_CLASS; 48 | }; 49 | 50 | template<> 51 | struct cm_traits { 52 | static const mxClassID CID = mxUINT64_CLASS; 53 | }; 54 | 55 | template<> 56 | struct cm_traits { 57 | static const mxClassID CID = mxUINT32_CLASS; 58 | }; 59 | 60 | template<> 61 | struct cm_traits { 62 | static const mxClassID CID = mxUINT32_CLASS; 63 | }; 64 | 65 | template<> 66 | struct cm_traits { 67 | static const mxClassID CID = mxUINT32_CLASS; 68 | }; 69 | 70 | template<> 71 | struct cm_traits { 72 | static const mxClassID CID = mxUINT32_CLASS; 73 | }; 74 | 75 | template<> 76 | struct cm_traits { 77 | static const mxClassID CID = mxUINT16_CLASS; 78 | }; 79 | 80 | template<> 81 | struct cm_traits { 82 | static const mxClassID CID = mxUINT16_CLASS; 83 | }; 84 | 85 | template<> 86 | struct cm_traits { 87 | static const mxClassID CID = mxUINT8_CLASS; 88 | }; 89 | 90 | template<> 91 | struct cm_traits { 92 | static const mxClassID CID = mxUINT8_CLASS; 93 | }; 94 | 95 | template<> 96 | struct cm_traits { 97 | static const mxClassID CID = mxCHAR_CLASS; 98 | }; 99 | 100 | template<> 101 | struct cm_traits { 102 | static const mxClassID CID = mxCHAR_CLASS; 103 | }; 104 | 105 | template<> 106 | struct cm_traits { 107 | static const mxClassID CID = mxINT8_CLASS; 108 | }; 109 | 110 | template<> 111 | struct cm_traits { 112 | static const mxClassID CID = mxINT8_CLASS; 113 | }; 114 | 115 | // matlab class id -> C/C++ native type 116 | 117 | template 118 | struct mc_traits { 119 | }; 120 | 121 | template<> 122 | struct mc_traits { 123 | typedef double CT; 124 | }; 125 | 126 | template<> 127 | struct mc_traits { 128 | typedef float CT; 129 | }; 130 | 131 | template<> 132 | struct mc_traits { 133 | typedef uint64_T CT; 134 | }; 135 | 136 | template<> 137 | struct mc_traits { 138 | typedef uint32_T CT; 139 | }; 140 | 141 | template<> 142 | struct mc_traits { 143 | typedef int32_T CT; 144 | }; 145 | 146 | template<> 147 | struct mc_traits { 148 | typedef int16_T CT; 149 | }; 150 | 151 | template<> 152 | struct mc_traits { 153 | typedef uint16_T CT; 154 | }; 155 | 156 | template<> 157 | struct mc_traits { 158 | typedef uint8_T CT; 159 | }; 160 | 161 | template<> 162 | struct mc_traits { 163 | typedef char CT; 164 | }; 165 | 166 | template<> 167 | struct mc_traits { 168 | typedef uint8_T CT; 169 | }; 170 | 171 | template<> 172 | struct mc_traits { 173 | typedef wchar_t CT; 174 | }; 175 | 176 | #include 177 | /* 178 | * Bi-directional mapping between openCV and Matlab class id 179 | */ 180 | 181 | // openCV -> matlab class id 182 | 183 | template 184 | struct cvm_traits { 185 | }; 186 | 187 | template<> 188 | struct cvm_traits { 189 | static const mxClassID CID = mxDOUBLE_CLASS; 190 | }; 191 | 192 | template<> 193 | struct cvm_traits { 194 | static const mxClassID CID = mxSINGLE_CLASS; 195 | }; 196 | 197 | template<> 198 | struct cvm_traits { 199 | static const mxClassID CID = mxUINT8_CLASS; 200 | }; 201 | 202 | template<> 203 | struct cvm_traits { 204 | static const mxClassID CID = mxDOUBLE_CLASS; 205 | }; 206 | 207 | template<> 208 | struct cvm_traits { 209 | static const mxClassID CID = mxSINGLE_CLASS; 210 | }; 211 | 212 | template<> 213 | struct cvm_traits { 214 | static const mxClassID CID = mxINT32_CLASS; 215 | }; 216 | 217 | template<> 218 | struct cvm_traits { 219 | static const mxClassID CID = mxINT16_CLASS; 220 | }; 221 | 222 | template<> 223 | struct cvm_traits { 224 | static const mxClassID CID = mxUINT16_CLASS; 225 | }; 226 | 227 | template<> 228 | struct cvm_traits { 229 | static const mxClassID CID = mxINT8_CLASS; 230 | }; 231 | 232 | template<> 233 | struct cvm_traits { 234 | static const mxClassID CID = mxUINT8_CLASS; 235 | }; 236 | 237 | 238 | // matlab class id-> openCV 239 | 240 | template 241 | struct mcv_traits { 242 | }; 243 | 244 | template<> 245 | struct mcv_traits { 246 | static const int CV_DEPTH = IPL_DEPTH_64F; 247 | static const int CV_TYPE = CV_64FC1; 248 | }; 249 | 250 | template<> 251 | struct mcv_traits { 252 | static const int CV_DEPTH = IPL_DEPTH_32F; 253 | static const int CV_TYPE = CV_32FC1; 254 | }; 255 | 256 | template<> 257 | struct mcv_traits { 258 | static const int CV_DEPTH = IPL_DEPTH_32S; 259 | static const int CV_TYPE = CV_32SC1; 260 | }; 261 | 262 | template<> 263 | struct mcv_traits { 264 | static const int CV_DEPTH = IPL_DEPTH_16S; 265 | static const int CV_TYPE = CV_16SC1; 266 | }; 267 | 268 | template<> 269 | struct mcv_traits { 270 | static const int CV_DEPTH = IPL_DEPTH_16U; 271 | static const int CV_TYPE = CV_16UC1; 272 | }; 273 | 274 | template<> 275 | struct mcv_traits { 276 | static const int CV_DEPTH = IPL_DEPTH_8S; 277 | static const int CV_TYPE = CV_8SC1; 278 | }; 279 | 280 | template<> 281 | struct mcv_traits { 282 | static const int CV_DEPTH = IPL_DEPTH_8U; 283 | static const int CV_TYPE = CV_8UC1; 284 | }; 285 | 286 | template<> 287 | struct mcv_traits { 288 | static const int CV_DEPTH = IPL_DEPTH_8U; 289 | static const int CV_TYPE = CV_8UC1; 290 | }; 291 | 292 | #endif // types_h__ -------------------------------------------------------------------------------- /src/mex/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(TARGET mexZED) 2 | 3 | if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") 4 | SET(CMAKE_BUILD_TYPE "RelWithDebInfo") 5 | endif() 6 | 7 | include_directories(${CUDA_INCLUDE_DIRS}) 8 | include_directories(${ZED_INCLUDE_DIRS}) 9 | include_directories(${OpenCV_INCLUDE_DIRS}) 10 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) 11 | 12 | link_directories(${ZED_LIBRARY_DIR}) 13 | link_directories(${OpenCV_LIBRARY_DIRS}) 14 | link_directories(${CUDA_LIBRARY_DIRS}) 15 | 16 | INCLUDE_DIRECTORIES(${MATLAB_INCLUDE_DIR}) 17 | add_library(${TARGET} SHARED mexZED.cpp ${CMAKE_SOURCE_DIR}/Matlabdef.def) 18 | 19 | target_link_libraries(${TARGET} 20 | ${MATLAB_LIBRARIES} 21 | ${ZED_LIBRARIES} 22 | ${OpenCV_LIBRARIES} 23 | ${CUDA_LIBRARIES} ${CUDA_NPP_LIBRARIES_ZED}) 24 | 25 | if(WIN32) 26 | SET_TARGET_PROPERTIES(${TARGET} PROPERTIES SUFFIX .mexw64) 27 | else(WIN32) 28 | if (CMAKE_SIZEOF_VOID_P MATCHES "8") 29 | SET_TARGET_PROPERTIES(${TARGET} PROPERTIES SUFFIX .mexa64 PREFIX "") 30 | else(CMAKE_SIZEOF_VOID_P MATCHES "8") 31 | SET_TARGET_PROPERTIES(${TARGET} PROPERTIES SUFFIX .mexglx PREFIX "") 32 | endif (CMAKE_SIZEOF_VOID_P MATCHES "8") 33 | endif(WIN32) 34 | 35 | install(TARGETS ${TARGET} DESTINATION ../matlab) 36 | -------------------------------------------------------------------------------- /src/mex/mexZED.cpp: -------------------------------------------------------------------------------- 1 | /********************************* 2 | ** Using ZED with Matlab ** 3 | *********************************/ 4 | 5 | // MEX header 6 | #include 7 | #include "matrix.h" 8 | 9 | // OpenCV 10 | #include 11 | 12 | // Matrix format conversion by Sun Peng : http://www.mathworks.com/matlabcentral/fileexchange/20927-c-c++-and-matlab-types-convertor 13 | #include "mc_convert.h" 14 | #include "iter.h" 15 | #include "types.h" 16 | 17 | // system header 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | // ZED 24 | #include 25 | 26 | #ifdef _DEBUG 27 | #error Select Release mode for compilation 28 | #endif 29 | 30 | // global var. 31 | static sl::Camera *zedCam = nullptr; 32 | sl::SpatialMappingParameters sp_p; 33 | 34 | const int BUFF_SIZE = 256; 35 | 36 | // C++ struct 37 | const char* fieldsParameters[] = {"serial_number", "firmware_version", "R", "t", "left_cam", "right_cam", "model"}; 38 | 39 | // Interop. OpenCV-Matlab matrix type (float) 40 | template 41 | OutputIterator fctCopy(InputIterator first, InputIterator last, OutputIterator result) { 42 | while(first != last) { 43 | *result = *first; 44 | ++result; 45 | ++first; 46 | } 47 | return result; 48 | } 49 | 50 | template 51 | mxArray* floatmat_to_mat(cv::Mat &matrix) { 52 | void* pBeg = matrix.data; 53 | int pitch = matrix.step; 54 | cv::Size size = matrix.size(); 55 | const mxClassID cid = cvm_traits::CID; 56 | mxArray* pArrOut = mxCreateNumericMatrix(size.height, size.width, cid, mxREAL); 57 | void* pBegOut = mxGetData(pArrOut); 58 | 59 | typedef typename mc_traits::CT T; 60 | pix_iterator_2d it_src1(static_cast (pBeg), size.width, size.height, pitch); 61 | pix_iterator_2d it_src2(static_cast (pBeg), size.width, size.height, pitch); 62 | it_src2.end(); 63 | pix_iterator_2d it_dest(static_cast (pBegOut), size.width, size.height); 64 | 65 | fctCopy(it_src1, it_src2, it_dest); 66 | 67 | return pArrOut; 68 | } 69 | 70 | // Interop. OpenCV-Matlab matrix type (rgb) 71 | template 72 | mxArray* rgbimage_to_mat(cv::Mat &image) { 73 | const int ndim = image.channels(); 74 | cv::Size size = image.size(); 75 | mwSize dims[3]; 76 | dims[0] = size.height; 77 | dims[1] = size.width; 78 | dims[2] = ndim; 79 | const mxClassID cid = cvm_traits::CID; 80 | mxArray* pArrOut = mxCreateNumericArray(ndim, dims, cid, mxREAL); 81 | 82 | void* pBeg = image.data; 83 | int pitch = image.step; 84 | void* pBegOut = mxGetData(pArrOut); 85 | 86 | typedef typename mc_traits::CT T; 87 | pix_iter_rgb it_src1(static_cast (pBeg), size.width, size.height, pitch); 88 | pix_iter_rgb it_src2(static_cast (pBeg), size.width, size.height, pitch); 89 | it_src2.end(); 90 | mxArray_iter_3d it_dest(static_cast (pBegOut), size.width, size.height, ndim); 91 | fctCopy(it_src1, it_src2, it_dest); 92 | return pArrOut; 93 | } 94 | 95 | // Interop. OpenCV-Matlab matrix type 96 | mxArray* CvMat_to_new_mxArr(cv::Mat &matrix) { 97 | const int TYPE = matrix.type(); 98 | if(CV_32FC1 == TYPE) 99 | return floatmat_to_mat(matrix); 100 | else if (CV_16UC1 == TYPE) 101 | return floatmat_to_mat(matrix); 102 | else if((CV_8UC3 == TYPE) || (CV_8UC1 == TYPE)) 103 | return rgbimage_to_mat(matrix); 104 | return mxCreateDoubleMatrix(0, 0, mxREAL); 105 | } 106 | 107 | inline cv::Mat cvtMat(sl::Matrix3f &in){ 108 | return cv::Mat(3, 3, CV_32FC1, in.r); 109 | } 110 | 111 | inline cv::Mat cvtMat(sl::Matrix4f &in){ 112 | return cv::Mat(4, 4, CV_32FC1, in.m); 113 | } 114 | 115 | inline bool checkZED() { 116 | if(zedCam) 117 | return true; 118 | else 119 | mexWarnMsgTxt("ZED is not initialized"); 120 | return false; 121 | } 122 | 123 | inline bool checkParams(int params, int required, bool print_warning = true) { 124 | if(params - 1 != required) { 125 | if (print_warning) { 126 | std::string error = "Invalid parameter number, " + std::to_string(required) + " required, " + std::to_string(params - 1) + " given."; 127 | mexWarnMsgTxt(error.c_str()); 128 | } 129 | return false; 130 | } 131 | return true; 132 | } 133 | 134 | inline bool checkParams(int params, int required_1, int required_2, bool print_warning = true) { 135 | if((params - 1 != required_1) && (params - 1 != required_2)) { 136 | if (print_warning) { 137 | std::string error = "Invalid parameter number, " + std::to_string(required_1) + " or " + std::to_string(required_2) + " required, " + std::to_string(params - 1) + " given."; 138 | mexWarnMsgTxt(error.c_str()); 139 | } 140 | return false; 141 | } 142 | return true; 143 | } 144 | 145 | inline bool checkParamsAtLeast(int params, int number, bool print_warning = true) { 146 | if (params - 1 < number) { 147 | if (print_warning) { 148 | std::string error = "Invalid parameter number, " + std::to_string(number) + " required, " + std::to_string(params - 1) + " given."; 149 | mexWarnMsgTxt(error.c_str()); 150 | } 151 | return false; 152 | } 153 | return true; 154 | } 155 | 156 | const char* fieldsIntra[] = {"cx", "cy", "disto", "d_fov", "fx" , "fy" , "h_fov" , "width" , "height" , "v_fov"}; 157 | void fillCameraParam(mxArray *pArray, sl::CameraParameters ¶m) { 158 | mxSetField(pArray, 0, fieldsIntra[0], mxCreateDoubleScalar(param.cx)); 159 | mxSetField(pArray, 0, fieldsIntra[1], mxCreateDoubleScalar(param.cy)); 160 | mxArray* matDisto = mxCreateDoubleMatrix(1, 5, mxREAL); 161 | memcpy(mxGetPr(matDisto), &(param.disto), 5 * sizeof(double)); 162 | mxSetField(pArray, 0, fieldsIntra[2], matDisto); 163 | mxSetField(pArray, 0, fieldsIntra[3], mxCreateDoubleScalar(param.d_fov)); 164 | mxSetField(pArray, 0, fieldsIntra[4], mxCreateDoubleScalar(param.fx)); 165 | mxSetField(pArray, 0, fieldsIntra[5], mxCreateDoubleScalar(param.fy)); 166 | mxSetField(pArray, 0, fieldsIntra[6], mxCreateDoubleScalar(param.h_fov)); 167 | mxSetField(pArray, 0, fieldsIntra[7], mxCreateDoubleScalar(param.image_size.width)); 168 | mxSetField(pArray, 0, fieldsIntra[8], mxCreateDoubleScalar(param.image_size.height)); 169 | mxSetField(pArray, 0, fieldsIntra[9], mxCreateDoubleScalar(param.v_fov)); 170 | } 171 | 172 | const char* point3D[] = { "x", "y", "z"}; 173 | const char* point2D[] = { "u", "v"}; 174 | const char* fieldsObjectData[] = { "id", "label", "tracking_state", "action_state", "confidence", "position", "velocity", "bounding_box_2d", "bounding_box_3d"}; 175 | void fillObjectData(mxArray* pArray, sl::ObjectData& obj, int idx) { 176 | 177 | mxSetField(pArray, idx, fieldsObjectData[0], mxCreateDoubleScalar(static_cast(obj.id))); 178 | mxSetField(pArray, idx, fieldsObjectData[1], mxCreateDoubleScalar(static_cast(obj.label))); 179 | mxSetField(pArray, idx, fieldsObjectData[2], mxCreateDoubleScalar(static_cast(obj.tracking_state))); 180 | mxSetField(pArray, idx, fieldsObjectData[3], mxCreateDoubleScalar(static_cast(obj.action_state))); 181 | mxSetField(pArray, idx, fieldsObjectData[4], mxCreateDoubleScalar(static_cast(obj.confidence))); 182 | 183 | mxArray* position = mxCreateStructMatrix(1, 1, 3, point3D); 184 | mxSetField(position, 0, "x", mxCreateDoubleScalar(obj.position.x)); 185 | mxSetField(position, 0, "y", mxCreateDoubleScalar(obj.position.y)); 186 | mxSetField(position, 0, "z", mxCreateDoubleScalar(obj.position.z)); 187 | mxSetField(pArray, idx, fieldsObjectData[5], position); 188 | 189 | mxArray* velocity = mxCreateStructMatrix(1, 1, 3, point3D); 190 | mxSetField(velocity, 0, "x", mxCreateDoubleScalar(obj.velocity.x)); 191 | mxSetField(velocity, 0, "y", mxCreateDoubleScalar(obj.velocity.y)); 192 | mxSetField(velocity, 0, "z", mxCreateDoubleScalar(obj.velocity.z)); 193 | mxSetField(pArray, idx, fieldsObjectData[6], velocity); 194 | 195 | mxArray* bounding_box_2d = mxCreateStructMatrix(1, obj.bounding_box_2d.size(), 2, point2D); 196 | for (int i = 0; i < obj.bounding_box_2d.size(); i++){ 197 | mxSetField(bounding_box_2d, i, "u", mxCreateDoubleScalar(obj.bounding_box_2d[i].x)); 198 | mxSetField(bounding_box_2d, i, "v", mxCreateDoubleScalar(obj.bounding_box_2d[i].y)); 199 | } 200 | mxSetField(pArray, idx, fieldsObjectData[7], bounding_box_2d); 201 | 202 | mxArray* bounding_box = mxCreateStructMatrix(1, obj.bounding_box.size(), 3, point3D); 203 | for (int i = 0; i < obj.bounding_box.size(); i++) { 204 | mxSetField(bounding_box, i, "x", mxCreateDoubleScalar(obj.bounding_box[i].x)); 205 | mxSetField(bounding_box, i, "y", mxCreateDoubleScalar(obj.bounding_box[i].y)); 206 | mxSetField(bounding_box, i, "z", mxCreateDoubleScalar(obj.bounding_box[i].z)); 207 | } 208 | mxSetField(pArray, idx, fieldsObjectData[8], bounding_box); 209 | } 210 | 211 | const char* fieldsObject[] = { "time_stamp", "is_new", "is_tracked", "object_list" }; 212 | void fillObjects(mxArray* pArray, sl::Objects& objs) { 213 | mxSetField(pArray, 0, fieldsObject[0], mxCreateDoubleScalar(static_cast(objs.timestamp.getMilliseconds()))); 214 | mxSetField(pArray, 0, fieldsObject[1], mxCreateDoubleScalar(static_cast(objs.is_new))); 215 | mxSetField(pArray, 0, fieldsObject[2], mxCreateDoubleScalar(static_cast(objs.is_tracked))); 216 | 217 | mxArray* obj = mxCreateStructMatrix(1, objs.object_list.size(), 9, fieldsObjectData); 218 | for (int i = 0; i < objs.object_list.size(); i++) 219 | fillObjectData(obj, objs.object_list[i], i); 220 | mxSetField(pArray, 0, fieldsObject[3], obj); 221 | } 222 | 223 | const char* fieldsBodyData[] = { "id", "tracking_state", "action_state", "confidence", "position", "velocity", "bounding_box_2d", "bounding_box_3d", "keypoint_2d", "keypoint" }; 224 | void fillBodyData(mxArray* pArray, sl::BodyData& body, int idx) { 225 | 226 | mxSetField(pArray, idx, fieldsBodyData[0], mxCreateDoubleScalar(static_cast(body.id))); 227 | mxSetField(pArray, idx, fieldsBodyData[1], mxCreateDoubleScalar(static_cast(body.tracking_state))); 228 | mxSetField(pArray, idx, fieldsBodyData[2], mxCreateDoubleScalar(static_cast(body.action_state))); 229 | mxSetField(pArray, idx, fieldsBodyData[3], mxCreateDoubleScalar(static_cast(body.confidence))); 230 | 231 | mxArray* position = mxCreateStructMatrix(1, 1, 3, point3D); 232 | mxSetField(position, 0, "x", mxCreateDoubleScalar(body.position.x)); 233 | mxSetField(position, 0, "y", mxCreateDoubleScalar(body.position.y)); 234 | mxSetField(position, 0, "z", mxCreateDoubleScalar(body.position.z)); 235 | mxSetField(pArray, idx, fieldsBodyData[4], position); 236 | 237 | mxArray* velocity = mxCreateStructMatrix(1, 1, 3, point3D); 238 | mxSetField(velocity, 0, "x", mxCreateDoubleScalar(body.velocity.x)); 239 | mxSetField(velocity, 0, "y", mxCreateDoubleScalar(body.velocity.y)); 240 | mxSetField(velocity, 0, "z", mxCreateDoubleScalar(body.velocity.z)); 241 | mxSetField(pArray, idx, fieldsBodyData[5], velocity); 242 | 243 | mxArray* bounding_box_2d = mxCreateStructMatrix(1, body.bounding_box_2d.size(), 2, point2D); 244 | for (int i = 0; i < body.bounding_box_2d.size(); i++) { 245 | mxSetField(bounding_box_2d, i, "u", mxCreateDoubleScalar(body.bounding_box_2d[i].x)); 246 | mxSetField(bounding_box_2d, i, "v", mxCreateDoubleScalar(body.bounding_box_2d[i].y)); 247 | } 248 | mxSetField(pArray, idx, fieldsBodyData[6], bounding_box_2d); 249 | 250 | mxArray* bounding_box = mxCreateStructMatrix(1, body.bounding_box.size(), 3, point3D); 251 | for (int i = 0; i < body.bounding_box.size(); i++) { 252 | mxSetField(bounding_box, i, "x", mxCreateDoubleScalar(body.bounding_box[i].x)); 253 | mxSetField(bounding_box, i, "y", mxCreateDoubleScalar(body.bounding_box[i].y)); 254 | mxSetField(bounding_box, i, "z", mxCreateDoubleScalar(body.bounding_box[i].z)); 255 | } 256 | mxSetField(pArray, idx, fieldsBodyData[7], bounding_box); 257 | 258 | mxArray* keypoint_2d = mxCreateStructMatrix(1, body.keypoint_2d.size(), 2, point2D); 259 | for (int i = 0; i < body.keypoint_2d.size(); i++) { 260 | mxSetField(keypoint_2d, i, "u", mxCreateDoubleScalar(body.keypoint_2d[i].x)); 261 | mxSetField(keypoint_2d, i, "v", mxCreateDoubleScalar(body.keypoint_2d[i].y)); 262 | } 263 | mxSetField(pArray, idx, fieldsBodyData[8], keypoint_2d); 264 | 265 | mxArray* keypoint = mxCreateStructMatrix(1, body.keypoint.size(), 3, point3D); 266 | for (int i = 0; i < body.keypoint.size(); i++) { 267 | mxSetField(keypoint, i, "x", mxCreateDoubleScalar(body.keypoint[i].x)); 268 | mxSetField(keypoint, i, "y", mxCreateDoubleScalar(body.keypoint[i].y)); 269 | mxSetField(keypoint, i, "z", mxCreateDoubleScalar(body.keypoint[i].z)); 270 | } 271 | mxSetField(pArray, idx, fieldsBodyData[9], keypoint); 272 | } 273 | 274 | const char* fieldsBodies[] = { "time_stamp", "is_new", "is_tracked", "body_format", "body_list"}; 275 | void fillBodies(mxArray* pArray, sl::Bodies& bodies) { 276 | mxSetField(pArray, 0, fieldsBodies[0], mxCreateDoubleScalar(static_cast(bodies.timestamp.getMilliseconds()))); 277 | mxSetField(pArray, 0, fieldsBodies[1], mxCreateDoubleScalar(static_cast(bodies.is_new))); 278 | mxSetField(pArray, 0, fieldsBodies[2], mxCreateDoubleScalar(static_cast(bodies.is_tracked))); 279 | mxSetField(pArray, 0, fieldsBodies[3], mxCreateDoubleScalar(static_cast(bodies.body_format))); 280 | 281 | mxArray* obj = mxCreateStructMatrix(1, bodies.body_list.size(), 10, fieldsBodyData); 282 | for (int i = 0; i < bodies.body_list.size(); i++) 283 | fillBodyData(obj, bodies.body_list[i], i); 284 | mxSetField(pArray, 0, fieldsBodies[4], obj); 285 | } 286 | 287 | const char* fieldsTemp[] = {"IMU", "BAROMETER", "ONBOARD_LEFT", "ONBOARD_RIGHT" }; 288 | void fillTemp(mxArray* pArray, sl::SensorsData::TemperatureData& temp) { 289 | mxSetField(pArray, 0, fieldsTemp[0], mxCreateDoubleScalar(temp.temperature_map[sl::SensorsData::TemperatureData::SENSOR_LOCATION::IMU])); 290 | mxSetField(pArray, 0, fieldsTemp[1], mxCreateDoubleScalar(temp.temperature_map[sl::SensorsData::TemperatureData::SENSOR_LOCATION::BAROMETER])); 291 | mxSetField(pArray, 0, fieldsTemp[2], mxCreateDoubleScalar(temp.temperature_map[sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_LEFT])); 292 | mxSetField(pArray, 0, fieldsTemp[3], mxCreateDoubleScalar(temp.temperature_map[sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT])); 293 | } 294 | 295 | const char* fieldsMag[] = {"is_available", "timestamp", "magnetic_field_calibrated", "magnetic_field_uncalibrated", "effective_rate" }; 296 | void fillMag(mxArray* pArray, sl::SensorsData::MagnetometerData& mag) { 297 | //is_available 298 | mxSetField(pArray, 0, fieldsMag[0], mxCreateDoubleScalar(mag.is_available)); 299 | 300 | //timestamp 301 | mxSetField(pArray, 0, fieldsMag[1], mxCreateDoubleScalar(mag.timestamp.getMilliseconds())); 302 | 303 | //magnetic_field_calibrated 304 | const mxClassID cid = cvm_traits::CID; 305 | mxArray* magnetic_field_calibrated = mxCreateNumericMatrix(1, 3, cid, mxREAL); 306 | memcpy(mxGetPr(magnetic_field_calibrated), &(mag.magnetic_field_calibrated), 3 * sizeof(float)); 307 | mxSetField(pArray, 0, fieldsMag[2], magnetic_field_calibrated); 308 | 309 | //magnetic_field_uncalibrated 310 | mxArray* magnetic_field_uncalibrated = mxCreateNumericMatrix(1, 3, cid, mxREAL); 311 | memcpy(mxGetPr(magnetic_field_uncalibrated), &(mag.magnetic_field_uncalibrated), 3 * sizeof(float)); 312 | mxSetField(pArray, 0, fieldsMag[3], magnetic_field_uncalibrated); 313 | 314 | //effective_rate 315 | mxSetField(pArray, 0, fieldsMag[4], mxCreateDoubleScalar(mag.effective_rate)); 316 | } 317 | 318 | const char* fieldsBaro[] = {"is_available", "timestamp", "pressure", "relative_altitude", "effective_rate" }; 319 | void fillBaro(mxArray* pArray, sl::SensorsData::BarometerData& baro) { 320 | //is_available 321 | mxSetField(pArray, 0, fieldsBaro[0], mxCreateDoubleScalar(baro.is_available)); 322 | 323 | //timestamp 324 | mxSetField(pArray, 0, fieldsBaro[1], mxCreateDoubleScalar(baro.timestamp.getMilliseconds())); 325 | 326 | //pressure 327 | mxSetField(pArray, 0, fieldsBaro[2], mxCreateDoubleScalar(baro.pressure)); 328 | 329 | //relative_altitude 330 | mxSetField(pArray, 0, fieldsBaro[3], mxCreateDoubleScalar(baro.relative_altitude)); 331 | 332 | //effective_rate 333 | mxSetField(pArray, 0, fieldsBaro[4], mxCreateDoubleScalar(baro.effective_rate)); 334 | } 335 | 336 | const char* fieldsIMU[] = {"is_available", "timestamp", "pose", "pose_covariance", "angular_velocity", "linear_acceleration","angular_velocity_covariance","linear_acceleration_covariance","effective_rate"}; 337 | void fillImu(mxArray* pArray, sl::SensorsData::IMUData& imu) { 338 | //is_available 339 | mxSetField(pArray, 0, fieldsIMU[0], mxCreateDoubleScalar(imu.is_available)); 340 | 341 | //timestamp 342 | mxSetField(pArray, 0, fieldsIMU[1], mxCreateDoubleScalar(imu.timestamp.getMilliseconds())); 343 | 344 | //pose 345 | cv::Mat pose = cvtMat(imu.pose); 346 | mxSetField(pArray, 0, fieldsIMU[2], CvMat_to_new_mxArr(pose)); 347 | 348 | //pose_covariance 349 | cv::Mat pose_covariance = cvtMat(imu.pose_covariance); 350 | mxSetField(pArray, 0, fieldsIMU[3], CvMat_to_new_mxArr(pose_covariance)); 351 | 352 | //angular_velocity 353 | const mxClassID cid = cvm_traits::CID; 354 | mxArray* angular_v = mxCreateNumericMatrix(1, 3, cid, mxREAL); 355 | memcpy(mxGetPr(angular_v), &(imu.angular_velocity), 3 * sizeof(float)); 356 | mxSetField(pArray, 0, fieldsIMU[4], angular_v); 357 | 358 | //linear_acceleration 359 | mxArray* linear_a = mxCreateNumericMatrix(1, 3, cid, mxREAL); 360 | memcpy(mxGetPr(linear_a), &(imu.linear_acceleration), 3 * sizeof(float)); 361 | mxSetField(pArray, 0, fieldsIMU[5], linear_a); 362 | 363 | //angular_velocity_covariance 364 | cv::Mat angular_velocity_covariance = cvtMat(imu.angular_velocity_covariance); 365 | mxSetField(pArray, 0, fieldsIMU[6], CvMat_to_new_mxArr(angular_velocity_covariance)); 366 | 367 | //linear_acceleration_covariance 368 | cv::Mat linear_acceleration_covariance = cvtMat(imu.linear_acceleration_covariance); 369 | mxSetField(pArray, 0, fieldsIMU[7], CvMat_to_new_mxArr(linear_acceleration_covariance)); 370 | 371 | //effective_rate 372 | mxSetField(pArray, 0, fieldsIMU[8], mxCreateDoubleScalar(imu.effective_rate)); 373 | } 374 | 375 | const char* fieldsSensors[] = {"BarometerData", "IMUData", "MagnetometerData", "TemperatureData"}; 376 | void fillSensors(mxArray* pArray, sl::SensorsData& data) { 377 | auto baro = mxCreateStructMatrix(1, 1, 5, fieldsBaro); 378 | fillBaro(baro, data.barometer); 379 | mxSetField(pArray, 0, fieldsSensors[0], baro); 380 | 381 | auto imu = mxCreateStructMatrix(1, 1, 9, fieldsIMU); 382 | fillImu(imu, data.imu); 383 | mxSetField(pArray, 0, fieldsSensors[1], imu); 384 | 385 | auto mag = mxCreateStructMatrix(1, 1, 5, fieldsMag); 386 | fillMag(mag, data.magnetometer); 387 | mxSetField(pArray, 0, fieldsSensors[2], mag); 388 | 389 | auto temp = mxCreateStructMatrix(1, 1, 4, fieldsTemp); 390 | fillTemp(temp, data.temperature); 391 | mxSetField(pArray, 0, fieldsSensors[3], temp); 392 | } 393 | 394 | template 395 | inline void getValue(std::string ref, std::string curr, mxArray* v_in, T& output) { 396 | if(ref == curr) { 397 | if(mxIsNumeric(v_in)) { 398 | int output_ID = mxGetPr(v_in)[0]; 399 | output = static_cast(output_ID); 400 | //mexPrintf("Set %s\n", curr.c_str()); 401 | } 402 | else 403 | mexPrintf("Can not convert %s\n", curr.c_str()); 404 | } 405 | } 406 | 407 | /* MEX entry function */ 408 | void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 409 | 410 | sl::ERROR_CODE err = sl::ERROR_CODE::FAILURE; 411 | // each sub function is referred by a string 'command' 412 | char command[BUFF_SIZE]; 413 | mxGetString(prhs[0], command, BUFF_SIZE); 414 | 415 | if(!strcmp(command, "open")) { 416 | zedCam = new sl::Camera(); 417 | sl::InitParameters initParams; 418 | // check if we have ONE argument 419 | if(checkParams(nrhs, 1)) { 420 | auto arg = prhs[1]; 421 | // check if we have a parameter structure 422 | if(mxIsStruct(arg)) { 423 | // for all fields of parameter structure overwrite parameters 424 | for(int32_t i = 0; i < mxGetNumberOfFields(arg); i++) { 425 | const char *field_name = mxGetFieldNameByNumber(arg, i); 426 | mxArray *field_val = mxGetFieldByNumber(arg, 0, i); 427 | if(mxIsChar(field_val) && (!strcmp(field_name, "svo_input_filename"))){ 428 | char string_val[BUFF_SIZE]; 429 | mxGetString(field_val, string_val, BUFF_SIZE); 430 | initParams.input.setFromSVOFile(string_val); 431 | }else{ 432 | getValue(field_name, "depth_mode", field_val, initParams.depth_mode); 433 | getValue(field_name, "coordinate_units", field_val, initParams.coordinate_units); 434 | getValue(field_name, "coordinate_system", field_val, initParams.coordinate_system); 435 | getValue(field_name, "camera_resolution", field_val, initParams.camera_resolution); 436 | getValue(field_name, "sdk_verbose", field_val, initParams.sdk_verbose); 437 | getValue(field_name, "sdk_gpu_id", field_val, initParams.sdk_gpu_id); 438 | getValue(field_name, "depth_minimum_distance", field_val, initParams.depth_minimum_distance); 439 | getValue(field_name, "depth_maximum_distance", field_val, initParams.depth_maximum_distance); 440 | getValue(field_name, "camera_disable_self_calib", field_val, initParams.camera_disable_self_calib); 441 | getValue(field_name, "camera_image_flip", field_val, initParams.camera_image_flip); 442 | getValue(field_name, "camera_fps", field_val, initParams.camera_fps); 443 | getValue(field_name, "svo_real_time_mode", field_val, initParams.svo_real_time_mode); 444 | getValue(field_name, "depth_stabilization", field_val, initParams.depth_stabilization); 445 | getValue(field_name, "enable_right_side_measure", field_val, initParams.enable_right_side_measure); 446 | } 447 | } 448 | } 449 | } 450 | err = zedCam->open(initParams); 451 | 452 | std::cout << "VERBOSE " << initParams.sdk_verbose << std::endl; 453 | // we return the string associated with the error 454 | plhs[0] = mxCreateString(sl::toString(err).c_str()); 455 | } 456 | 457 | else if(!strcmp(command, "close")) { 458 | if(checkZED() && checkParams(nrhs, 0)) { 459 | zedCam->close(); 460 | delete zedCam; 461 | zedCam = nullptr; 462 | err = sl::ERROR_CODE::SUCCESS; 463 | } 464 | } 465 | 466 | else if(!strcmp(command, "isOpened")) { 467 | if(checkZED() && checkParams(nrhs, 0)) { 468 | double val = zedCam->isOpened(); 469 | plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL); 470 | memcpy(mxGetPr(plhs[0]), &val, 1 * sizeof(double)); 471 | err = sl::ERROR_CODE::SUCCESS; 472 | } 473 | } 474 | 475 | else if(!strcmp(command, "grab")) { 476 | if(checkZED()) { 477 | sl::RuntimeParameters param; 478 | // check if we have a parameter structure 479 | if ((nrhs > 1) && (mxIsStruct(prhs[1]))) { 480 | // for all fields of parameter structure overwrite parameters 481 | for (int32_t i = 0; i < mxGetNumberOfFields(prhs[1]); i++) { 482 | const char *field_name = mxGetFieldNameByNumber(prhs[1], i); 483 | mxArray *field_val = mxGetFieldByNumber(prhs[1], 0, i); 484 | 485 | getValue(field_name, "confidence_threshold", field_val, param.confidence_threshold); 486 | getValue(field_name, "enable_fill_mode", field_val, param.enable_fill_mode); 487 | getValue(field_name, "remove_saturated_areas", field_val, param.remove_saturated_areas); 488 | getValue(field_name, "texture_confidence_threshold", field_val, param.texture_confidence_threshold); 489 | getValue(field_name, "enable_depth", field_val, param.enable_depth); 490 | getValue(field_name, "measure3D_reference_frame", field_val, param.measure3D_reference_frame); 491 | } 492 | } 493 | err = zedCam->grab(param); 494 | } 495 | // we return the string associated with the error 496 | plhs[0] = mxCreateString(sl::toString(err).c_str()); 497 | } 498 | 499 | else if(!strcmp(command, "retrieveImage")) { 500 | if(checkZED()) { 501 | sl::VIEW view = sl::VIEW::LEFT; 502 | if(checkParams(nrhs, 1, 3)) { 503 | double *ptr_ = mxGetPr(prhs[1]); 504 | int val = ptr_[0]; 505 | if(val < static_cast(sl::VIEW::LAST)) 506 | view = static_cast(val); 507 | else { 508 | mexWarnMsgTxt("Unknown VIEW requested"); 509 | return; 510 | } 511 | } 512 | int width = 0, height = 0; 513 | if(nrhs == 4) { 514 | double *ptr_ = mxGetPr(prhs[2]); 515 | width = ptr_[0]; 516 | ptr_ = mxGetPr(prhs[3]); 517 | height = ptr_[0]; 518 | } 519 | 520 | sl::Mat tmp; 521 | err = zedCam->retrieveImage(tmp, view, sl::MEM::CPU, sl::Resolution(width, height)); 522 | int cv_type = (view == sl::VIEW::LEFT_GRAY || view == sl::VIEW::RIGHT_GRAY || view == sl::VIEW::LEFT_UNRECTIFIED_GRAY || view == sl::VIEW::RIGHT_UNRECTIFIED_GRAY) ? CV_8UC1 : CV_8UC4; 523 | cv::Mat cv_tmp(tmp.getHeight(), tmp.getWidth(), cv_type, tmp.getPtr()); 524 | 525 | if(cv_type == CV_8UC4){ 526 | cv::Mat image_rgb; 527 | cv::cvtColor(cv_tmp, image_rgb, cv::COLOR_RGBA2RGB); 528 | plhs[0] = CvMat_to_new_mxArr(image_rgb); 529 | }else{ 530 | cv::Mat image_gray; 531 | cv::cvtColor(cv_tmp, image_gray, cv::COLOR_GRAY2RGB); 532 | plhs[0] = CvMat_to_new_mxArr(image_gray); 533 | } 534 | } 535 | } 536 | 537 | else if(!strcmp(command, "retrieveMeasure")) { 538 | if(checkZED()) { 539 | sl::MEASURE measure = sl::MEASURE::DEPTH; 540 | if(checkParams(nrhs, 1, 3)) { 541 | double *ptr_ = mxGetPr(prhs[1]); 542 | int val = ptr_[0]; 543 | if(val < static_cast(sl::MEASURE::LAST)) 544 | measure = static_cast(val); 545 | else { 546 | mexWarnMsgTxt("Unknown MEASURE requested"); 547 | return; 548 | } 549 | } 550 | 551 | int width = 0, height = 0; 552 | if(nrhs == 4) { 553 | double *ptr_ = mxGetPr(prhs[2]); 554 | width = ptr_[0]; 555 | ptr_ = mxGetPr(prhs[3]); 556 | height = ptr_[0]; 557 | } 558 | 559 | sl::Mat tmp; 560 | err = zedCam->retrieveMeasure(tmp, measure, sl::MEM::CPU, sl::Resolution(width, height)); 561 | int cv_type = ((measure >= sl::MEASURE::XYZ && measure <= sl::MEASURE::NORMALS) || (measure >= sl::MEASURE::XYZ_RIGHT && measure < sl::MEASURE::LAST)) ? CV_32FC4 : CV_32FC1; 562 | cv::Mat measure_mat(tmp.getHeight(), tmp.getWidth(), cv_type, tmp.getPtr()); 563 | if(cv_type == CV_32FC4) { 564 | std::vector mat_v; 565 | cv::split(measure_mat, mat_v); 566 | plhs[0] = CvMat_to_new_mxArr(mat_v[0]); 567 | plhs[1] = CvMat_to_new_mxArr(mat_v[1]); 568 | plhs[2] = CvMat_to_new_mxArr(mat_v[2]); 569 | plhs[3] = CvMat_to_new_mxArr(mat_v[3]); 570 | } else 571 | plhs[0] = CvMat_to_new_mxArr(measure_mat); 572 | } 573 | } 574 | 575 | else if(!strcmp(command, "setSVOPosition")) { 576 | if(checkZED() && checkParams(nrhs, 1)) { 577 | double *ptr_ = mxGetPr(prhs[1]); 578 | int val = ptr_[0]; 579 | zedCam->setSVOPosition(val); 580 | err = sl::ERROR_CODE::SUCCESS; 581 | } 582 | } 583 | 584 | else if(!strcmp(command, "getSVOPosition")) { 585 | if(checkZED() && checkParams(nrhs, 0)) { 586 | double val = zedCam->getSVOPosition(); 587 | plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL); 588 | memcpy(mxGetPr(plhs[0]), &val, 1 * sizeof(double)); 589 | err = sl::ERROR_CODE::SUCCESS; 590 | } 591 | } 592 | 593 | else if(!strcmp(command, "getSVONumberOfFrames")) { 594 | if(checkZED() && checkParams(nrhs, 0)) { 595 | double val = zedCam->getSVONumberOfFrames(); 596 | plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL); 597 | memcpy(mxGetPr(plhs[0]), &val, 1 * sizeof(double)); 598 | err = sl::ERROR_CODE::SUCCESS; 599 | } 600 | } 601 | 602 | else if(!strcmp(command, "setCameraSettings")) { 603 | if(checkZED() && checkParamsAtLeast(nrhs, 2)) { 604 | char settingName[64]; 605 | mxGetString(prhs[1], settingName, 64); 606 | 607 | if (!strcmp(settingName, "aec_agc_roi")) { 608 | double* ptr_ = mxGetPr(prhs[2]); 609 | sl::Rect roi(ptr_[0], ptr_[1], ptr_[2], ptr_[3]); 610 | 611 | sl::SIDE side = sl::SIDE::BOTH; 612 | if (nrhs == 4) { 613 | ptr_ = mxGetPr(prhs[3]); 614 | side = static_cast((int)ptr_[0]); 615 | } 616 | 617 | bool reset = false; 618 | if (nrhs == 5) { 619 | ptr_ = mxGetPr(prhs[3]); 620 | reset = (int)ptr_[0]; 621 | } 622 | zedCam->setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC_ROI, roi, side, reset); 623 | } else { 624 | 625 | double* ptr_ = mxGetPr(prhs[2]); 626 | int val = ptr_[0]; 627 | 628 | if (!strcmp(settingName, "brightness")) 629 | zedCam->setCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS, val); 630 | else if (!strcmp(settingName, "contrast")) 631 | zedCam->setCameraSettings(sl::VIDEO_SETTINGS::CONTRAST, val); 632 | else if (!strcmp(settingName, "hue")) 633 | zedCam->setCameraSettings(sl::VIDEO_SETTINGS::HUE, val); 634 | else if (!strcmp(settingName, "saturation")) 635 | zedCam->setCameraSettings(sl::VIDEO_SETTINGS::SATURATION, val); 636 | else if (!strcmp(settingName, "gain")) 637 | zedCam->setCameraSettings(sl::VIDEO_SETTINGS::GAIN, val); 638 | else if (!strcmp(settingName, "exposure")) 639 | zedCam->setCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE, val); 640 | else if (!strcmp(settingName, "aec_agc")) 641 | zedCam->setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, val); 642 | else if (!strcmp(settingName, "whitebalance")) 643 | zedCam->setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE, val); 644 | else 645 | mexWarnMsgTxt("Unknown VIDEO SETTINGS"); 646 | } 647 | err = sl::ERROR_CODE::SUCCESS; 648 | } 649 | } 650 | 651 | else if(!strcmp(command, "getCameraSettings")) { 652 | if(checkZED() && checkParams(nrhs, 1)) { 653 | char settingName[64]; 654 | mxGetString(prhs[1], settingName, 64); 655 | 656 | if (!strcmp(settingName, "aec_agc_roi")) { 657 | sl::Rect roi; 658 | double val[4]; 659 | zedCam->getCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC_ROI, roi); 660 | val[0] = roi.x; 661 | val[1] = roi.y; 662 | val[2] = roi.width; 663 | val[3] = roi.height; 664 | plhs[0] = mxCreateDoubleMatrix(1, 4, mxREAL); 665 | memcpy(mxGetPr(plhs[0]), &val, 4 * sizeof(double)); 666 | } else { 667 | int val = 0; 668 | if (!strcmp(settingName, "brightness")) 669 | zedCam->getCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS, val); 670 | else if (!strcmp(settingName, "contrast")) 671 | zedCam->getCameraSettings(sl::VIDEO_SETTINGS::CONTRAST, val); 672 | else if (!strcmp(settingName, "hue")) 673 | zedCam->getCameraSettings(sl::VIDEO_SETTINGS::HUE, val); 674 | else if (!strcmp(settingName, "saturation")) 675 | zedCam->getCameraSettings(sl::VIDEO_SETTINGS::SATURATION, val); 676 | else if (!strcmp(settingName, "gain")) 677 | zedCam->getCameraSettings(sl::VIDEO_SETTINGS::GAIN, val); 678 | else if (!strcmp(settingName, "exposure")) 679 | zedCam->getCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE, val); 680 | else if (!strcmp(settingName, "aec_agc")) 681 | zedCam->getCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, val); 682 | else if (!strcmp(settingName, "whitebalance")) 683 | zedCam->getCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE, val); 684 | else 685 | mexWarnMsgTxt("Unknown VIDEO SETTINGS"); 686 | 687 | plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL); 688 | memcpy(mxGetPr(plhs[0]), &val, 1 * sizeof(double)); 689 | } 690 | err = sl::ERROR_CODE::SUCCESS; 691 | } 692 | } 693 | 694 | else if(!strcmp(command, "getCurrentFPS")) { 695 | if(checkZED() && checkParams(nrhs, 0)) { 696 | double val = zedCam->getCurrentFPS(); 697 | plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL); 698 | memcpy(mxGetPr(plhs[0]), &val, 1 * sizeof(double)); 699 | err = sl::ERROR_CODE::SUCCESS; 700 | } 701 | } 702 | 703 | else if(!strcmp(command, "getTimestamp")) { 704 | if(checkZED() && checkParams(nrhs, 1)) { 705 | int time_ref = mxGetPr(prhs[1])[0]; 706 | double val = zedCam->getTimestamp(static_cast(time_ref)).getMilliseconds(); 707 | plhs[0] = mxCreateDoubleScalar(val); 708 | err = sl::ERROR_CODE::SUCCESS; 709 | } 710 | } 711 | 712 | else if(!strcmp(command, "getCameraInformation")) { 713 | if(checkZED() && checkParams(nrhs, 0)) { 714 | sl::CameraInformation camInfo = zedCam->getCameraInformation(); 715 | mxArray *pLeft; 716 | mxArray *pRight; 717 | plhs[0] = mxCreateStructMatrix(1, 1, 7, fieldsParameters); 718 | pLeft = mxCreateStructMatrix(1, 1, 10, fieldsIntra); 719 | pRight = mxCreateStructMatrix(1, 1, 10, fieldsIntra); 720 | 721 | mxSetField(plhs[0], 0, fieldsParameters[0], mxCreateDoubleScalar(camInfo.serial_number)); 722 | mxSetField(plhs[0], 0, fieldsParameters[1], mxCreateDoubleScalar(camInfo.camera_configuration.firmware_version)); 723 | 724 | auto calibration_parameters = camInfo.camera_configuration.calibration_parameters; 725 | const mxClassID cid = cvm_traits::CID; 726 | mxArray* rotation = mxCreateNumericMatrix(1, 3, cid, mxREAL); 727 | auto rotation_vec = calibration_parameters.stereo_transform.getRotationVector(); 728 | memcpy(mxGetPr(rotation), &rotation_vec, sizeof(sl::float3)); 729 | mxSetField(plhs[0], 0, fieldsParameters[2], rotation); 730 | 731 | mxArray* translation = mxCreateNumericMatrix(1, 3, cid, mxREAL); 732 | auto translation_vec = calibration_parameters.stereo_transform.getTranslation(); 733 | memcpy(mxGetPr(translation), &translation_vec, sizeof(sl::float3)); 734 | mxSetField(plhs[0], 0, fieldsParameters[3], translation); 735 | 736 | fillCameraParam(pLeft, calibration_parameters.left_cam); 737 | mxSetField(plhs[0], 0, fieldsParameters[4], pLeft); 738 | 739 | fillCameraParam(pRight, calibration_parameters.right_cam); 740 | mxSetField(plhs[0], 0, fieldsParameters[5], pRight); 741 | 742 | mxSetField(plhs[0], 0, fieldsParameters[6], mxCreateDoubleScalar(static_cast(camInfo.camera_model))); 743 | err = sl::ERROR_CODE::SUCCESS; 744 | } 745 | } 746 | 747 | else if(!strcmp(command, "enablePositionalTracking")) { 748 | if(checkZED()) { 749 | sl::PositionalTrackingParameters param; 750 | // check if we have a parameter structure 751 | if((nrhs > 1) && (mxIsStruct(prhs[1]))) { 752 | sl::Transform tranform; 753 | // for all fields of parameter structure overwrite parameters 754 | for(int32_t i = 0; i < mxGetNumberOfFields(prhs[1]); i++) { 755 | const char *field_name = mxGetFieldNameByNumber(prhs[1], i); 756 | mxArray *field_val = mxGetFieldByNumber(prhs[1], 0, i); 757 | if(mxIsChar(field_val) && (!strcmp(field_name, "area_file_path"))){ 758 | char string_val[BUFF_SIZE]; 759 | mxGetString(field_val, string_val, BUFF_SIZE); 760 | param.area_file_path.set(string_val); 761 | } else { 762 | getValue(field_name, "enable_area_memory", field_val, param.enable_area_memory); 763 | getValue(field_name, "enable_imu_fusion", field_val, param.enable_imu_fusion); 764 | getValue(field_name, "enable_pose_smoothing", field_val, param.enable_pose_smoothing); 765 | getValue(field_name, "set_as_static", field_val, param.set_as_static); 766 | getValue(field_name, "set_floor_as_origin", field_val, param.set_floor_as_origin); 767 | if(!strcmp(field_name, "initial_world_transform")) { 768 | for(int col = 0; col < mxGetN(field_val); col++) 769 | for(int row = 0; row < mxGetM(field_val); row++) 770 | tranform(row, col) = (mxGetPr(field_val))[row + col*mxGetM(field_val)]; 771 | param.initial_world_transform = tranform; 772 | } 773 | } 774 | } 775 | } 776 | err = zedCam->enablePositionalTracking(param); 777 | } 778 | // we return the string associated with the error 779 | plhs[0] = mxCreateString(sl::toString(err).c_str()); 780 | } 781 | 782 | else if(!strcmp(command, "getPosition")) { 783 | if(checkZED() && checkParams(nrhs, 0)) { 784 | sl::REFERENCE_FRAME ref_f = sl::REFERENCE_FRAME::WORLD; 785 | if (nrhs == 2) { 786 | int val = mxGetPr(prhs[1])[0]; 787 | ref_f = static_cast(val); 788 | } 789 | 790 | sl::Pose path; 791 | auto state = zedCam->getPosition(path,ref_f); 792 | auto position = cvtMat(path.pose_data); 793 | plhs[0] = CvMat_to_new_mxArr(position); 794 | plhs[1] = mxCreateString(sl::toString(state).c_str()); 795 | 796 | err = sl::ERROR_CODE::SUCCESS; 797 | } 798 | } 799 | 800 | else if(!strcmp(command, "getSensorsData")) { 801 | if(checkZED() && checkParams(nrhs, 1)) { 802 | sl::SensorsData sensors; 803 | sl::TIME_REFERENCE t_ref = sl::TIME_REFERENCE::IMAGE; 804 | if (nrhs == 2) { 805 | int val = mxGetPr(prhs[1])[0]; 806 | t_ref = static_cast(val); 807 | } 808 | err = zedCam->getSensorsData(sensors, t_ref); 809 | plhs[0] = mxCreateStructMatrix(1, 1, 4, fieldsSensors); 810 | fillSensors(plhs[0], sensors); 811 | } 812 | } 813 | 814 | else if(!strcmp(command, "disablePositionalTracking")) { 815 | if(checkZED()) { 816 | if((nrhs > 1) && (mxIsChar(prhs[1]))) { 817 | char area_file_path[BUFF_SIZE]; 818 | mxGetString(prhs[1], area_file_path, BUFF_SIZE); 819 | zedCam->disablePositionalTracking(area_file_path); 820 | } else 821 | zedCam->disablePositionalTracking(); 822 | err = sl::ERROR_CODE::SUCCESS; 823 | } 824 | } 825 | 826 | else if(!strcmp(command, "resetTracking")) { 827 | if(checkZED() && checkParams(nrhs, 0)) { 828 | sl::Transform path; 829 | err = zedCam->resetPositionalTracking(path); 830 | } 831 | } 832 | 833 | else if(!strcmp(command, "enableRecording")) { 834 | if(checkZED()) { 835 | if(nrhs == 2) { 836 | sl::RecordingParameters param; 837 | if(mxIsStruct(prhs[1])){ 838 | for(int32_t i = 0; i < mxGetNumberOfFields(prhs[1]); i++) { 839 | const char *field_name = mxGetFieldNameByNumber(prhs[1], i); 840 | mxArray *field_val = mxGetFieldByNumber(prhs[1], 0, i); 841 | if(mxIsChar(field_val) && (!strcmp(field_name, "video_filename"))) { 842 | char svo_path[BUFF_SIZE]; 843 | mxGetString(field_val, svo_path, BUFF_SIZE); 844 | param.video_filename.set(svo_path); 845 | }else{ 846 | getValue(field_name, "bitrate", field_val, param.bitrate); 847 | getValue(field_name, "compression_mode", field_val, param.compression_mode); 848 | getValue(field_name, "target_framerate", field_val, param.target_framerate); 849 | getValue(field_name, "transcode_streaming_input", field_val, param.transcode_streaming_input); 850 | } 851 | } 852 | }else if(mxIsChar(prhs[1])) { 853 | char svo_path[BUFF_SIZE]; 854 | mxGetString(prhs[1], svo_path, BUFF_SIZE); 855 | param.video_filename.set(svo_path); 856 | } 857 | err = zedCam->enableRecording(param); 858 | } 859 | } 860 | // we return the string associated with the error 861 | plhs[0] = mxCreateString(sl::toString(err).c_str()); 862 | } 863 | 864 | else if(!strcmp(command, "disableRecording")) { 865 | if (checkZED()) { 866 | zedCam->disableRecording(); 867 | err = sl::ERROR_CODE::SUCCESS; 868 | } 869 | } 870 | 871 | else if (!strcmp(command, "enableObjectDetection")) { 872 | if (checkZED()) { 873 | sl::ObjectDetectionParameters param; 874 | if(nrhs == 2) { 875 | auto arg = prhs[1]; 876 | // check if we have a parameter structure 877 | if(mxIsStruct(arg)) { 878 | // for all fields of parameter structure overwrite parameters 879 | for(int32_t i = 0; i < mxGetNumberOfFields(arg); i++) { 880 | const char *field_name = mxGetFieldNameByNumber(arg, i); 881 | mxArray *field_val = mxGetFieldByNumber(arg, 0, i); 882 | getValue(field_name, "detection_model", field_val, param.detection_model); 883 | getValue(field_name, "filtering_mode", field_val, param.filtering_mode); 884 | getValue(field_name, "max_range", field_val, param.max_range); 885 | getValue(field_name, "enable_mask_output", field_val, param.enable_segmentation); 886 | getValue(field_name, "enable_tracking", field_val, param.enable_tracking); 887 | } 888 | } 889 | } 890 | err = zedCam->enableObjectDetection(param); 891 | } 892 | } 893 | 894 | else if (!strcmp(command, "retrieveObjects")) { 895 | if (checkZED()) { 896 | sl::ObjectDetectionRuntimeParameters param; 897 | if(nrhs == 2) { 898 | auto arg = prhs[1]; 899 | // check if we have a parameter structure 900 | if(mxIsStruct(arg)) { 901 | // for all fields of parameter structure overwrite parameters 902 | for(int32_t i = 0; i < mxGetNumberOfFields(arg); i++) { 903 | const char *field_name = mxGetFieldNameByNumber(arg, i); 904 | auto field_val = mxGetFieldByNumber(arg, 0, i); 905 | getValue(field_name, "detection_confidence_threshold", field_val, param.detection_confidence_threshold); 906 | } 907 | } else 908 | param.detection_confidence_threshold = static_cast(mxGetPr(arg)[0]); 909 | } 910 | 911 | sl::Objects objs; 912 | err = zedCam->retrieveObjects(objs, param); 913 | plhs[0] = mxCreateStructMatrix(1, 1, 4, fieldsObject); 914 | fillObjects(plhs[0], objs); 915 | } 916 | } 917 | 918 | else if (!strcmp(command, "disableObjectDetection")) { 919 | if (checkZED()) { 920 | zedCam->disableObjectDetection(); 921 | err = sl::ERROR_CODE::SUCCESS; 922 | } 923 | } 924 | 925 | else if (!strcmp(command, "enableBodyTracking")) { 926 | if (checkZED()) { 927 | sl::BodyTrackingParameters param; 928 | if (nrhs == 2) { 929 | auto arg = prhs[1]; 930 | // check if we have a parameter structure 931 | if (mxIsStruct(arg)) { 932 | // for all fields of parameter structure overwrite parameters 933 | for (int32_t i = 0; i < mxGetNumberOfFields(arg); i++) { 934 | const char* field_name = mxGetFieldNameByNumber(arg, i); 935 | mxArray* field_val = mxGetFieldByNumber(arg, 0, i); 936 | getValue(field_name, "detection_model", field_val, param.detection_model); 937 | getValue(field_name, "body_format", field_val, param.body_format); 938 | getValue(field_name, "enable_tracking", field_val, param.enable_tracking); 939 | getValue(field_name, "enable_body_fitting", field_val, param.enable_body_fitting); 940 | getValue(field_name, "enable_segmentation", field_val, param.enable_segmentation); 941 | getValue(field_name, "instance_module_id", field_val, param.instance_module_id); 942 | } 943 | } 944 | } 945 | err = zedCam->enableBodyTracking(param); 946 | } 947 | } 948 | 949 | else if (!strcmp(command, "retrieveBodies")) { 950 | if (checkZED()) { 951 | sl::BodyTrackingRuntimeParameters param; 952 | unsigned int instance_id = 0; 953 | if (nrhs == 2) { 954 | auto arg = prhs[1]; 955 | // check if we have a parameter structure 956 | if (mxIsStruct(arg)) { 957 | // for all fields of parameter structure overwrite parameters 958 | for (int32_t i = 0; i < mxGetNumberOfFields(arg); i++) { 959 | const char* field_name = mxGetFieldNameByNumber(arg, i); 960 | auto field_val = mxGetFieldByNumber(arg, 0, i); 961 | getValue(field_name, "detection_confidence_threshold", field_val, param.detection_confidence_threshold); 962 | getValue(field_name, "minimum_keypoints_threshold", field_val, param.minimum_keypoints_threshold); 963 | getValue(field_name, "instance_id", field_val, instance_id); 964 | } 965 | } 966 | else 967 | param.detection_confidence_threshold = static_cast(mxGetPr(arg)[0]); 968 | } 969 | 970 | sl::Bodies bodies; 971 | err = zedCam->retrieveBodies(bodies, param, instance_id); 972 | plhs[0] = mxCreateStructMatrix(1, 1, 5, fieldsBodies); 973 | fillBodies(plhs[0], bodies); 974 | } 975 | } 976 | 977 | else if (!strcmp(command, "disableBodyTracking")) { 978 | if (checkZED()) { 979 | zedCam->disableBodyTracking(); 980 | err = sl::ERROR_CODE::SUCCESS; 981 | } 982 | } 983 | else if(!strcmp(command, "getSDKVersion")) { 984 | if(checkParams(nrhs, 0)) { 985 | const char* version = sl::Camera::getSDKVersion(); 986 | plhs[0] = mxCreateString(version); 987 | err = sl::ERROR_CODE::SUCCESS; 988 | } 989 | } 990 | 991 | else if(!strcmp(command, "isZEDconnected")) { 992 | if(checkParams(nrhs, 0)) { 993 | double val = sl::Camera::getDeviceList().size(); 994 | plhs[0] = mxCreateDoubleScalar(val); 995 | err = sl::ERROR_CODE::SUCCESS; 996 | } 997 | } 998 | 999 | else if (!strcmp(command, "savePointCloudAs")) { 1000 | if (checkParams(nrhs, 2) || checkParams(nrhs, 3)) { 1001 | double *ptr_ = mxGetPr(prhs[1]); 1002 | int format = ptr_[0]; 1003 | 1004 | char save_path[BUFF_SIZE]; 1005 | if (mxIsChar(prhs[2])) { 1006 | mxGetString(prhs[2], save_path, BUFF_SIZE); 1007 | } 1008 | 1009 | int color = 0; 1010 | if (checkParams(nrhs, 3)) { 1011 | double *ptr1_ = mxGetPr(prhs[3]); 1012 | color = ptr1_[0]; 1013 | } 1014 | sl::Mat pc; 1015 | zedCam->retrieveMeasure(pc, sl::MEASURE::XYZBGRA); 1016 | err = pc.write(save_path); 1017 | plhs[0] = mxCreateString(sl::toString(err).c_str()); 1018 | } 1019 | } 1020 | 1021 | else if (!strcmp(command, "saveDepthAs")) { 1022 | if (checkParams(nrhs, 2) || checkParams(nrhs, 3)) { 1023 | double *ptr_ = mxGetPr(prhs[1]); 1024 | int format = ptr_[0]; 1025 | 1026 | char save_path[BUFF_SIZE]; 1027 | if (mxIsChar(prhs[2])) { 1028 | mxGetString(prhs[2], save_path, BUFF_SIZE); 1029 | } 1030 | 1031 | int color = 0; 1032 | if (checkParams(nrhs, 3)) { 1033 | double *ptr1_ = mxGetPr(prhs[3]); 1034 | color = ptr1_[0]; 1035 | } 1036 | 1037 | sl::Mat depth; 1038 | zedCam->retrieveMeasure(depth, sl::MEASURE::DEPTH); 1039 | err = depth.write(save_path); 1040 | plhs[0] = mxCreateString(sl::toString(err).c_str()); 1041 | } 1042 | } 1043 | else if (!strcmp(command, "enableSpatialMapping")) { 1044 | if (checkZED()) { 1045 | if (nrhs == 2) { 1046 | auto arg = prhs[1]; 1047 | // check if we have a parameter structure 1048 | if (mxIsStruct(arg)) { 1049 | // for all fields of parameter structure overwrite parameters 1050 | for (int32_t i = 0; i < mxGetNumberOfFields(arg); i++) { 1051 | const char* field_name = mxGetFieldNameByNumber(arg, i); 1052 | mxArray* field_val = mxGetFieldByNumber(arg, 0, i); 1053 | getValue(field_name, "map_type", field_val, sp_p.map_type); 1054 | getValue(field_name, "max_memory_usage", field_val, sp_p.max_memory_usage); 1055 | getValue(field_name, "range_meter", field_val, sp_p.range_meter); 1056 | getValue(field_name, "resolution_meter", field_val, sp_p.resolution_meter); 1057 | } 1058 | } 1059 | } 1060 | err = zedCam->enableSpatialMapping(sp_p); 1061 | } 1062 | } 1063 | else if (!strcmp(command, "extractWholeSpatialMap")) { 1064 | if (checkZED()) { 1065 | if (sp_p.map_type == sl::SpatialMappingParameters::SPATIAL_MAP_TYPE::MESH) { 1066 | sl::Mesh mesh; 1067 | err = zedCam->extractWholeSpatialMap(mesh); 1068 | if (err == sl::ERROR_CODE::SUCCESS) { 1069 | cv::Mat vert(mesh.vertices.size(), 3, CV_32FC1); 1070 | for (int i = 0; i < vert.rows; i++) { 1071 | auto v = mesh.vertices[i]; 1072 | vert.at(i, 0) = v.x; 1073 | vert.at(i, 1) = v.y; 1074 | vert.at(i, 2) = v.z; 1075 | } 1076 | cv::Mat faces(mesh.triangles.size(), 3, CV_16UC1); 1077 | for (int i = 0; i < faces.rows; i++) { 1078 | auto f = mesh.triangles[i]; 1079 | faces.at(i, 0) = f.x; 1080 | faces.at(i, 1) = f.y; 1081 | faces.at(i, 2) = f.z; 1082 | } 1083 | plhs[0] = CvMat_to_new_mxArr(vert); 1084 | plhs[1] = CvMat_to_new_mxArr(faces); 1085 | } 1086 | } 1087 | else{ 1088 | sl::FusedPointCloud fpc; 1089 | err = zedCam->extractWholeSpatialMap(fpc); 1090 | if (err == sl::ERROR_CODE::SUCCESS) { 1091 | cv::Mat vert(fpc.vertices.size(), 3, CV_32FC1); 1092 | cv::Mat clrs(fpc.vertices.size(), 1, CV_8UC3); 1093 | for (int i = 0; i < vert.rows; i++) { 1094 | auto v = fpc.vertices[i]; 1095 | vert.at(i, 0) = v.x; 1096 | vert.at(i, 1) = v.y; 1097 | vert.at(i, 2) = v.z; 1098 | // depack the color 1099 | uint32_t color_uint = *(uint32_t*)&v.w; 1100 | auto color_uchar = (uchar*)&color_uint; 1101 | clrs.at(i, 0) = cv::Vec3b(color_uchar[0], color_uchar[1], color_uchar[2]); 1102 | } 1103 | plhs[0] = CvMat_to_new_mxArr(vert); 1104 | plhs[1] = CvMat_to_new_mxArr(clrs); 1105 | } 1106 | } 1107 | } 1108 | } 1109 | else { 1110 | std::string undefined_fct("ZED SDK MEX does not contains specified function: " + std::string(command)); 1111 | mexWarnMsgTxt(undefined_fct.c_str()); 1112 | err = sl::ERROR_CODE::SUCCESS; 1113 | } 1114 | 1115 | if (err != sl::ERROR_CODE::SUCCESS) { 1116 | std::string error_msg("ZED SDK error when calling: " + std::string(command) + ", Error: "+ std::string(sl::toString(err))); 1117 | mexWarnMsgTxt(error_msg.c_str()); 1118 | } 1119 | } 1120 | --------------------------------------------------------------------------------