247 lines
9.0 KiB
C++
247 lines
9.0 KiB
C++
//
|
|
// Created by xtkuang on 2025/6/4.
|
|
//
|
|
|
|
#include "mechmind_camera.h"
|
|
|
|
using namespace std;
|
|
using namespace cmvr::device;
|
|
using namespace mmind::eye;
|
|
|
|
MechmindCamera::MechmindCamera(const XmlNode& config) : AbstractCamera(config) {
|
|
try {
|
|
ip_ = config.getAttrString("ip");
|
|
if (ip_.empty()) {
|
|
state_.is_error = true;
|
|
state_.error_message = "ip attribute not set";
|
|
LOG(ERROR) << "[MechmindCamera] (MechmindCamera): ip attribute not set";
|
|
throw std::runtime_error("[MechmindCamera] (MechmindCamera): ip attribute not set");
|
|
}
|
|
|
|
align_ = cfg_.getAttrDefault("align", true);
|
|
|
|
const string temp = cfg_.getAttrDefault("_2dtype", "color");
|
|
if (temp == "color") {
|
|
image2d_type_ = COLOR;
|
|
}
|
|
else if (temp == "gray") {
|
|
image2d_type_ = GRAY;
|
|
}
|
|
}
|
|
catch (const exception& e) {
|
|
const string error_msg = "[MechmindCamera] (start): " + string(e.what());
|
|
LOG(ERROR) << error_msg;
|
|
throw std::runtime_error(error_msg);
|
|
}
|
|
|
|
}
|
|
|
|
void MechmindCamera::init() {
|
|
std::lock_guard lock(dev_mtx_);
|
|
clear_error_();
|
|
state_.is_initialized = false;
|
|
state_.is_initialized = true;
|
|
}
|
|
|
|
void MechmindCamera::start() {
|
|
try {
|
|
std::lock_guard lock(dev_mtx_);
|
|
clear_error_();
|
|
auto status = camera_.connect(ip_);
|
|
if (status.isOK()) {
|
|
state_.is_opened = true;
|
|
}
|
|
else {
|
|
const string error_msg = "start camera failed, error_code=" + to_string(status.errorCode);
|
|
LOG(ERROR) << "[MechmindCamera] (start): "<< error_msg;
|
|
throw std::runtime_error(error_msg);
|
|
}
|
|
}
|
|
catch (const std::exception& e) {
|
|
const string error_msg = "[MechmindCamera] (start): " + string(e.what());
|
|
LOG(ERROR) << error_msg;
|
|
throw std::runtime_error(error_msg);
|
|
}
|
|
}
|
|
|
|
void MechmindCamera::stop() {
|
|
try {
|
|
std::lock_guard lock(dev_mtx_);
|
|
clear_error_();
|
|
camera_.disconnect();
|
|
state_.is_opened = false;
|
|
}
|
|
catch (const exception &e) {
|
|
const string error_msg = "[MechmindCamera] (stop): " + string(e.what());
|
|
LOG(ERROR) << error_msg;
|
|
throw std::runtime_error(error_msg);
|
|
}
|
|
}
|
|
|
|
void MechmindCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
|
|
try {
|
|
std::lock_guard lock(dev_mtx_);
|
|
clear_error_();
|
|
if (!state_.is_initialized || !state_.is_opened) {
|
|
LOG(ERROR) << "[MechmindCamera] (getRGBImage): camera not opened";
|
|
throw runtime_error("[MechmindCamera] (getRGBImage): camera not opened");
|
|
}
|
|
|
|
if (image2d_type_ != COLOR) {
|
|
LOG(ERROR) << "[MechmindCamera] (getRGBImage): image 2d type not color";
|
|
throw runtime_error("[MechmindCamera] (getRGBImage): image 2d type not color");
|
|
}
|
|
|
|
Frame2D frame2D;
|
|
auto ret = camera_.capture2D(frame2D);
|
|
if (!ret.isOK()) {
|
|
const string error_msg = "get color image failed, error_code=" + to_string(ret.errorCode);
|
|
LOG(ERROR) << "[MechmindCamera] (getRGBImage): "<< error_msg;
|
|
throw std::runtime_error(error_msg);
|
|
}
|
|
switch (frame2D.colorType()) {
|
|
case ColorTypeOf2DCamera::Monochrome:
|
|
{
|
|
GrayScale2DImage grayImage = frame2D.getGrayScaleImage();
|
|
color = cv::Mat(grayImage.height(), grayImage.width(), CV_8UC1, grayImage.data());
|
|
break;
|
|
}
|
|
case ColorTypeOf2DCamera::Color:
|
|
{
|
|
Color2DImage colorImage = frame2D.getColorImage();
|
|
color = cv::Mat(colorImage.height(), colorImage.width(), CV_8UC3, colorImage.data());
|
|
break;
|
|
}
|
|
case ColorTypeOf2DCamera::Undefined:
|
|
break;
|
|
}
|
|
}
|
|
catch (const exception &e) {
|
|
const string error_msg = "[MechmindCamera] (getRGBImage): " + string(e.what());
|
|
LOG(ERROR) << error_msg;
|
|
throw std::runtime_error(error_msg);
|
|
}
|
|
}
|
|
|
|
void MechmindCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
|
|
try {
|
|
std::lock_guard lock(dev_mtx_);
|
|
clear_error_();
|
|
if (!state_.is_initialized || !state_.is_opened) {
|
|
LOG(ERROR) << "[MechmindCamera] (getDepthImage): camera not opened";
|
|
throw runtime_error("[MechmindCamera] (getDepthImage): camera not opened");
|
|
}
|
|
|
|
Frame3D frame3D;
|
|
auto ret = camera_.capture3D(frame3D);
|
|
if (!ret.isOK()) {
|
|
const string error_msg = "get depth image failed, error_code=" + to_string(ret.errorCode);
|
|
LOG(ERROR) << "[MechmindCamera] (getDepthImage): "<< error_msg;
|
|
throw std::runtime_error(error_msg);
|
|
}
|
|
DepthMap depthMap = frame3D.getDepthMap();
|
|
depth = cv::Mat(depthMap.height(), depthMap.width(), CV_32FC1, depthMap.data());
|
|
}
|
|
catch (const exception &e) {
|
|
const string error_msg = "[MechmindCamera] (getDepthImage): " + string(e.what());
|
|
LOG(ERROR) << error_msg;
|
|
throw std::runtime_error(error_msg);
|
|
}
|
|
}
|
|
|
|
void MechmindCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) {
|
|
try {
|
|
std::lock_guard lock(dev_mtx_);
|
|
clear_error_();
|
|
if (!state_.is_initialized || !state_.is_opened) {
|
|
LOG(ERROR) << "[MechmindCamera] (getDepthImage): camera not opened";
|
|
throw runtime_error("[MechmindCamera] (getDepthImage): camera not opened");
|
|
}
|
|
|
|
Frame2D frame_2d;
|
|
Frame3D frame_3d;
|
|
auto ret = camera_.capture2D(frame_2d);
|
|
if (!ret.isOK()) {
|
|
const string error_msg = "get color failed, error_code=" + to_string(ret.errorCode);
|
|
LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg;
|
|
throw std::runtime_error(error_msg);
|
|
}
|
|
Color2DImage color_image = frame_2d.getColorImage();
|
|
|
|
ret = camera_.capture3D(frame_3d);
|
|
if (!ret.isOK()) {
|
|
const string error_msg = "get depth failed, error_code=" + to_string(ret.errorCode);
|
|
LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg;
|
|
throw std::runtime_error(error_msg);
|
|
}
|
|
DepthMap depth_map = frame_3d.getDepthMap();
|
|
|
|
CameraIntrinsics intrinsics;
|
|
showError(camera_.getCameraIntrinsics(intrinsics));
|
|
|
|
GrayScale2DImage mask;
|
|
mask.resize(color_image.width(), color_image.height());
|
|
for (int y = 0; y < color_image.height(); ++y)
|
|
for (int x = 0; x < color_image.width(); ++x)
|
|
mask.at(y, x).gray = 255;
|
|
|
|
PointCloud pcl;
|
|
ret = getPointCloudAfterMapping(depth_map, mask, intrinsics, pcl);
|
|
if (!ret.isOK()) {
|
|
const string error_msg = "get pcl after mapping failed, error_code=" + to_string(ret.errorCode);
|
|
LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg;
|
|
throw std::runtime_error(error_msg);
|
|
}
|
|
|
|
TexturedPointCloud textured_pcl;
|
|
ret = getPointCloudAfterMapping(depth_map, mask, color_image, intrinsics, textured_pcl);
|
|
if (!ret.isOK()) {
|
|
const string error_msg = "get pcl after mapping failed, error_code=" + to_string(ret.errorCode);
|
|
LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg;
|
|
throw std::runtime_error(error_msg);
|
|
}
|
|
|
|
textured_plc_to_rgbd_(textured_pcl, color, depth);
|
|
}
|
|
catch (const exception &e) {
|
|
const string error_msg = "[MechmindCamera] (getRGBDImages): " + string(e.what());
|
|
LOG(ERROR) << error_msg;
|
|
throw std::runtime_error(error_msg);
|
|
}
|
|
|
|
}
|
|
|
|
void MechmindCamera::updateParams(const std::pair<std::string, std::string>& param)
|
|
{
|
|
//mmind::eye::UserSet currentUserSet = camera_.currentUserSet();
|
|
//std::vector<Parameter*> parameters = currentUserSet.getAvailableParameters();
|
|
throw runtime_error("[MechmindCamera] (startRecording): Not implemented");
|
|
}
|
|
|
|
void MechmindCamera::startRecording(const std::string& video_path) {
|
|
throw runtime_error("[MechmindCamera] (startRecording): Not implemented");
|
|
}
|
|
|
|
void MechmindCamera::stopRecording() {
|
|
throw runtime_error("[MechmindCamera] (startRecording): Not implemented");
|
|
}
|
|
|
|
void MechmindCamera::textured_plc_to_rgbd_(const TexturedPointCloud& cloud, cv::Mat& color, cv::Mat& depth) {
|
|
const int width = cloud.width();
|
|
const int height = cloud.height();
|
|
color = cv::Mat(height, width, CV_8UC3);
|
|
depth = cv::Mat(height, width, CV_32FC1);
|
|
for (int y = 0; y < height; ++y) {
|
|
for (int x = 0; x < width; ++x) {
|
|
const PointXYZBGR& pt = cloud.at(y, x);
|
|
if (std::isnan(pt.z)) {
|
|
depth.at<float>(y, x) = 0.0f;
|
|
color.at<cv::Vec3b>(y, x) = {0, 0, 0}; // black for invalid
|
|
} else {
|
|
depth.at<float>(y, x) = pt.z / 1000.0f; // mm to meters
|
|
color.at<cv::Vec3b>(y, x) = {pt.b, pt.g, pt.r}; // OpenCV is BGR
|
|
}
|
|
}
|
|
}
|
|
}
|