Mech-Eye API 2.2.0
API reference documentation for Mech-Eye Industrial 3D Camera
All Classes Functions Variables Enumerations Enumerator Pages
api_util.h
1/*******************************************************************************
2 *BSD 3-Clause License
3 *
4 *Copyright (c) 2016-2023, Mech-Mind Robotics
5 *All rights reserved.
6 *
7 *Redistribution and use in source and binary forms, with or without
8 *modification, are permitted provided that the following conditions are met:
9 *
10 *1. Redistributions of source code must retain the above copyright notice, this
11 * list of conditions and the following disclaimer.
12 *
13 *2. Redistributions in binary form must reproduce the above copyright notice,
14 * this list of conditions and the following disclaimer in the documentation
15 * and/or other materials provided with the distribution.
16 *
17 *3. Neither the name of the copyright holder nor the names of its
18 * contributors may be used to endorse or promote products derived from
19 * this software without specific prior written permission.
20 *
21 *THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 *AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 *IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24 *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
25 *FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26 *DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27 *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 *CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 *OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 ******************************************************************************/
32
33#pragma once
34#include <iostream>
35#include <set>
36#include <utility>
37#include "CameraProperties.h"
38#include "ErrorStatus.h"
39#include "CommonTypes.h"
40#include "Camera.h"
41
42inline void printCameraInfo(const mmind::eye::CameraInfo& cameraInfo)
43{
44 std::cout << "............................." << std::endl;
45 std::cout << "Camera Model Name: " << cameraInfo.model << std::endl;
46 std::cout << "Camera Serial Number: " << cameraInfo.serialNumber << std::endl;
47 std::cout << "Camera IP Address: " << cameraInfo.ipAddress << std::endl;
48 std::cout << "Camera Subnet Mask: " << cameraInfo.subnetMask << std::endl;
49 std::cout << "Camera IP Assignment Method: "
50 << mmind::eye::ipAssignmentMethodToString(cameraInfo.ipAssignmentMethod) << std::endl;
51 std::cout << "Hardware Version: "
52 << "V" << cameraInfo.hardwareVersion.toString() << std::endl;
53 std::cout << "Firmware Version: "
54 << "V" << cameraInfo.firmwareVersion.toString() << std::endl;
55 std::cout << "............................." << std::endl;
56 std::cout << std::endl;
57}
58
59inline void printCameraStatus(const mmind::eye::CameraStatus& cameraStatus)
60{
61 std::cout << ".....Camera Temperature....." << std::endl;
62 std::cout << "CPU : " << cameraStatus.temperature.cpuTemperature << "°C"
63 << std::endl;
64 std::cout << "Projector Module: " << cameraStatus.temperature.projectorTemperature << "°C"
65 << std::endl;
66 std::cout << "............................" << std::endl;
67
68 std::cout << std::endl;
69}
70
71inline void printCameraResolutions(const mmind::eye::CameraResolutions& cameraResolutions)
72
73{
74 std::cout << "Texture Map size : (width : " << cameraResolutions.texture.width
75 << ", height : " << cameraResolutions.texture.height << ")." << std::endl;
76 std::cout << "Depth Map size : (width : " << cameraResolutions.depth.width
77 << ", height : " << cameraResolutions.depth.height << ")." << std::endl;
78}
79
80inline void printCameraMatrix(const std::string& title,
81 const mmind::eye::CameraMatrix& cameraMatrix)
82{
83 std::cout << title << ": " << std::endl
84 << " [" << cameraMatrix.fx << ", " << 0 << ", " << cameraMatrix.cx << "]"
85
86 << std::endl
87 << " [" << 0 << ", " << cameraMatrix.fy << ", " << cameraMatrix.cy << "]"
88
89 << std::endl
90 << " [" << 0 << ", " << 0 << ", " << 1 << "]" << std::endl;
91 std::cout << std::endl;
92}
93
94inline void printCameraDistCoeffs(const std::string& title,
95 const mmind::eye::CameraDistortion& distCoeffs)
96{
97 std::cout << title << ": " << std::endl
98 << " k1: " << distCoeffs.k1 << ", k2: " << distCoeffs.k2
99 << ", p1: " << distCoeffs.p1 << ", p2: " << distCoeffs.p2 << ", k3: " << distCoeffs.k3
100 << std::endl;
101 std::cout << std::endl;
102}
103
104inline void printTransform(const std::string& title, const mmind::eye::Transformation& transform)
105{
106 std::cout << "Rotation: " << title << ": " << std::endl;
107 for (int i = 0; i < 3; i++) {
108 std::cout << " [";
109 for (int j = 0; j < 3; j++) {
110 std::cout << transform.rotation[i][j];
111 if (j != 2)
112 std::cout << ", ";
113 }
114 std::cout << "]" << std::endl;
115 }
116 std::cout << std::endl;
117 std::cout << "Translation " << title << ": " << std::endl;
118 std::cout << " X: " << transform.translation[0] << "mm, Y: " << transform.translation[1]
119 << "mm, Z: " << transform.translation[2] << "mm" << std::endl;
120 std::cout << std::endl;
121}
122
123inline void printCameraIntrinsics(const mmind::eye::CameraIntrinsics& intrinsics)
124{
125 printCameraMatrix("Texture Camera Matrix", intrinsics.texture.cameraMatrix);
126 printCameraDistCoeffs("Texture Camera Distortion Coefficients",
127 intrinsics.texture.cameraDistortion);
128
129 printCameraMatrix("Depth Camera Matrix", intrinsics.depth.cameraMatrix);
130 printCameraDistCoeffs("Depth Camera Distortion Coefficients",
131 intrinsics.depth.cameraDistortion);
132
133 printTransform("from Depth Camera to Texture Camera", intrinsics.depthToTexture);
134}
135
136inline bool findAndConnect(mmind::eye::Camera& device)
137{
138 std::cout << "Find Mech-Eye Industrial 3D Cameras..." << std::endl;
139 std::vector<mmind::eye::CameraInfo> deviceInfoList = mmind::eye::Camera::discoverCameras();
140
141 if (deviceInfoList.empty()) {
142 std::cout << "No Mech-Eye Industrial 3D Cameras found." << std::endl;
143 return false;
144 }
145
146 for (int i = 0; i < deviceInfoList.size(); i++) {
147 std::cout << "Mech-Eye device index : " << i << std::endl;
148 printCameraInfo(deviceInfoList[i]);
149 }
150
151 std::cout << "Please enter the device index you want to connect: ";
152 unsigned inputIndex = 0;
153
154 while (true) {
155 std::string str;
156 std::cin >> str;
157 if (std::all_of(str.begin(), str.end(), ::isdigit) &&
158 atoi(str.c_str()) < deviceInfoList.size()) {
159 inputIndex = atoi(str.c_str());
160 break;
161 }
162 std::cout << "Input invalid. Please enter the device index you want to connect: ";
163 }
164
166 status = device.connect(deviceInfoList[inputIndex]);
167
168 if (!status.isOK()) {
169 showError(status);
170 return false;
171 }
172
173 std::cout << "Connect Mech-Eye Industrial 3D Camera Successfully." << std::endl;
174 return true;
175}
176
177inline std::vector<mmind::eye::Camera> findAndConnectMultiCamera()
178{
179 std::cout << "Find Mech-Eye Industrial 3D Cameras..." << std::endl;
180 std::vector<mmind::eye::CameraInfo> cameraInfoList = mmind::eye::Camera::discoverCameras();
181
182 if (cameraInfoList.empty()) {
183 std::cout << "No Mech-Eye Industrial 3D Cameras found." << std::endl;
184 return {};
185 }
186
187 for (int i = 0; i < cameraInfoList.size(); i++) {
188 std::cout << "Mech-Eye device index : " << i << std::endl;
189 printCameraInfo(cameraInfoList[i]);
190 }
191
192 std::string str;
193 std::set<unsigned> indices;
194
195 while (true) {
196 std::cout << "Please enter the device index you want to connect: " << std::endl;
197 std::cout << "Enter the character 'c' to terminate adding devices" << std::endl;
198
199 std::cin >> str;
200 if (str == "c")
201 break;
202 if (std::all_of(str.begin(), str.end(), ::isdigit) &&
203 atoi(str.c_str()) < cameraInfoList.size())
204 indices.insert(atoi(str.c_str()));
205 else
206 std::cout << "Input invalid. Please enter the device index you want to connect: ";
207 }
208
209 std::vector<mmind::eye::Camera> cameraList(indices.size());
210
211 auto iter = indices.cbegin();
212 for (int i = 0; i < indices.size(); ++i, ++iter) {
213 showError(cameraList[i].connect(cameraInfoList[*iter]));
214 }
215
216 return cameraList;
217}
218
219inline bool confirmCapture3D()
220{
221 std::cout << "Do you want the camera to capture 3D image ? Please input y/n to confirm: "
222 << std::endl;
223 while (true) {
224 std::string confirmStr;
225 std::cin >> confirmStr;
226 if (confirmStr == "y") {
227 return true;
228 } else if (confirmStr == "n") {
229 std::cout << "program ends!" << std::endl;
230 return false;
231 } else {
232 std::cout << "Please input y/n again!" << std::endl;
233 }
234 }
235}
Operates the Mech-Eye Industrial 3D Cameras. Use Camera::connect to connect an available camera,...
Definition Camera.h:55
static std::vector< CameraInfo > discoverCameras()
Discovers all available cameras, and returns the camera information list for them....
ErrorStatus connect(const CameraInfo &info, unsigned int timeoutMs=5000)
Connects to a camera via CameraInfo.
std::string toString() const
Converts a Version object to a string of Version.
Definition Version.h:71
Describes the distortion parameters.
double k1
Radial distortion coefficients.
double k3
Radial distortion coefficients.
double p2
Tangential distortion coefficients.
double k2
Radial distortion coefficients.
double p1
Tangential distortion coefficients.
Defines the camera information.
IpAssignmentMethod ipAssignmentMethod
The IP assignment method of the device.
std::string serialNumber
Device serial number.
Version hardwareVersion
The version of the hardware which is pre-determined from the factory.
std::string model
Device model name, such as Mech-Eye Nano.
Version firmwareVersion
The version of the firmware which can be upgraded.
std::string subnetMask
The IP subnet mask of the device.
std::string ipAddress
The IP address of the device.
Defines the 3D camera intrinsic, including the texture source camera, depth source camera and the tra...
Intrinsics2DCamera texture
The intrinsic parameters of the camera for capturing the texture image.
Intrinsics2DCamera depth
The intrinsic parameters of the camera for capturing the depth map.
Describes the camera intrinsic matrix.
double cx
Principal point.
double fy
Focal lengths.
double cy
Principal point.
double fx
Focal lengths.
Defines the camera resolutions information, including texture image resolution and depth map resoluti...
Describes the camera running status.
float projectorTemperature
Projector module temperature in degrees Celsius.
float cpuTemperature
CPU temperature in the device motherboard in degrees Celsius.
Describes the types of errors.
Definition ErrorStatus.h:12
bool isOK() const
Returns true if the operation succeeds.
Definition ErrorStatus.h:73
Defines the rigid body transformations, including rotation matrix and translation vector.
double rotation[3][3]
3*3 rotation matrix.
double translation[3]
3*1 translation vector in [x(mm), y(mm), z(mm)].