|
|
@@ -1,22 +1,28 @@
|
|
|
#include "oblique_calibrator.h"
|
|
|
+
|
|
|
#include "core/imgui_utility.hpp"
|
|
|
#include "core/math_helper.hpp"
|
|
|
#include "core_v2/utility.hpp"
|
|
|
#include "device_v5/ndi_stray_point_tracker.h"
|
|
|
-#include "module_v5/transform_provider.h"
|
|
|
+#include "image_process_v5/image_process.h"
|
|
|
+#include "image_process_v5/process_python.h"
|
|
|
+#include "image_process_v5/sp_image.h"
|
|
|
#include "module_v5/algorithms.h"
|
|
|
+#include "module_v5/transform_provider.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/beast/core/detail/base64.hpp>
|
|
|
#include <boost/filesystem/operations.hpp>
|
|
|
+#include <glm/gtx/rotate_vector.hpp>
|
|
|
+#include <glm/trigonometric.hpp>
|
|
|
+#include <opencv2/imgproc.hpp>
|
|
|
#include <utility>
|
|
|
+#include <vgMath.h>
|
|
|
|
|
|
namespace ba = boost::asio;
|
|
|
namespace bs = boost::signals2;
|
|
|
@@ -25,23 +31,114 @@ using namespace sophiar;
|
|
|
extern local_connection *g_sophiar_conn;
|
|
|
|
|
|
namespace {
|
|
|
- struct rotation_center_calib
|
|
|
- : std::enable_shared_from_this<rotation_center_calib> {
|
|
|
+ namespace b64 = boost::beast::detail::base64;
|
|
|
+
|
|
|
+ template<typename T>
|
|
|
+ requires(std::is_pod_v<std::remove_cvref_t<T>>)
|
|
|
+ std::string any_to_string(T &&v) {
|
|
|
+ const auto ret_size = b64::encoded_size(sizeof(T));
|
|
|
+ auto ret = std::string(ret_size, '\0');
|
|
|
+ b64::encode(ret.data(), &v, sizeof(T));
|
|
|
+ return ret;
|
|
|
+ }
|
|
|
+
|
|
|
+ template<typename T>
|
|
|
+ requires(std::is_pod_v<T>)
|
|
|
+ T string_to_any(const std::string &str) {
|
|
|
+ constexpr auto buf_size = (sizeof(T) + 2) / 3 * 3;
|
|
|
+ char buf[buf_size];
|
|
|
+ assert(b64::decoded_size(str.length()) == buf_size);
|
|
|
+ b64::decode(buf, str.data(), str.length());
|
|
|
+ return *(T *) buf;
|
|
|
+ }
|
|
|
+
|
|
|
+ using contour_type = std::vector<cv::Point>;
|
|
|
+
|
|
|
+ Eigen::Matrix2Xf to_matrix(const contour_type &ps) {
|
|
|
+ auto ret = Eigen::Matrix2Xf(2, ps.size());
|
|
|
+ for ( int i = 0; i < ps.size(); ++i ) {
|
|
|
+ ret(0, i) = ps[i].x;
|
|
|
+ ret(1, i) = ps[i].y;
|
|
|
+ }
|
|
|
+ return ret;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::optional<float> calc_rotation_angle(const sp_image &img) { // RGB image
|
|
|
+ std::vector<contour_type> contours;
|
|
|
+ {
|
|
|
+ const auto img_gray = image_rgb_to_gray(img);
|
|
|
+ const auto img_binary = image_gray_to_binary(img_gray, 20);
|
|
|
+ const auto helper = read_access_helper(img_binary.host());
|
|
|
+ cv::findContours(img_binary.cv_mat(helper.ptr()), contours, //
|
|
|
+ cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
|
|
|
+ }
|
|
|
+
|
|
|
+ // find largest contour
|
|
|
+ contour_type pin_contour;
|
|
|
+ float max_area = 0.f;
|
|
|
+ for ( const auto &contour : contours ) {
|
|
|
+ if ( auto area = std::abs(cv::contourArea(contour)); //
|
|
|
+ area > max_area ) {
|
|
|
+ pin_contour = contour;
|
|
|
+ max_area = area;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if ( pin_contour.empty() ) return {};
|
|
|
+
|
|
|
+ // 2D circle fitting
|
|
|
+ auto fit_input = fitting_circle_2d::input_type();
|
|
|
+ fit_input.points = to_matrix(pin_contour);
|
|
|
+ const auto fit_2d = fitting_circle_2d()(fit_input);
|
|
|
+ const auto radius = fit_2d.radius;
|
|
|
+ const auto center = fit_2d.center;
|
|
|
+ if ( radius < 410 || radius > 420 ) return {}; // TODO: config magic number
|
|
|
+
|
|
|
+ std::vector<glm::vec2> pin_points;
|
|
|
+ for ( const auto &p : pin_contour ) {
|
|
|
+ auto point = to_vec2(p);
|
|
|
+ if ( auto dis = glm::distance(point, center); //
|
|
|
+ std::abs(dis - radius) > 10 ) {
|
|
|
+ pin_points.push_back(point);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ auto pin_center = std::ranges::fold_left(pin_points, glm::vec2(), std::plus());
|
|
|
+ pin_center /= pin_points.size();
|
|
|
+ auto angle = std::atan2(pin_center.y - center.y, pin_center.x - center.x);
|
|
|
+ return angle + M_PI / 2;
|
|
|
+ }
|
|
|
+
|
|
|
+} // namespace
|
|
|
+
|
|
|
+namespace {
|
|
|
+ struct rotation_center_calib {
|
|
|
obj_name_type image_name = {};
|
|
|
|
|
|
using result_type = rotate_registration_result_type;
|
|
|
|
|
|
- std::optional<glm::vec2> result;
|
|
|
- std::vector<sp_image> calib_images;
|
|
|
- std::atomic_size_t reg_tasks;
|
|
|
- std::vector<glm::vec2> reg_results;
|
|
|
+ std::optional<glm::vec2> result;
|
|
|
+ bs::signal<void(glm::vec2)> result_sig;
|
|
|
+ std::vector<sp_image> calib_images;
|
|
|
+ std::atomic_size_t reg_tasks;
|
|
|
+ std::vector<glm::vec2> reg_results;
|
|
|
+
|
|
|
+ void set_result(const std::string &str) {
|
|
|
+ set_result(string_to_any<glm::vec2>(str));
|
|
|
+ }
|
|
|
+
|
|
|
+ void set_result(glm::vec2 _ret) {
|
|
|
+ result = _ret;
|
|
|
+ SPDLOG_INFO("Rotation Center: {}", *result);
|
|
|
+ SPDLOG_INFO("Rotation Center Str: {}", any_to_string(*result));
|
|
|
+ MAIN_DETACH([this] { result_sig(*result); });
|
|
|
+ }
|
|
|
|
|
|
void reg_callback(const result_type ret, const size_t index) {
|
|
|
reg_results[index] = ret.center;
|
|
|
- if (--reg_tasks == 0) {
|
|
|
+ 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; });
|
|
|
+ MAIN_DETACH([=, this] { set_result(sum); });
|
|
|
}
|
|
|
}
|
|
|
|
|
|
@@ -51,57 +148,68 @@ namespace {
|
|
|
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);
|
|
|
- });
|
|
|
+ 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], //
|
|
|
+ [this, _index = index++](auto ret) { 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 ( ImGui::Button("Collect") ) {
|
|
|
+ MAIN_DETACH([this] {
|
|
|
+ auto img = OBJ_QUERY(sp_image, image_name);
|
|
|
+ calib_images.emplace_back(img);
|
|
|
});
|
|
|
}
|
|
|
- if (true) {
|
|
|
- auto guard = imgui_valid_style_guard(can_operate);
|
|
|
+ if ( true ) {
|
|
|
+ auto guard = imgui_disable_guard(!can_operate);
|
|
|
ImGui::SameLine();
|
|
|
- if (ImGui::Button("Calibrate")) {
|
|
|
- TP_DETACH([ptr = shared_from_this()] { ptr->start_calib(); });
|
|
|
+ if ( ImGui::Button("Calibrate") ) {
|
|
|
+ TP_DETACH([this] { start_calib(); });
|
|
|
}
|
|
|
+ }
|
|
|
+ if ( true ) {
|
|
|
+ auto guard = imgui_valid_style_guard(can_operate);
|
|
|
ImGui::Text("Image Samples: %ld", num_samples);
|
|
|
}
|
|
|
+ if ( true ) {
|
|
|
+ const auto is_ok = (bool) result;
|
|
|
+ auto guard = imgui_valid_style_guard(is_ok);
|
|
|
+ ImGui::Text("Result OK: %s", is_ok ? "True" : "False");
|
|
|
+ }
|
|
|
}
|
|
|
};
|
|
|
|
|
|
- struct rotation_ball_calib
|
|
|
- : std::enable_shared_from_this<rotation_ball_calib> {
|
|
|
+ struct 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();
|
|
|
+ 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;
|
|
|
+ bs::connection stray_points_conn;
|
|
|
+ std::atomic_flag is_calib;
|
|
|
|
|
|
struct result_type {
|
|
|
- float radius;
|
|
|
- glm::vec3 center, axis, start;
|
|
|
+ float radius;
|
|
|
+ glm::vec3 center, axis, start_vec;
|
|
|
} result = {};
|
|
|
|
|
|
- std::atomic_flag result_ok;
|
|
|
+ std::atomic_flag result_ok;
|
|
|
+ bs::signal<void(result_type)> result_sig;
|
|
|
+ float last_ball_error = invalid_angle; // distance in mm
|
|
|
+ bool passthrough = false;
|
|
|
+ bool frozen_angle = false;
|
|
|
+
|
|
|
+ obj_conn_type img_conn;
|
|
|
+ float img_angle = invalid_angle;
|
|
|
|
|
|
void calib_callback(const stray_points_type &points) {
|
|
|
- if (ball_points.empty()) {
|
|
|
- if (points.size() != 1) {
|
|
|
+ if ( ball_points.empty() ) {
|
|
|
+ if ( points.size() != 1 ) {
|
|
|
stop_calib();
|
|
|
SPDLOG_ERROR("Failed to load anchor point.");
|
|
|
}
|
|
|
@@ -109,84 +217,121 @@ namespace {
|
|
|
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);
|
|
|
- });
|
|
|
+ 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; }
|
|
|
+ if ( distance(last_point, closest_point) > distance_threshold ) { return; }
|
|
|
ball_points.emplace_back(closest_point);
|
|
|
}
|
|
|
|
|
|
+ void update_image_angle(const float angle) { // angle in radius
|
|
|
+ if ( !result_ok.test() ) return;
|
|
|
+ if ( passthrough ) {
|
|
|
+ g_sophiar_conn->update_transform_variable( //
|
|
|
+ conf.len_to_camera_name, Eigen::Isometry3d::Identity());
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if ( frozen_angle ) { return; }
|
|
|
+
|
|
|
+ OBJ_SAVE(conf.angle_name, angle);
|
|
|
+ if ( std::isnan(angle) ) {
|
|
|
+ g_sophiar_conn->update_transform_variable( //
|
|
|
+ conf.len_to_camera_name, {});
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ const auto cur_axis = glm::rotate(result.start_vec, angle, -result.axis);
|
|
|
+ auto matrix = glm::mat4();
|
|
|
+ matrix[2] = glm::vec4(result.axis, 0); // Z-axis
|
|
|
+ matrix[0] = glm::vec4(cur_axis, 0); // X-axis
|
|
|
+ const auto y_axis = glm::cross(result.axis, cur_axis); // Z x X -> Y
|
|
|
+ matrix[1] = glm::vec4(y_axis, 0); // Y-axis
|
|
|
+ matrix[3] = glm::vec4(result.center, 1);
|
|
|
+ g_sophiar_conn->update_transform_variable(conf.len_to_camera_name, //
|
|
|
+ to_eigen_transform<double, Eigen::Isometry>(matrix));
|
|
|
+ }
|
|
|
+
|
|
|
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_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, {});
|
|
|
+ last_ball_error = dis_op(closest_point);
|
|
|
+ if ( last_ball_error > radius_threshold ) {
|
|
|
+ // update_image_angle(invalid_angle); // TODO: add options to ignore missing
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
- auto start_vec = result.start - result.center;
|
|
|
auto current_vec = closest_point - result.center;
|
|
|
- start_vec /= glm::length(start_vec);
|
|
|
+ current_vec -= glm::dot(current_vec, result.axis);
|
|
|
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);
|
|
|
+ // https://stackoverflow.com/questions/5188561/signed-angle-between-two-3d-vectors-with-same-origin-within-the-same-plane
|
|
|
+ auto angle = std::atan2( //
|
|
|
+ glm::dot(glm::cross(result.start_vec, current_vec), result.axis), //
|
|
|
+ glm::dot(result.start_vec, current_vec));
|
|
|
+ update_image_angle(angle);
|
|
|
}
|
|
|
|
|
|
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;
|
|
|
+ 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) {
|
|
|
+ std::ranges::for_each(points, [=](auto &point) { //
|
|
|
point.pos = transform_p(t_matrix, point.pos);
|
|
|
});
|
|
|
|
|
|
- if (is_calib.test()) {
|
|
|
+ if ( result_ok.test() ) {
|
|
|
+ // tracking_callback(points); // TODO: Debug, use angle from image
|
|
|
+ } else if ( is_calib.test() ) {
|
|
|
calib_callback(points);
|
|
|
- } else {
|
|
|
- tracking_callback(points);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
void start_calib() {
|
|
|
+ result_ok.clear();
|
|
|
ball_points.clear();
|
|
|
is_calib.test_and_set();
|
|
|
}
|
|
|
|
|
|
- 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;
|
|
|
+ void set_result(const std::string &str) {
|
|
|
+ set_result(string_to_any<result_type>(str));
|
|
|
+ }
|
|
|
+
|
|
|
+ void set_result(const result_type &_ret) {
|
|
|
+ result = _ret;
|
|
|
+ SPDLOG_INFO("Rotation Information:");
|
|
|
+ SPDLOG_INFO("\tRadius: {}", result.radius);
|
|
|
+ SPDLOG_INFO("\tCenter: {}", result.center);
|
|
|
+ SPDLOG_INFO("\tAxis: {}", result.axis);
|
|
|
+ SPDLOG_INFO("\tStart Dir: {}", result.start_vec);
|
|
|
+ SPDLOG_INFO("Rotation Str: {}", any_to_string(result));
|
|
|
+
|
|
|
result_ok.test_and_set();
|
|
|
- SPDLOG_INFO("Rotation Ball RMS = {:.2f}mm", ret.error_rms);
|
|
|
+ MAIN_DETACH([this] { result_sig(result); });
|
|
|
+ }
|
|
|
+
|
|
|
+ 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 fit_ret = fitting_circle_3d()(input);
|
|
|
+ SPDLOG_INFO("Rotation Ball RMS = {:.2f}mm", fit_ret.error_rms);
|
|
|
+
|
|
|
+ auto start_vec = ball_points[0] - fit_ret.center;
|
|
|
+ start_vec -= glm::dot(fit_ret.axis, start_vec);
|
|
|
+ start_vec /= glm::length(start_vec);
|
|
|
+
|
|
|
+ auto ret = result_type();
|
|
|
+ ret.center = fit_ret.center;
|
|
|
+ ret.axis = fit_ret.axis;
|
|
|
+ ret.start_vec = start_vec;
|
|
|
+ ret.radius = fit_ret.radius;
|
|
|
+ set_result(ret);
|
|
|
}
|
|
|
|
|
|
void stop_calib() {
|
|
|
@@ -194,149 +339,228 @@ namespace {
|
|
|
calc_rotation_ball();
|
|
|
}
|
|
|
|
|
|
+ void img_callback(const obj_name_type name) {
|
|
|
+ assert(name == conf.image_name);
|
|
|
+ try {
|
|
|
+ const auto img = OBJ_QUERY(sp_image, conf.image_name);
|
|
|
+ if ( const auto angle = calc_rotation_angle(img); angle ) {
|
|
|
+ img_angle = *angle;
|
|
|
+ } else {
|
|
|
+ img_angle = invalid_angle;
|
|
|
+ }
|
|
|
+ } catch ( ... ) { //
|
|
|
+ img_angle = invalid_angle;
|
|
|
+ }
|
|
|
+ update_image_angle(img_angle);
|
|
|
+ }
|
|
|
+
|
|
|
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) {
|
|
|
+ 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(); });
|
|
|
+ if ( ImGui::Button("Start") ) {
|
|
|
+ MAIN_DETACH([this] { start_calib(); });
|
|
|
}
|
|
|
} else {
|
|
|
- if (ImGui::Button("Stop")) {
|
|
|
- MAIN_DETACH([ptr=shared_from_this()] { ptr->stop_calib(); });
|
|
|
+ if ( ImGui::Button("Stop") ) {
|
|
|
+ MAIN_DETACH([this] { stop_calib(); });
|
|
|
}
|
|
|
}
|
|
|
- if (true) {
|
|
|
+ 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());
|
|
|
+ if ( is_calib_val ) { ImGui::Text("Collected Samples: %ld", ball_points.size()); }
|
|
|
+
|
|
|
+ const bool is_ok = result_ok.test();
|
|
|
+ if ( true ) {
|
|
|
+ auto guard = imgui_valid_style_guard(is_ok);
|
|
|
+ ImGui::Text("Result OK: %s", is_ok ? "True" : "False");
|
|
|
}
|
|
|
- if (result_ok.test()) {
|
|
|
- ImGui::Text("Current Angle (deg): %.2f",
|
|
|
- glm::degrees(OBJ_QUERY(float, conf.angle_name)));
|
|
|
+ if ( is_ok ) {
|
|
|
+ ImGui::Checkbox("Passthrough", &passthrough);
|
|
|
+ if ( !passthrough ) {
|
|
|
+ ImGui::SameLine();
|
|
|
+ ImGui::Checkbox("Frozen", &frozen_angle);
|
|
|
+
|
|
|
+ {
|
|
|
+ auto guard = imgui_valid_style_guard(!std::isnan(img_angle));
|
|
|
+ ImGui::Text("Image Angle (deg): %.2f", glm::degrees(img_angle));
|
|
|
+ }
|
|
|
+
|
|
|
+ try {
|
|
|
+
|
|
|
+ // ImGui::Text("Current Angle (deg): %.2f", //
|
|
|
+ // glm::degrees(OBJ_QUERY(float, conf.angle_name)));
|
|
|
+ // ImGui::Text("Current Point Error (mm): %.2f", last_ball_error);
|
|
|
+ } catch ( ... ) { (void) 0; }
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
|
|
|
struct create_config {
|
|
|
- std::string camera_ref_name;
|
|
|
- std::string len_to_camera_name;
|
|
|
+ std::string camera_ref_name;
|
|
|
+ std::string len_to_camera_name;
|
|
|
obj_name_type angle_name = {};
|
|
|
+ obj_name_type image_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);
|
|
|
- });
|
|
|
+ explicit rotation_ball_calib(create_config _conf) : conf(std::move(_conf)) {
|
|
|
+ stray_points_conn = get_ndi_stray_points_sig()->connect( //
|
|
|
+ [this](auto points) { points_callback(std::move(points)); });
|
|
|
+ img_conn = OBJ_SIG(conf.image_name) //
|
|
|
+ ->connect([this](auto name) { img_callback(name); });
|
|
|
}
|
|
|
|
|
|
~rotation_ball_calib() {
|
|
|
stray_points_conn.disconnect();
|
|
|
+ img_conn.disconnect();
|
|
|
}
|
|
|
};
|
|
|
|
|
|
- struct reverse_rotation_image {
|
|
|
- obj_conn_type img_conn;
|
|
|
+ struct image_rotation_helper {
|
|
|
+ obj_conn_type img_conn;
|
|
|
+ std::optional<glm::vec2> center;
|
|
|
+ float last_angle = 0.f;
|
|
|
|
|
|
void img_callback_impl() {
|
|
|
- if (!conf.center->result) return;
|
|
|
+ if ( !center ) 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);
|
|
|
+ if ( conf.source->passthrough ) {
|
|
|
+ OBJ_SAVE(conf.output_name, img);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ const auto rot_img = image_rotate(img, glm::degrees(last_angle), center);
|
|
|
OBJ_SAVE(conf.output_name, rot_img);
|
|
|
}
|
|
|
|
|
|
void img_callback(const obj_name_type name) {
|
|
|
assert(name == conf.input_name);
|
|
|
+ try {
|
|
|
+ auto angle = OBJ_QUERY(float, conf.angle_name);
|
|
|
+ if ( !conf.inverse ) { angle = -angle; }
|
|
|
+ if ( !std::isnan(angle) ) { last_angle = angle; }
|
|
|
+ } catch ( ... ) { (void) 0; }
|
|
|
try {
|
|
|
img_callback_impl();
|
|
|
- } catch (...) { (void) 0; }
|
|
|
+ } 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;
|
|
|
+ obj_name_type input_name = {};
|
|
|
+ obj_name_type output_name = {};
|
|
|
+ obj_name_type angle_name = {};
|
|
|
+ bool inverse = false;
|
|
|
+ rotation_ball_calib *source = nullptr;
|
|
|
} 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); });
|
|
|
+ explicit image_rotation_helper(create_config _conf) : conf(_conf) {
|
|
|
+ img_conn = OBJ_SIG(conf.input_name)->connect([this](auto name) { //
|
|
|
+ img_callback(name);
|
|
|
+ });
|
|
|
}
|
|
|
|
|
|
- ~reverse_rotation_image() {
|
|
|
+ ~image_rotation_helper() {
|
|
|
img_conn.disconnect();
|
|
|
}
|
|
|
};
|
|
|
-}
|
|
|
-
|
|
|
-namespace {
|
|
|
- constexpr obj_name_type rev_rot_name_offset = 0x0100;
|
|
|
-}
|
|
|
+} // namespace
|
|
|
|
|
|
struct oblique_calibrator::impl {
|
|
|
create_config conf;
|
|
|
|
|
|
+ rotation_center_calib calib_center;
|
|
|
+ std::optional<rotation_ball_calib> calib_ball;
|
|
|
+ std::optional<image_rotation_helper> rev_rot_helper;
|
|
|
+ std::optional<image_rotation_helper> img_rot_helper;
|
|
|
+ std::optional<camera_calibrator> calib_cam;
|
|
|
|
|
|
- enum {
|
|
|
- IDLE,
|
|
|
- ROTATION_BALL
|
|
|
- } status = IDLE;
|
|
|
+ void show_ui() {
|
|
|
+ if ( ImGui::TreeNode("Rotation Center") ) {
|
|
|
+ calib_center.show_ui();
|
|
|
+ ImGui::TreePop();
|
|
|
+ }
|
|
|
+ if ( calib_center.result && ImGui::TreeNode("Rotation Ball") ) {
|
|
|
+ calib_ball->show_ui();
|
|
|
+ ImGui::TreePop();
|
|
|
+ }
|
|
|
+ if ( calib_ball->result_ok.test() && ImGui::TreeNode("Hand-Eye") ) {
|
|
|
+ calib_cam->show();
|
|
|
+ ImGui::TreePop();
|
|
|
+ }
|
|
|
+ }
|
|
|
|
|
|
- std::shared_ptr<rotation_center_calib> rot_center;
|
|
|
- std::shared_ptr<rotation_ball_calib> rot_ball;
|
|
|
- std::optional<reverse_rotation_image> rev_rot_img;
|
|
|
+ explicit impl(create_config _conf) : conf(std::move(_conf)) {
|
|
|
+ if ( true ) {
|
|
|
+ calib_center.image_name = conf.image_name;
|
|
|
+ calib_center.result_sig.connect([this](auto center) {
|
|
|
+ rev_rot_helper->center = center;
|
|
|
+ img_rot_helper->center = center;
|
|
|
+ });
|
|
|
+ }
|
|
|
|
|
|
- void show_ui() const {
|
|
|
- if (true) {
|
|
|
- ImGui::SeparatorText("Rotation Center");
|
|
|
- auto id_guard = imgui_id_guard("rotation_ball");
|
|
|
- rot_center->show_ui();
|
|
|
+ if ( 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_ref_to_camera_ref_name;
|
|
|
+ sub_conf.angle_name = conf.angle_name;
|
|
|
+ sub_conf.image_name = conf.image_name;
|
|
|
+ calib_ball.emplace(sub_conf);
|
|
|
}
|
|
|
- if (true) {
|
|
|
- ImGui::SeparatorText("Rotation Ball");
|
|
|
- auto id_guard = imgui_id_guard("rotation_center");
|
|
|
- rot_ball->show_ui();
|
|
|
+
|
|
|
+ if ( true ) {
|
|
|
+ auto sub_conf = image_rotation_helper::create_config();
|
|
|
+ sub_conf.input_name = conf.image_name;
|
|
|
+ sub_conf.output_name = conf.image_inv_rotate_name;
|
|
|
+ sub_conf.angle_name = conf.angle_name;
|
|
|
+ sub_conf.inverse = true;
|
|
|
+ sub_conf.source = &calib_ball.value();
|
|
|
+ rev_rot_helper.emplace(sub_conf);
|
|
|
+
|
|
|
+ sub_conf.input_name = conf.image_fix_in_name;
|
|
|
+ sub_conf.output_name = conf.image_fix_out_name;
|
|
|
+ sub_conf.inverse = false;
|
|
|
+ img_rot_helper.emplace(sub_conf);
|
|
|
}
|
|
|
- }
|
|
|
|
|
|
- explicit impl(create_config conf)
|
|
|
- : conf(std::move(conf)) {
|
|
|
- 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);
|
|
|
+ if ( true ) {
|
|
|
+ auto sub_conf = camera_calibrator::create_config();
|
|
|
+ sub_conf.img_name = conf.image_inv_rotate_name;
|
|
|
+ sub_conf.board_type = conf.board_type;
|
|
|
+ sub_conf.pattern_size = conf.pattern_size;
|
|
|
+ sub_conf.corner_distance = conf.corner_distance;
|
|
|
+ sub_conf.cb_func = conf.cb_func;
|
|
|
+ sub_conf.ref_transform_var = conf.len_ref_name;
|
|
|
+ sub_conf.ref_error_var = conf.camera_ref_error_name;
|
|
|
+ sub_conf.result_transform_var = conf.camera_to_len_ref_name;
|
|
|
+ calib_cam.emplace(sub_conf);
|
|
|
}
|
|
|
}
|
|
|
};
|
|
|
|
|
|
-oblique_calibrator::oblique_calibrator(const create_config &conf)
|
|
|
- : pimpl(std::make_unique<impl>(conf)) {
|
|
|
-}
|
|
|
+oblique_calibrator::oblique_calibrator(const create_config &conf) //
|
|
|
+ : pimpl(std::make_unique<impl>(conf)) { }
|
|
|
|
|
|
oblique_calibrator::~oblique_calibrator() = default;
|
|
|
|
|
|
void oblique_calibrator::show_ui() const {
|
|
|
pimpl->show_ui();
|
|
|
}
|
|
|
+
|
|
|
+void oblique_calibrator::render() const {
|
|
|
+ pimpl->calib_cam->render();
|
|
|
+}
|
|
|
+
|
|
|
+void oblique_calibrator::set_rotation_center(const std::string &str) const {
|
|
|
+ pimpl->calib_center.set_result(str);
|
|
|
+}
|
|
|
+
|
|
|
+void oblique_calibrator::set_rotation_ball(const std::string &str) const {
|
|
|
+ pimpl->calib_ball->set_result(str);
|
|
|
+}
|
|
|
+
|
|
|
+void oblique_calibrator::set_camera_calib(const camera_calibrator::simulate_info_type &sim_info) const {
|
|
|
+ pimpl->calib_cam->simulate_process(sim_info);
|
|
|
+}
|
