|
|
@@ -1,8 +1,9 @@
|
|
|
#include "orb_camera_impl.h"
|
|
|
-#include "core/image_utility.hpp"
|
|
|
-#include "core/image_utility_v2.h"
|
|
|
+#include "codec/image_codec.h"
|
|
|
#include "core/object_manager.h"
|
|
|
#include "core/utility.hpp"
|
|
|
+#include "image_process/process_funcs.h"
|
|
|
+#include "third_party/static_block.hpp"
|
|
|
|
|
|
#include <boost/asio/post.hpp>
|
|
|
|
|
|
@@ -13,6 +14,8 @@
|
|
|
#include <glm/glm.hpp>
|
|
|
#include <glm/gtc/type_ptr.hpp>
|
|
|
|
|
|
+#include <xxhash.h>
|
|
|
+
|
|
|
using boost::asio::post;
|
|
|
|
|
|
namespace orb_camera_impl {
|
|
|
@@ -37,9 +40,29 @@ namespace orb_camera_impl {
|
|
|
RET_ERROR_P;
|
|
|
}
|
|
|
|
|
|
- // convert uncompressed video frame to image_xxx
|
|
|
+ int ob_fmt_to_cv_type(OBFormat fmt) {
|
|
|
+ switch (fmt) {
|
|
|
+ // @formatter:off
|
|
|
+ case OB_FORMAT_RGB: return CV_8UC3;
|
|
|
+ case OB_FORMAT_Y16: return CV_16FC1;
|
|
|
+ // @formatter:on
|
|
|
+ default: {
|
|
|
+ RET_ERROR_E;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // convert uncompressed video frame to image_ptr
|
|
|
template<typename T>
|
|
|
auto video_uc_frame_to_image(const video_frame_type &frame) {
|
|
|
+// auto img_size = cv::Size(frame->width(), frame->height());
|
|
|
+// auto img = create_image(img_size, ob_fmt_to_cv_type(frame->format()));
|
|
|
+// assert(img->size_in_bytes() == frame->dataSize());
|
|
|
+// auto img_mem = img->memory(MEM_HOST);
|
|
|
+// memcpy(img_mem.start_ptr(), frame->data(), img->size_in_bytes());
|
|
|
+// img_mem.modified();
|
|
|
+// return img;
|
|
|
+
|
|
|
auto info = image_info_type<T>();
|
|
|
info.ptr = std::shared_ptr<T>( // extend frame's lifetime
|
|
|
(T *) frame->data(), [pf = frame](void *) {});
|
|
|
@@ -51,6 +74,14 @@ namespace orb_camera_impl {
|
|
|
}
|
|
|
|
|
|
auto mjpeg_frame_to_image(const video_frame_type &frame) { // TODO: accelerate with CUDA
|
|
|
+// assert(frame->format() == OB_FORMAT_MJPG);
|
|
|
+// auto img_data = cv::_InputArray((uchar *) frame->data(), frame->dataSize());
|
|
|
+// auto img_bgr = cv::imdecode(img_data, cv::IMREAD_UNCHANGED);
|
|
|
+// auto img_rgb = create_image(img_bgr.size(), CV_8UC3);
|
|
|
+// cv::cvtColor(img_bgr, img_rgb->cv_mat(), cv::COLOR_BGR2RGB);
|
|
|
+// img_rgb->host_modified();
|
|
|
+// return img_rgb;
|
|
|
+
|
|
|
assert(frame->format() == OB_FORMAT_MJPG);
|
|
|
auto img_data = cv::_InputArray((uchar *) frame->data(), frame->dataSize());
|
|
|
auto img_bgr = cv::imdecode(img_data, cv::IMREAD_UNCHANGED);
|
|
|
@@ -60,33 +91,98 @@ namespace orb_camera_impl {
|
|
|
}
|
|
|
|
|
|
image_f32c1 depth_y16_to_mm(const image_u16c1 &y16, float scale) { // TODO: accelerate with CUDA
|
|
|
+// auto f32_img = create_image(y16->size(), CV_32FC1);
|
|
|
+// y16->cv_mat().convertTo(f32_img->cv_mat(), CV_32FC1, scale);
|
|
|
+// f32_img->host_modified();
|
|
|
+// return f32_img;
|
|
|
+
|
|
|
auto y16_info = y16->as_host_info();
|
|
|
auto f32_info = create_image_info<float1>(y16_info.size, MEM_HOST);
|
|
|
y16->as_mat().convertTo(f32_info.as_mat(), CV_32FC1, scale);
|
|
|
return create_image(f32_info);
|
|
|
}
|
|
|
|
|
|
- pc_ptr pc_frame_to_pc(const frame_type &frame) {
|
|
|
- assert(frame->format() == OB_FORMAT_RGB_POINT);
|
|
|
- auto size = frame->dataSize() / sizeof(OBColorPoint);
|
|
|
- auto pc = create_pc(size, PC_XYZ_RGB);
|
|
|
- auto pc_mem = pc->memory(MEM_HOST);
|
|
|
- auto start_ptr = (pc_xyz_rgb_type *) pc_mem.start_ptr();
|
|
|
- auto ptr = start_ptr;
|
|
|
- for (auto k = 0; k < size; ++k) { // TODO: do conversion in CUDA
|
|
|
- auto ob_ptr = (OBColorPoint *) frame->data() + k;
|
|
|
- if (ob_ptr->z == 0) continue;
|
|
|
- ptr->x = ob_ptr->x;
|
|
|
- ptr->y = ob_ptr->y;
|
|
|
- ptr->z = ob_ptr->z;
|
|
|
- ptr->r = ob_ptr->r;
|
|
|
- ptr->g = ob_ptr->g;
|
|
|
- ptr->b = ob_ptr->b;
|
|
|
- ++ptr;
|
|
|
+ data_type remap_codec::encode(const image_ptr &img, bool force_idr) {
|
|
|
+ if (force_idr) {
|
|
|
+ cache.clear();
|
|
|
}
|
|
|
- pc->shrink(ptr - start_ptr);
|
|
|
- pc_mem.modified();
|
|
|
- return pc;
|
|
|
+
|
|
|
+ auto writer = network_writer();
|
|
|
+ auto info = img->get_meta_ext<remap_info>(META_ORB_REMAP_INFO);
|
|
|
+ info.write_to(writer);
|
|
|
+ return writer.current_data();
|
|
|
+ }
|
|
|
+
|
|
|
+ image_ptr remap_codec::decode(const data_type &data) {
|
|
|
+ auto key_id = XXH64(data.start_ptr(), data.size, hash_seed);
|
|
|
+ if (auto iter = cache.find(key_id); iter != cache.end()) {
|
|
|
+ return iter->second;
|
|
|
+ }
|
|
|
+
|
|
|
+ auto info = remap_info();
|
|
|
+ auto reader = network_reader(data);
|
|
|
+ info.fill_from(reader);
|
|
|
+ assert(reader.empty());
|
|
|
+
|
|
|
+ // @formatter:off
|
|
|
+ cv::Mat cam_mat_cv = (cv::Mat_<float>(3, 3) <<
|
|
|
+ info.fx, 0.f, info.cx,
|
|
|
+ 0, info.fy, info.cy,
|
|
|
+ 0, 0, 1);
|
|
|
+ cv::Mat dist_cv = cv::Mat_<float>(info.k);
|
|
|
+ // @formatter:on
|
|
|
+
|
|
|
+ auto img_size = cv::Size(info.width, info.height);
|
|
|
+ auto points_size = cv::Size(img_size.area(), 1);
|
|
|
+ auto points_mat = cv::Mat(points_size, CV_32FC2);
|
|
|
+ for (auto iy = 0; iy < info.height; ++iy)
|
|
|
+ for (auto ix = 0; ix < info.width; ++ix) {
|
|
|
+ auto id = iy * info.width + ix;
|
|
|
+ points_mat.at<cv::Vec2f>(id) = cv::Vec2f(ix, iy);
|
|
|
+ }
|
|
|
+
|
|
|
+ auto undistort_img = create_image(img_size, CV_32FC2);
|
|
|
+ auto img_data = undistort_img->memory(MEM_HOST).start_ptr();
|
|
|
+ auto undistort_mat = cv::Mat(points_size, CV_32FC2, img_data);
|
|
|
+ cv::undistortPoints(points_mat, undistort_mat, cam_mat_cv, dist_cv);
|
|
|
+ undistort_img->host_modified();
|
|
|
+ undistort_img->set_meta_any(META_ORB_REMAP_INFO, info);
|
|
|
+ undistort_img->set_meta_any(META_IMAGE_SPECIAL_CODEC, REMAP_CODEC_NAME);
|
|
|
+
|
|
|
+ assert(!cache.contains(key_id));
|
|
|
+ cache.emplace(key_id, undistort_img);
|
|
|
+ return undistort_img;
|
|
|
+ }
|
|
|
+
|
|
|
+ image_ptr remap_codec::decode(const OBCameraParam ¶m) { // TODO: accelerate with CUDA
|
|
|
+ auto info = remap_info();
|
|
|
+
|
|
|
+ auto &cam_in = param.rgbIntrinsic;
|
|
|
+ // @formatter:off
|
|
|
+ info.fx = cam_in.fx; info.fy = cam_in.fy;
|
|
|
+ info.cx = cam_in.cx; info.cy = cam_in.cy;
|
|
|
+ info.width = cam_in.width; info.height = cam_in.height;
|
|
|
+ // @formatter:on
|
|
|
+
|
|
|
+ auto &dist = param.rgbDistortion;
|
|
|
+ info.k = {dist.k1, dist.k2, dist.p1, dist.p2,
|
|
|
+ dist.k3, dist.k4, dist.k5, dist.k6,};
|
|
|
+
|
|
|
+ auto writer = network_writer();
|
|
|
+ info.write_to(writer);
|
|
|
+ return decode(writer.current_data());
|
|
|
+ }
|
|
|
+
|
|
|
+ remap_codec re_codec;
|
|
|
+
|
|
|
+ static_block {
|
|
|
+ auto enc_func = [](const image_ptr &img, bool force_idr) {
|
|
|
+ return re_codec.encode(img, force_idr);
|
|
|
+ };
|
|
|
+ auto dec_func = [](const data_type &data) {
|
|
|
+ return re_codec.decode(data);
|
|
|
+ };
|
|
|
+ register_image_special_codec(REMAP_CODEC_NAME, enc_func, dec_func);
|
|
|
}
|
|
|
|
|
|
}
|
|
|
@@ -116,8 +212,6 @@ orb_camera::impl::~impl() {
|
|
|
if (is_capturing) {
|
|
|
stop();
|
|
|
}
|
|
|
-
|
|
|
- aux_thread->join();
|
|
|
}
|
|
|
|
|
|
orb_camera::vi_list_ptr orb_camera::impl::create_video_info_list(const pf_list_type &pf_list) {
|
|
|
@@ -164,42 +258,11 @@ orb_camera::vi_list_ptr orb_camera::impl::query_d2c_info(uint32_t c_conf_index)
|
|
|
return create_video_info_list(d_pf);
|
|
|
}
|
|
|
|
|
|
-void orb_camera::impl::generate_undistort_map() { // TODO: accelerate with CUDA
|
|
|
+void orb_camera::impl::generate_undistort_map() {
|
|
|
if (remap_name == invalid_obj_name) return;
|
|
|
-
|
|
|
auto cam_params = pipe->getCameraParam();
|
|
|
-// assert(cam_params.rgbIntrinsic == cam_params.depthIntrinsic);
|
|
|
-// assert(cam_params.rgbDistortion == cam_params.depthDistortion);
|
|
|
- auto cam_in = cam_params.rgbIntrinsic;
|
|
|
- // @formatter:off
|
|
|
- cv::Mat cam_mat_cv = (cv::Mat_<float>(3, 3) <<
|
|
|
- cam_in.fx, 0.f, cam_in.cx,
|
|
|
- 0, cam_in.fy, cam_in.cy,
|
|
|
- 0, 0, 1);
|
|
|
- // @formatter:on
|
|
|
-
|
|
|
- auto cam_dist = cam_params.rgbDistortion;
|
|
|
- // @formatter:off
|
|
|
- cv::Mat dist_cv = (cv::Mat_<float>(8, 1) <<
|
|
|
- cam_dist.k1, cam_dist.k2, cam_dist.p1, cam_dist.p2,
|
|
|
- cam_dist.k3, cam_dist.k4, cam_dist.k5, cam_dist.k6);
|
|
|
- // @formatter:on
|
|
|
-
|
|
|
- auto img_size = cv::Size(cam_in.width, cam_in.height);
|
|
|
- auto points_size = cv::Size(img_size.area(), 1);
|
|
|
- auto points_mat = cv::Mat(points_size, CV_32FC2);
|
|
|
- for (auto iy = 0; iy < cam_in.height; ++iy)
|
|
|
- for (auto ix = 0; ix < cam_in.width; ++ix) {
|
|
|
- auto id = iy * cam_in.width + ix;
|
|
|
- points_mat.at<cv::Vec2f>(id) = cv::Vec2f(ix, iy);
|
|
|
- }
|
|
|
-
|
|
|
- auto undistort_img = create_image(img_size, CV_32FC2);
|
|
|
- auto data = undistort_img->memory(MEM_HOST).start_ptr();
|
|
|
- auto undistort_mat = cv::Mat(points_size, CV_32FC2, data);
|
|
|
- cv::undistortPoints(points_mat, undistort_mat, cam_mat_cv, dist_cv);
|
|
|
- undistort_img->host_modified();
|
|
|
- OBJ_SAVE(remap_name, undistort_img);
|
|
|
+ remap_img = re_codec.decode(cam_params);
|
|
|
+ OBJ_SAVE(remap_name, remap_img);
|
|
|
}
|
|
|
|
|
|
bool orb_camera::impl::start(start_config conf) {
|
|
|
@@ -212,6 +275,10 @@ bool orb_camera::impl::start(start_config conf) {
|
|
|
c_prof = c_pf_list->getProfile(conf.color.config_index);
|
|
|
ob_conf->enableStream(c_prof);
|
|
|
c_name = conf.color.name;
|
|
|
+
|
|
|
+ // notify frame rate
|
|
|
+ auto c_fmt = c_prof->as<ob::VideoStreamProfile>();
|
|
|
+ OBJ_PIN_META(c_name, META_REFRESH_RATE, (size_t) c_fmt->fps());
|
|
|
}
|
|
|
|
|
|
auto d_prof = std::shared_ptr<ob::StreamProfile>();
|
|
|
@@ -233,20 +300,7 @@ bool orb_camera::impl::start(start_config conf) {
|
|
|
|
|
|
pipe->start(ob_conf, [this](auto frames) { frames_callback(frames); });
|
|
|
is_capturing = true;
|
|
|
-
|
|
|
- // Example says that the filter depends on the started pipeline.
|
|
|
- if (conf.pc.enable) {
|
|
|
- pc_filter = std::make_shared<ob::PointCloudFilter>();
|
|
|
- auto params = pipe->getCameraParam();
|
|
|
- pc_filter->setCameraParam(pipe->getCameraParam());
|
|
|
- pc_filter->setCreatePointFormat(OB_FORMAT_RGB_POINT);
|
|
|
- pc_filter->setCallBack([this](auto frame) { pc_callback(frame); });
|
|
|
- pc_name = conf.pc.name;
|
|
|
- }
|
|
|
-
|
|
|
- // open a new thread to do the time-consuming calculation work
|
|
|
- aux_thread = std::make_unique<std::jthread>(
|
|
|
- [this] { generate_undistort_map(); });
|
|
|
+ generate_undistort_map();
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
@@ -272,28 +326,31 @@ void orb_camera::impl::frames_callback(const frames_type &frames) {
|
|
|
d_img = depth_y16_to_mm(y16_img, d_frame->getValueScale());
|
|
|
}
|
|
|
|
|
|
- if (pc_filter != nullptr &&
|
|
|
+ auto pc_out = pc_ptr();
|
|
|
+ if (pc_name != invalid_obj_name &&
|
|
|
c_img != nullptr && d_img != nullptr) { // depth images may not be ready
|
|
|
- pc_filter->setPositionDataScaled(frames->depthFrame()->getValueScale());
|
|
|
- pc_filter->pushFrame(frames);
|
|
|
+ auto d_conf = gen_pc_rgbd::config_direct{
|
|
|
+ .color_img = create_image(c_img),
|
|
|
+ .depth_img = create_image(d_img),
|
|
|
+ .remap_img = remap_img,
|
|
|
+ .pc_out = &pc_out,
|
|
|
+ .stream = stream,
|
|
|
+ };
|
|
|
+ gen_pc_rgbd::call_direct(d_conf);
|
|
|
+ assert(pc_out != nullptr);
|
|
|
}
|
|
|
|
|
|
- post(*ctx, [=, _c_name = c_name, _d_name = d_name] {
|
|
|
- if (c_img != nullptr) { OBJ_SAVE(_c_name, c_img); }
|
|
|
- if (d_img != nullptr) { OBJ_SAVE(_d_name, d_img); }
|
|
|
+ post(*ctx, [=, this] {
|
|
|
+ if (c_img != nullptr) { OBJ_SAVE(c_name, c_img); }
|
|
|
+ if (d_img != nullptr) { OBJ_SAVE(d_name, d_img); }
|
|
|
+ if (pc_out != nullptr) { OBJ_SAVE(pc_name, pc_out); }
|
|
|
});
|
|
|
}
|
|
|
|
|
|
-void orb_camera::impl::pc_callback(const frame_type &frame) {
|
|
|
- auto pc = pc_frame_to_pc(frame);
|
|
|
- OBJ_SAVE(pc_name, pc);
|
|
|
-}
|
|
|
-
|
|
|
void orb_camera::impl::stop() {
|
|
|
assert(is_capturing);
|
|
|
pipe->stop();
|
|
|
is_capturing = false;
|
|
|
- pc_filter = nullptr;
|
|
|
}
|
|
|
|
|
|
orb_camera::pointer orb_camera::create(create_config conf) {
|