Permalink
06d6b6b Sep 22, 2018
2 contributors

Users who have contributed to this file

@adujardin @nesnes
78 lines (63 sloc) 2.94 KB
///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2018, STEREOLABS.
//
// All rights reserved.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////
#include <sl/Camera.hpp>
using namespace sl;
int main(int argc, char **argv) {
// Create a ZED camera object
Camera zed;
// Set configuration parameters
InitParameters init_params;
init_params.depth_mode = DEPTH_MODE_PERFORMANCE; // Use PERFORMANCE depth mode
init_params.coordinate_units = UNIT_MILLIMETER; // Use millimeter units (for depth measurements)
// Open the camera
ERROR_CODE err = zed.open(init_params);
if (err != SUCCESS)
exit(-1);
// Set runtime parameters after opening the camera
RuntimeParameters runtime_parameters;
runtime_parameters.sensing_mode = SENSING_MODE_STANDARD; // Use STANDARD sensing mode
// Capture 50 images and depth, then stop
int i = 0;
sl::Mat image, depth, point_cloud;
while (i < 50) {
// A new image is available if grab() returns SUCCESS
if (zed.grab(runtime_parameters) == SUCCESS) {
// Retrieve left image
zed.retrieveImage(image, VIEW_LEFT);
// Retrieve depth map. Depth is aligned on the left image
zed.retrieveMeasure(depth, MEASURE_DEPTH);
// Retrieve colored point cloud. Point cloud is aligned on the left image.
zed.retrieveMeasure(point_cloud, MEASURE_XYZRGBA);
// Get and print distance value in mm at the center of the image
// We measure the distance camera - object using Euclidean distance
int x = image.getWidth() / 2;
int y = image.getHeight() / 2;
sl::float4 point_cloud_value;
point_cloud.getValue(x, y, &point_cloud_value);
float distance = sqrt(point_cloud_value.x * point_cloud_value.x + point_cloud_value.y * point_cloud_value.y + point_cloud_value.z * point_cloud_value.z);
printf("Distance to Camera at (%d, %d): %f mm\n", x, y, distance);
// Increment the loop
i++;
}
}
// Close the camera
zed.close();
return 0;
}