api_util.h

/*******************************************************************************
 *BSD 3-Clause License
 *
 *Copyright (c) 2016-2024, Mech-Mind Robotics
 *All rights reserved.
 *
 *Redistribution and use in source and binary forms, with or without
 *modification, are permitted provided that the following conditions are met:
 *
 *1. Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 *
 *2. Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 *3. Neither the name of the copyright holder nor the names of its
 *   contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission.
 *
 *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 HOLDER 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.
 ******************************************************************************/
 
#pragma once
#include <iostream>
#include <set>
#include <utility>
#include <regex>
#include "CameraProperties.h"
#include "ErrorStatus.h"
#include "CommonTypes.h"
#include "Camera.h"
 
inline void printCameraInfo(const mmind::eye::CameraInfo& cameraInfo)
{
    std::cout << "............................." << std::endl;
    std::cout << "Camera Model Name:           " << cameraInfo.model << std::endl;
    std::cout << "Camera Serial Number:        " << cameraInfo.serialNumber << std::endl;
    std::cout << "Camera IP Address:           " << cameraInfo.ipAddress << std::endl;
    std::cout << "Camera Subnet Mask:          " << cameraInfo.subnetMask << std::endl;
    std::cout << "Camera IP Assignment Method: "
              << mmind::eye::ipAssignmentMethodToString(cameraInfo.ipAssignmentMethod) << std::endl;
    std::cout << "Hardware Version:            "
              << "V" << cameraInfo.hardwareVersion.toString() << std::endl;
    std::cout << "Firmware Version:            "
              << "V" << cameraInfo.firmwareVersion.toString() << std::endl;
    std::cout << "............................." << std::endl;
    std::cout << std::endl;
}
 
inline void printCameraStatus(const mmind::eye::CameraStatus& cameraStatus)
{
    std::cout << ".....Camera Temperature....." << std::endl;
    std::cout << "CPU :               " << cameraStatus.temperature.cpuTemperature << "°C"
              << std::endl;
    std::cout << "Projector Module:   " << cameraStatus.temperature.projectorTemperature << "°C"
              << std::endl;
    std::cout << "............................" << std::endl;
 
    std::cout << std::endl;
}
 
inline void printCameraResolutions(const mmind::eye::CameraResolutions& cameraResolutions)
 
{
    std::cout << "Texture Map size : (width : " << cameraResolutions.texture.width
              << ", height : " << cameraResolutions.texture.height << ")." << std::endl;
    std::cout << "Depth Map size : (width : " << cameraResolutions.depth.width
              << ", height : " << cameraResolutions.depth.height << ")." << std::endl;
}
 
inline void printCameraMatrix(const std::string& title,
                              const mmind::eye::CameraMatrix& cameraMatrix)
{
    std::cout << title << ": " << std::endl
              << "    [" << cameraMatrix.fx << ", " << 0 << ", " << cameraMatrix.cx << "]"
 
              << std::endl
              << "    [" << 0 << ", " << cameraMatrix.fy << ", " << cameraMatrix.cy << "]"
 
              << std::endl
              << "    [" << 0 << ", " << 0 << ", " << 1 << "]" << std::endl;
    std::cout << std::endl;
}
 
inline void printCameraDistCoeffs(const std::string& title,
                                  const mmind::eye::CameraDistortion& distCoeffs)
{
    std::cout << title << ": " << std::endl
              << "    k1: " << distCoeffs.k1 << ", k2: " << distCoeffs.k2
              << ", p1: " << distCoeffs.p1 << ", p2: " << distCoeffs.p2 << ", k3: " << distCoeffs.k3
              << std::endl;
    std::cout << std::endl;
}
 
inline void printTransform(const std::string& title, const mmind::eye::Transformation& transform)
{
    std::cout << "Rotation: " << title << ": " << std::endl;
    for (int i = 0; i < 3; i++) {
        std::cout << "    [";
        for (int j = 0; j < 3; j++) {
            std::cout << transform.rotation[i][j];
            if (j != 2)
                std::cout << ", ";
        }
        std::cout << "]" << std::endl;
    }
    std::cout << std::endl;
    std::cout << "Translation " << title << ": " << std::endl;
    std::cout << "    X: " << transform.translation[0] << "mm, Y: " << transform.translation[1]
              << "mm, Z: " << transform.translation[2] << "mm" << std::endl;
    std::cout << std::endl;
}
 
inline void printCameraIntrinsics(const mmind::eye::CameraIntrinsics& intrinsics)
{
    printCameraMatrix("Texture Camera Matrix", intrinsics.texture.cameraMatrix);
    printCameraDistCoeffs("Texture Camera Distortion Coefficients",
                          intrinsics.texture.cameraDistortion);
 
    printCameraMatrix("Depth Camera Matrix", intrinsics.depth.cameraMatrix);
    printCameraDistCoeffs("Depth Camera Distortion Coefficients",
                          intrinsics.depth.cameraDistortion);
 
    printTransform("From Depth Camera to Texture Camera", intrinsics.depthToTexture);
}
 
inline bool findAndConnect(mmind::eye::Camera& device)
{
    std::cout << "Find Mech-Eye Industrial 3D Cameras..." << std::endl;
    std::vector<mmind::eye::CameraInfo> deviceInfoList = mmind::eye::Camera::discoverCameras();
 
    if (deviceInfoList.empty()) {
        std::cout << "No Mech-Eye Industrial 3D Cameras found." << std::endl;
        return false;
    }
 
    for (int i = 0; i < deviceInfoList.size(); i++) {
        std::cout << "Mech-Eye device index : " << i << std::endl;
        printCameraInfo(deviceInfoList[i]);
    }
 
    std::cout << "Please enter the device index you want to connect: ";
    unsigned inputIndex = 0;
 
    while (true) {
        std::string str;
        std::cin >> str;
        if (std::regex_match(str.begin(), str.end(), std::regex{"[0-9]+"}) &&
            atoi(str.c_str()) < deviceInfoList.size()) {
            inputIndex = atoi(str.c_str());
            break;
        }
        std::cout << "Input invalid. Please enter the device index you want to connect: ";
    }
 
    mmind::eye::ErrorStatus status;
    status = device.connect(deviceInfoList[inputIndex]);
 
    if (!status.isOK()) {
        showError(status);
        return false;
    }
 
    std::cout << "Connect Mech-Eye Industrial 3D Camera Successfully." << std::endl;
    return true;
}
 
inline std::vector<mmind::eye::Camera> findAndConnectMultiCamera()
{
    std::cout << "Find Mech-Eye Industrial 3D Cameras..." << std::endl;
    std::vector<mmind::eye::CameraInfo> cameraInfoList = mmind::eye::Camera::discoverCameras();
 
    if (cameraInfoList.empty()) {
        std::cout << "No Mech-Eye Industrial 3D Cameras found." << std::endl;
        return {};
    }
 
    for (int i = 0; i < cameraInfoList.size(); i++) {
        std::cout << "Mech-Eye device index : " << i << std::endl;
        printCameraInfo(cameraInfoList[i]);
    }
 
    std::string str;
    std::set<unsigned> indices;
 
    while (true) {
        std::cout << "Please enter the device index you want to connect: " << std::endl;
        std::cout << "Enter the character 'c' to terminate adding devices" << std::endl;
 
        std::cin >> str;
        if (str == "c")
            break;
        if (std::regex_match(str.begin(), str.end(), std::regex{"[0-9]+"}) &&
            atoi(str.c_str()) < cameraInfoList.size())
            indices.insert(atoi(str.c_str()));
        else
            std::cout << "Input invalid. Please enter the device index you want to connect: ";
    }
 
    std::vector<mmind::eye::Camera> cameraList{};
 
    auto iter = indices.cbegin();
    for (int i = 0; i < indices.size(); ++i, ++iter) {
        mmind::eye::Camera camera;
        auto status = camera.connect(cameraInfoList[*iter]);
        if (status.isOK())
            cameraList.push_back(camera);
        else
            showError(status);
    }
 
    return cameraList;
}
 
inline bool confirmCapture3D()
{
    std::cout << "Do you want the camera to capture 3D image ? Please input y/n to confirm: "
              << std::endl;
    while (true) {
        std::string confirmStr;
        std::cin >> confirmStr;
        if (confirmStr == "y") {
            return true;
        } else if (confirmStr == "n") {
            std::cout << "program ends!" << std::endl;
            return false;
        } else {
            std::cout << "Please input y/n again!" << std::endl;
        }
    }
}