|
|
@@ -6,131 +6,327 @@
|
|
|
#include "module_v5/transform_provider.h"
|
|
|
#include "module_v5/algorithms.h"
|
|
|
#include "third_party/scope_guard.hpp"
|
|
|
+#include "image_process_v5/sp_image.h"
|
|
|
+#include "image_process_v5/process_python.h"
|
|
|
+#include "image_process_v5/image_process.h"
|
|
|
|
|
|
// from sophiar
|
|
|
#include "core/local_connection.h"
|
|
|
|
|
|
#include <boost/asio/io_context.hpp>
|
|
|
#include <boost/asio/post.hpp>
|
|
|
+#include <boost/filesystem/operations.hpp>
|
|
|
+#include <utility>
|
|
|
|
|
|
namespace ba = boost::asio;
|
|
|
namespace bs = boost::signals2;
|
|
|
-using namespace std::placeholders;
|
|
|
+using namespace sophiar;
|
|
|
|
|
|
-extern ba::io_context *main_ctx;
|
|
|
+extern local_connection *g_sophiar_conn;
|
|
|
|
|
|
namespace {
|
|
|
- constexpr auto rotation_ball_distance_threshold = 50; // 50mm
|
|
|
-}
|
|
|
+ struct rotation_center_calib
|
|
|
+ : std::enable_shared_from_this<rotation_center_calib> {
|
|
|
+ obj_name_type image_name = {};
|
|
|
|
|
|
-struct oblique_calibrator::impl {
|
|
|
- create_config conf;
|
|
|
+ using result_type = rotate_registration_result_type;
|
|
|
|
|
|
- enum {
|
|
|
- IDLE,
|
|
|
- ROTATION_BALL
|
|
|
- } status = IDLE;
|
|
|
+ std::optional<glm::vec2> result;
|
|
|
+ std::vector<sp_image> calib_images;
|
|
|
+ std::atomic_size_t reg_tasks;
|
|
|
+ std::vector<glm::vec2> reg_results;
|
|
|
+
|
|
|
+ void reg_callback(const result_type ret, const size_t index) {
|
|
|
+ reg_results[index] = ret.center;
|
|
|
+ if (--reg_tasks == 0) {
|
|
|
+ auto sum = std::ranges::fold_left(reg_results, glm::vec2(), std::plus());
|
|
|
+ sum /= reg_results.size();
|
|
|
+ MAIN_DETACH([=, ptr = shared_from_this()] { ptr->result = sum; });
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ void start_calib() {
|
|
|
+ const auto num_samples = calib_images.size();
|
|
|
+ assert(calib_images.size() >= 2);
|
|
|
+ reg_tasks = num_samples * (num_samples - 1);
|
|
|
+ reg_results.resize(num_samples);
|
|
|
+ auto index = 0;
|
|
|
+ for (auto i = 0; i < num_samples; i++)
|
|
|
+ for (auto j = 0; j < num_samples; j++) {
|
|
|
+ image_rotate_registration(
|
|
|
+ calib_images[i], calib_images[j],
|
|
|
+ [ptr = shared_from_this(), _index = index++](auto ret) {
|
|
|
+ ptr->reg_callback(ret, _index);
|
|
|
+ });
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ void show_ui() {
|
|
|
+ const auto num_samples = calib_images.size();
|
|
|
+ const bool can_operate = num_samples >= 2;
|
|
|
+ if (ImGui::Button("Collect")) {
|
|
|
+ MAIN_DETACH([ptr = shared_from_this()] {
|
|
|
+ auto img = OBJ_QUERY(sp_image, ptr->image_name);
|
|
|
+ ptr->calib_images.emplace_back(img);
|
|
|
+ });
|
|
|
+ }
|
|
|
+ if (true) {
|
|
|
+ auto guard = imgui_valid_style_guard(can_operate);
|
|
|
+ ImGui::SameLine();
|
|
|
+ if (ImGui::Button("Calibrate")) {
|
|
|
+ TP_DETACH([ptr = shared_from_this()] { ptr->start_calib(); });
|
|
|
+ }
|
|
|
+ ImGui::Text("Image Samples: %ld", num_samples);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ };
|
|
|
+
|
|
|
+ struct rotation_ball_calib
|
|
|
+ : std::enable_shared_from_this<rotation_ball_calib> {
|
|
|
+ static constexpr auto distance_threshold = 50; // 50mm
|
|
|
+ static constexpr auto radius_threshold = 2; // 2mm
|
|
|
+ static constexpr auto invalid_angle = std::numeric_limits<float>::quiet_NaN();
|
|
|
+
|
|
|
+ using stray_points_type = ndi_stray_point_tracker::points_type;
|
|
|
+ std::vector<glm::vec3> ball_points;
|
|
|
+ bs::connection stray_points_conn;
|
|
|
+ std::atomic_flag is_calib;
|
|
|
+
|
|
|
+ struct result_type {
|
|
|
+ float radius;
|
|
|
+ glm::vec3 center, axis, start;
|
|
|
+ } result = {};
|
|
|
|
|
|
- // rotation ball
|
|
|
- using stray_points_type = ndi_stray_point_tracker::points_type;
|
|
|
- std::vector<glm::vec3> ball_points;
|
|
|
- bs::connection stray_points_conn;
|
|
|
- std::future<void> ball_future;
|
|
|
- float ball_radius = 0;
|
|
|
- glm::vec3 ball_center = {}, ball_axis = {}, ball_start = {};
|
|
|
- bool ball_ok = false;
|
|
|
-
|
|
|
- void rotation_ball_callback(stray_points_type points) {
|
|
|
- if (points.empty()) return;
|
|
|
- const auto ref_matrix =
|
|
|
- query_transform(conf.camera_ref_name).matrix;
|
|
|
- if (!ref_matrix) return;
|
|
|
- const auto t_matrix = inverse(*ref_matrix);
|
|
|
- std::ranges::for_each(points, [=](auto &point) {
|
|
|
- point.pos = transform_p(t_matrix, point.pos);
|
|
|
- });
|
|
|
-
|
|
|
- if (ball_points.empty()) {
|
|
|
- if (points.size() != 1) {
|
|
|
- stop_rotation_ball();
|
|
|
- SPDLOG_ERROR("Failed to load anchor point.");
|
|
|
+ std::atomic_flag result_ok;
|
|
|
+
|
|
|
+ void calib_callback(const stray_points_type &points) {
|
|
|
+ if (ball_points.empty()) {
|
|
|
+ if (points.size() != 1) {
|
|
|
+ stop_calib();
|
|
|
+ SPDLOG_ERROR("Failed to load anchor point.");
|
|
|
+ }
|
|
|
+ ball_points.emplace_back(points[0].pos);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ auto last_point = *ball_points.rbegin();
|
|
|
+ const auto closest_iter = std::ranges::min_element(
|
|
|
+ points, std::less(), [=](const auto &point) {
|
|
|
+ return glm::distance(last_point, point.pos);
|
|
|
+ });
|
|
|
+ const auto closest_point = closest_iter->pos;
|
|
|
+ if (distance(last_point, closest_point) > distance_threshold) { return; }
|
|
|
+ ball_points.emplace_back(closest_point);
|
|
|
+ }
|
|
|
+
|
|
|
+ void tracking_callback(const stray_points_type &points) {
|
|
|
+ if (!result_ok.test()) return;
|
|
|
+ auto dis_op = [=, this](const glm::vec3 point) {
|
|
|
+ const auto dis = glm::distance(result.center, point);
|
|
|
+ return std::abs(dis - result.radius);
|
|
|
+ };
|
|
|
+ const auto closest_iter = std::ranges::min_element(
|
|
|
+ points, std::less(), [=](auto p) { return dis_op(p.pos); });
|
|
|
+ const auto closest_point = closest_iter->pos;
|
|
|
+ if (dis_op(closest_point) > radius_threshold) {
|
|
|
+ OBJ_SAVE(conf.angle_name, invalid_angle);
|
|
|
+ g_sophiar_conn->update_transform_variable(
|
|
|
+ conf.len_to_camera_name, {});
|
|
|
+ return;
|
|
|
}
|
|
|
- ball_points.emplace_back(points[0].pos);
|
|
|
- return;
|
|
|
+
|
|
|
+ auto start_vec = result.start - result.center;
|
|
|
+ auto current_vec = closest_point - result.center;
|
|
|
+ start_vec /= glm::length(start_vec);
|
|
|
+ current_vec /= glm::length(current_vec);
|
|
|
+ auto angle = std::acos(glm::dot(start_vec, current_vec));
|
|
|
+ OBJ_SAVE(conf.angle_name, angle);
|
|
|
+
|
|
|
+ auto matrix = glm::mat4();
|
|
|
+ matrix[2] = glm::vec4(result.axis, 0); // Z-axis
|
|
|
+ matrix[0] = glm::vec4(current_vec, 0); // X-axis
|
|
|
+ const auto y_axis = glm::cross(result.axis, current_vec); // Z x X -> Y
|
|
|
+ matrix[1] = glm::vec4(y_axis, 0); // Y-axis
|
|
|
+ matrix[3] = glm::vec4(result.center, 1);
|
|
|
+ auto eigen_tm = Eigen::Isometry3d();
|
|
|
+ to_eigen_transform(matrix, eigen_tm);
|
|
|
+ g_sophiar_conn->update_transform_variable(
|
|
|
+ conf.len_to_camera_name, eigen_tm);
|
|
|
}
|
|
|
|
|
|
- auto last_point = *ball_points.rbegin();
|
|
|
- const auto closest_iter = std::ranges::min_element(
|
|
|
- points, std::less(), [=](const auto &point) {
|
|
|
- return glm::distance(last_point, point.pos);
|
|
|
+ void points_callback(stray_points_type points) {
|
|
|
+ if (points.empty()) return;
|
|
|
+ const auto ref_matrix =
|
|
|
+ query_transform(conf.camera_ref_name).matrix;
|
|
|
+ if (!ref_matrix) return;
|
|
|
+ const auto t_matrix = inverse(*ref_matrix);
|
|
|
+ std::ranges::for_each(points, [=](auto &point) {
|
|
|
+ point.pos = transform_p(t_matrix, point.pos);
|
|
|
});
|
|
|
- const auto closest_point = closest_iter->pos;
|
|
|
- if (distance(last_point, closest_point)
|
|
|
- > rotation_ball_distance_threshold) { return; }
|
|
|
- ball_points.emplace_back(closest_point);
|
|
|
- }
|
|
|
|
|
|
- void start_rotation_ball() {
|
|
|
- ball_points.clear();
|
|
|
- stray_points_conn = get_ndi_stray_points_sig()
|
|
|
- ->connect(std::bind(&impl::rotation_ball_callback, this, _1));
|
|
|
- status = ROTATION_BALL;
|
|
|
- }
|
|
|
+ if (is_calib.test()) {
|
|
|
+ calib_callback(points);
|
|
|
+ } else {
|
|
|
+ tracking_callback(points);
|
|
|
+ }
|
|
|
+ }
|
|
|
|
|
|
- void calc_rotation_ball() {
|
|
|
- auto closer = sg::make_scope_guard([this] { status = IDLE; });
|
|
|
- if (ball_points.size() < 3) return;
|
|
|
-
|
|
|
- auto input = fitting_circle_3d::input_type();
|
|
|
- input.points = to_eigen(ball_points);
|
|
|
- const auto result = fitting_circle_3d()(input);
|
|
|
- ball_center = result.center;
|
|
|
- ball_axis = result.axis;
|
|
|
- ball_start = ball_points[0];
|
|
|
- ball_radius = result.radius;
|
|
|
- SPDLOG_INFO("Rotation Ball RMS = {:.2f}mm", result.error_rms);
|
|
|
- ball_ok = true;
|
|
|
- }
|
|
|
+ void start_calib() {
|
|
|
+ ball_points.clear();
|
|
|
+ is_calib.test_and_set();
|
|
|
+ }
|
|
|
|
|
|
- void stop_rotation_ball() {
|
|
|
- stray_points_conn.disconnect();
|
|
|
- ball_future = TP_SUBMIT([this] { calc_rotation_ball(); });
|
|
|
- }
|
|
|
+ void calc_rotation_ball() {
|
|
|
+ if (ball_points.size() < 3) return;
|
|
|
+ auto input = fitting_circle_3d::input_type();
|
|
|
+ input.points = to_eigen(ball_points);
|
|
|
+ const auto ret = fitting_circle_3d()(input);
|
|
|
+ result.center = ret.center;
|
|
|
+ result.axis = ret.axis;
|
|
|
+ result.start = ball_points[0];
|
|
|
+ result.radius = ret.radius;
|
|
|
+ result_ok.test_and_set();
|
|
|
+ SPDLOG_INFO("Rotation Ball RMS = {:.2f}mm", ret.error_rms);
|
|
|
+ }
|
|
|
|
|
|
- void show_rotation_ball_ui() {
|
|
|
- const auto num_stray_point = get_ndi_stray_points().size();
|
|
|
- const bool can_operate = num_stray_point == 1;
|
|
|
- if (status == IDLE) {
|
|
|
- auto guard = imgui_disable_guard(!can_operate);
|
|
|
- if (ImGui::Button("Start")) {
|
|
|
- post(*main_ctx, [this] { start_rotation_ball(); });
|
|
|
+ void stop_calib() {
|
|
|
+ is_calib.clear();
|
|
|
+ calc_rotation_ball();
|
|
|
+ }
|
|
|
+
|
|
|
+ void show_ui() {
|
|
|
+ const auto num_stray_point = get_ndi_stray_points().size();
|
|
|
+ const bool can_operate = num_stray_point == 1;
|
|
|
+ const auto is_calib_val = is_calib.test();
|
|
|
+ if (!is_calib_val) {
|
|
|
+ auto guard = imgui_disable_guard(!can_operate);
|
|
|
+ if (ImGui::Button("Start")) {
|
|
|
+ MAIN_DETACH([ptr=shared_from_this()] { ptr->start_calib(); });
|
|
|
+ }
|
|
|
+ } else {
|
|
|
+ if (ImGui::Button("Stop")) {
|
|
|
+ MAIN_DETACH([ptr=shared_from_this()] { ptr->stop_calib(); });
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (true) {
|
|
|
+ auto guard = imgui_valid_style_guard(can_operate);
|
|
|
+ ImGui::Text("Stray Points: %ld", num_stray_point);
|
|
|
+ }
|
|
|
+ if (is_calib_val) {
|
|
|
+ ImGui::Text("Collected Samples: %ld", ball_points.size());
|
|
|
}
|
|
|
- } else if (status == ROTATION_BALL) {
|
|
|
- if (ImGui::Button("Stop")) {
|
|
|
- post(*main_ctx, [this] { stop_rotation_ball(); });
|
|
|
+ if (result_ok.test()) {
|
|
|
+ ImGui::Text("Current Angle (deg): %.2f",
|
|
|
+ glm::degrees(OBJ_QUERY(float, conf.angle_name)));
|
|
|
}
|
|
|
}
|
|
|
- if (true) {
|
|
|
- auto guard = imgui_valid_style_guard(can_operate || status == ROTATION_BALL);
|
|
|
- ImGui::Text("Stray Points: %ld", num_stray_point);
|
|
|
+
|
|
|
+ struct create_config {
|
|
|
+ std::string camera_ref_name;
|
|
|
+ std::string len_to_camera_name;
|
|
|
+ obj_name_type angle_name = {};
|
|
|
+ } conf;
|
|
|
+
|
|
|
+ explicit rotation_ball_calib(create_config _conf)
|
|
|
+ : conf(std::move(_conf)) {
|
|
|
+ stray_points_conn = get_ndi_stray_points_sig()->connect(
|
|
|
+ [ptr = shared_from_this()](auto points) {
|
|
|
+ ptr->points_callback(points);
|
|
|
+ });
|
|
|
}
|
|
|
- if (status == ROTATION_BALL) {
|
|
|
- ImGui::Text("Collected Samples: %ld", ball_points.size());
|
|
|
+
|
|
|
+ ~rotation_ball_calib() {
|
|
|
+ stray_points_conn.disconnect();
|
|
|
}
|
|
|
- }
|
|
|
+ };
|
|
|
+
|
|
|
+ struct reverse_rotation_image {
|
|
|
+ obj_conn_type img_conn;
|
|
|
+
|
|
|
+ void img_callback_impl() {
|
|
|
+ if (!conf.center->result) return;
|
|
|
+ const auto img = OBJ_QUERY(sp_image, conf.input_name);
|
|
|
+ const auto angle = OBJ_QUERY(float, conf.angle_name);
|
|
|
+ const auto rot_img = image_rotate(img, -angle, conf.center->result);
|
|
|
+ OBJ_SAVE(conf.output_name, rot_img);
|
|
|
+ }
|
|
|
+
|
|
|
+ void img_callback(const obj_name_type name) {
|
|
|
+ assert(name == conf.input_name);
|
|
|
+ try {
|
|
|
+ img_callback_impl();
|
|
|
+ } catch (...) { (void) 0; }
|
|
|
+ }
|
|
|
+
|
|
|
+ struct create_config {
|
|
|
+ obj_name_type input_name = {};
|
|
|
+ obj_name_type output_name = {};
|
|
|
+ obj_name_type angle_name = {};
|
|
|
+ std::shared_ptr<rotation_center_calib> center;
|
|
|
+ } conf;
|
|
|
+
|
|
|
+ explicit reverse_rotation_image(create_config _conf)
|
|
|
+ : conf(std::move(_conf)) {
|
|
|
+ img_conn = OBJ_SIG(conf.input_name)->connect(
|
|
|
+ [this](auto name) { img_callback(name); });
|
|
|
+ }
|
|
|
+
|
|
|
+ ~reverse_rotation_image() {
|
|
|
+ img_conn.disconnect();
|
|
|
+ }
|
|
|
+ };
|
|
|
+}
|
|
|
+
|
|
|
+namespace {
|
|
|
+ constexpr obj_name_type rev_rot_name_offset = 0x0100;
|
|
|
+}
|
|
|
|
|
|
- void show_ui() {
|
|
|
- ImGui::SeparatorText("Rotation Ball");
|
|
|
- show_rotation_ball_ui();
|
|
|
+struct oblique_calibrator::impl {
|
|
|
+ create_config conf;
|
|
|
+
|
|
|
+
|
|
|
+ enum {
|
|
|
+ IDLE,
|
|
|
+ ROTATION_BALL
|
|
|
+ } status = IDLE;
|
|
|
+
|
|
|
+ std::shared_ptr<rotation_center_calib> rot_center;
|
|
|
+ std::shared_ptr<rotation_ball_calib> rot_ball;
|
|
|
+ std::optional<reverse_rotation_image> rev_rot_img;
|
|
|
+
|
|
|
+ void show_ui() const {
|
|
|
+ if (true) {
|
|
|
+ ImGui::SeparatorText("Rotation Center");
|
|
|
+ auto id_guard = imgui_id_guard("rotation_ball");
|
|
|
+ rot_center->show_ui();
|
|
|
+ }
|
|
|
+ if (true) {
|
|
|
+ ImGui::SeparatorText("Rotation Ball");
|
|
|
+ auto id_guard = imgui_id_guard("rotation_center");
|
|
|
+ rot_ball->show_ui();
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
explicit impl(create_config conf)
|
|
|
: conf(std::move(conf)) {
|
|
|
- }
|
|
|
-
|
|
|
- ~impl() {
|
|
|
- if (ball_future.valid()) {
|
|
|
- ball_future.wait();
|
|
|
+ if constexpr (true) {
|
|
|
+ rot_center = std::make_shared<rotation_center_calib>();
|
|
|
+ rot_center->image_name = conf.image_name;
|
|
|
+ }
|
|
|
+ if constexpr (true) {
|
|
|
+ auto sub_conf = rotation_ball_calib::create_config();
|
|
|
+ sub_conf.camera_ref_name = conf.camera_ref_name;
|
|
|
+ sub_conf.len_to_camera_name = conf.len_to_camera_name;
|
|
|
+ sub_conf.angle_name = conf.angle_name;
|
|
|
+ rot_ball = std::make_shared<rotation_ball_calib>(sub_conf);
|
|
|
+ }
|
|
|
+ if constexpr (true) {
|
|
|
+ auto sub_conf = reverse_rotation_image::create_config();
|
|
|
+ sub_conf.input_name = conf.image_name;
|
|
|
+ sub_conf.output_name = conf.image_name + rev_rot_name_offset;
|
|
|
+ sub_conf.angle_name = conf.angle_name;
|
|
|
+ sub_conf.center = rot_center;
|
|
|
+ rev_rot_img.emplace(sub_conf);
|
|
|
}
|
|
|
}
|
|
|
};
|