创建仓库
This commit is contained in:
commit
a22f80f7b6
|
|
@ -0,0 +1,2 @@
|
|||
cmake-build-release
|
||||
.idea
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
cmake_minimum_required(VERSION 3.26)
|
||||
project(power_rune)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
|
||||
find_package( Eigen3 REQUIRED )
|
||||
include_directories( ${EIGEN3_INCLUDE_DIRS} )
|
||||
find_package(OpenCV REQUIRED)
|
||||
include_directories(${OpenCV_INCLUDE_DIRS})
|
||||
#Python
|
||||
find_package(PythonLibs REQUIRED)
|
||||
include_directories(${PYTHON_INCLUDE_DIRS})
|
||||
|
||||
include_directories(include)
|
||||
#遍历所有Cpp文件
|
||||
file(GLOB_RECURSE CPP_FILES src/*.cpp)
|
||||
add_executable(power_rune ${CPP_FILES})
|
||||
|
||||
target_link_libraries(power_rune Eigen3::Eigen ${OpenCV_LIBS} ${PYTHON_LIBRARIES})
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 659 KiB |
|
|
@ -0,0 +1,65 @@
|
|||
//
|
||||
// Created by sph on 2024/1/4.
|
||||
//
|
||||
|
||||
#ifndef ADAMFITTING_ADAMFITTER_H
|
||||
#define ADAMFITTING_ADAMFITTER_H
|
||||
|
||||
#include<Eigen/Dense>
|
||||
#include <vector>
|
||||
|
||||
namespace fitter {
|
||||
|
||||
class AdamFitter {
|
||||
private:
|
||||
double a_;
|
||||
double omega_;
|
||||
double phi_;
|
||||
double b_;
|
||||
int num_iterations_;
|
||||
|
||||
double beta1_ = 0.9;
|
||||
double beta2_ = 0.999;
|
||||
double learning_rate_ = 0.1;
|
||||
double epsilon_ = 1e-8;
|
||||
double convergence_threshold_ = 1e-6;
|
||||
|
||||
//一阶矩和二阶矩
|
||||
double m_a_;
|
||||
double v_a_;
|
||||
double m_omega_;
|
||||
double v_omega_;
|
||||
double m_phi_;
|
||||
double v_phi_;
|
||||
double m_b_;
|
||||
double v_b_;
|
||||
|
||||
static double loss(const Eigen::ArrayXd &angular_velocity, const Eigen::ArrayXd &angular_velocity_pred);
|
||||
|
||||
public:
|
||||
explicit AdamFitter(double a = 0.9125,
|
||||
double omega = 1.9420,
|
||||
double phi = 1.0707,
|
||||
double b = 1.1775,
|
||||
int num_iterations = 1000,
|
||||
double beta1 = 0.9,
|
||||
double beta2 = 0.999,
|
||||
double learning_rate = 0.1,
|
||||
double epsilon = 1e-6,
|
||||
double convergence_threshold = 1e-5,
|
||||
double m_a = 0.0,
|
||||
double v_a = 0.0,
|
||||
double m_omega = 0.0,
|
||||
double v_omega = 0.0,
|
||||
double m_phi = 0.0,
|
||||
double v_phi = 0.0,
|
||||
double m_b = 0.0,
|
||||
double v_b = 0.0);
|
||||
|
||||
bool Fitting(const Eigen::ArrayXd &t, const Eigen::ArrayXd &angular_velocity, std::vector<double> *result);
|
||||
static Eigen::ArrayXd sine_function(const Eigen::ArrayXd &t, double a, double omega, double phi, double b);
|
||||
|
||||
};
|
||||
} // fitter
|
||||
|
||||
#endif //ADAMFITTING_ADAMFITTER_H
|
||||
|
|
@ -0,0 +1,153 @@
|
|||
#ifndef POWER_RUNE_BLADE_H
|
||||
#define POWER_RUNE_BLADE_H
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
namespace rm_power_rune {
|
||||
const int RED = 0;
|
||||
const int BLUE = 1;
|
||||
|
||||
struct FarEnd : public cv::RotatedRect {
|
||||
FarEnd() = default;
|
||||
|
||||
explicit FarEnd(const cv::RotatedRect &box, const cv::Point2f r) : cv::RotatedRect(box) {
|
||||
box.points(p);
|
||||
std::sort(p, p + 4,
|
||||
[&r](const cv::Point2f &a, const cv::Point2f &b) { return cv::norm(a - r) < cv::norm(b - r); });
|
||||
|
||||
double tilt_angle_0 = std::atan2(p[0].y - r.y, p[0].x - r.x);
|
||||
double tilt_angle_1 = std::atan2(p[1].y - r.y, p[1].x - r.x);
|
||||
|
||||
tilt_angle_0 = tilt_angle_0 / CV_PI * 180;
|
||||
tilt_angle_1 = tilt_angle_1 / CV_PI * 180;
|
||||
|
||||
|
||||
if (tilt_angle_0 < 0) {
|
||||
tilt_angle_0 = -tilt_angle_0;
|
||||
} else {
|
||||
tilt_angle_0 = 360 - tilt_angle_0;
|
||||
}
|
||||
if (tilt_angle_1 < 0) {
|
||||
tilt_angle_1 = -tilt_angle_1;
|
||||
} else {
|
||||
tilt_angle_1 = 360 - tilt_angle_1;
|
||||
}
|
||||
|
||||
if (tilt_angle_0 < 90 && tilt_angle_1 > 270) {
|
||||
corner_point_0 = p[0];
|
||||
corner_point_1 = p[1];
|
||||
} else if (tilt_angle_1 < 90 && tilt_angle_0 > 270) {
|
||||
corner_point_1 = p[0];
|
||||
corner_point_0 = p[1];
|
||||
} else if (tilt_angle_0 > tilt_angle_1) {
|
||||
corner_point_1 = p[1];
|
||||
corner_point_0 = p[0];
|
||||
} else if (tilt_angle_0 < tilt_angle_1) {
|
||||
corner_point_0 = p[1];
|
||||
corner_point_1 = p[0];
|
||||
}
|
||||
|
||||
cv::Point2f center = (corner_point_0 + corner_point_1) / 2;
|
||||
tilt_angle = std::atan2(center.y - r.y, center.x - r.x);
|
||||
tilt_angle = tilt_angle / CV_PI * 180;
|
||||
if (tilt_angle < 0) {
|
||||
tilt_angle = -tilt_angle;
|
||||
} else {
|
||||
tilt_angle = 360 - tilt_angle;
|
||||
}
|
||||
}
|
||||
|
||||
cv::Point2f p[4];
|
||||
cv::Point2f corner_point_0;
|
||||
cv::Point2f corner_point_1;
|
||||
double tilt_angle = 0;
|
||||
};
|
||||
|
||||
struct NearEnd : public cv::RotatedRect {
|
||||
NearEnd() = default;
|
||||
|
||||
NearEnd(const cv::RotatedRect &box, bool activated, const cv::Point2f r) : cv::RotatedRect(box) {
|
||||
is_activated = activated;
|
||||
box.points(p);
|
||||
|
||||
std::sort(p, p + 4,
|
||||
[&r](const cv::Point2f &a, const cv::Point2f &b) { return cv::norm(a - r) < cv::norm(b - r); });
|
||||
|
||||
double tilt_angle_2 = std::atan2(p[2].y - r.y, p[2].x - r.x);
|
||||
double tilt_angle_3 = std::atan2(p[3].y - r.y, p[3].x - r.x);
|
||||
tilt_angle_2 = tilt_angle_2 / CV_PI * 180;
|
||||
tilt_angle_3 = tilt_angle_3 / CV_PI * 180;
|
||||
|
||||
if (tilt_angle_2 < 0) {
|
||||
tilt_angle_2 = -tilt_angle_2;
|
||||
} else {
|
||||
tilt_angle_2 = 360 - tilt_angle_2;
|
||||
}
|
||||
if (tilt_angle_3 < 0) {
|
||||
tilt_angle_3 = -tilt_angle_3;
|
||||
} else {
|
||||
tilt_angle_3 = 360 - tilt_angle_3;
|
||||
}
|
||||
|
||||
if (tilt_angle_2 < 90 && tilt_angle_3 > 270) {
|
||||
corner_point_2 = p[3];
|
||||
corner_point_3 = p[2];
|
||||
} else if (tilt_angle_3 < 90 && tilt_angle_2 > 270) {
|
||||
corner_point_3 = p[3];
|
||||
corner_point_2 = p[2];
|
||||
} else if (tilt_angle_2 > tilt_angle_3) {
|
||||
corner_point_3 = p[2];
|
||||
corner_point_2 = p[3];
|
||||
} else if (tilt_angle_2 < tilt_angle_3) {
|
||||
corner_point_2 = p[2];
|
||||
corner_point_3 = p[3];
|
||||
}
|
||||
|
||||
cv::Point2f center = (corner_point_2 + corner_point_3) / 2;
|
||||
tilt_angle = std::atan2(center.y - r.y, center.x - r.x);
|
||||
tilt_angle = tilt_angle / CV_PI * 180;
|
||||
|
||||
if (tilt_angle < 0) {
|
||||
tilt_angle = -tilt_angle;
|
||||
} else {
|
||||
tilt_angle = 360 - tilt_angle;
|
||||
}
|
||||
}
|
||||
|
||||
cv::Point2f p[4];
|
||||
cv::Point2f corner_point_2;
|
||||
cv::Point2f corner_point_3;
|
||||
double tilt_angle = 0;
|
||||
bool is_activated = false;
|
||||
};
|
||||
|
||||
struct Blade {
|
||||
Blade() = default;
|
||||
|
||||
Blade(const NearEnd &near_end, const FarEnd &far_end) {
|
||||
top_left = far_end.corner_point_0;
|
||||
top_right = far_end.corner_point_1;
|
||||
bottom_right = near_end.corner_point_2;
|
||||
bottom_left = near_end.corner_point_3;
|
||||
center = ((top_right + top_left) / 2 + (bottom_left + bottom_right) / 2) / 2;
|
||||
|
||||
is_activated = near_end.is_activated;
|
||||
if(abs(far_end.tilt_angle - near_end.tilt_angle)>180)
|
||||
tilt_angle = (far_end.tilt_angle + near_end.tilt_angle + 360) / 2;
|
||||
else
|
||||
tilt_angle = (far_end.tilt_angle + near_end.tilt_angle) / 2;
|
||||
}
|
||||
|
||||
//以扇叶远端为上
|
||||
cv::Point2f top_left;
|
||||
cv::Point2f top_right;
|
||||
cv::Point2f bottom_right;
|
||||
cv::Point2f bottom_left;
|
||||
|
||||
cv::Point2f center;
|
||||
double tilt_angle = 0;
|
||||
bool is_activated = false;
|
||||
};
|
||||
|
||||
}// namespace rm_power_rune
|
||||
#endif //POWER_RUNE_BLADE_H
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
// Copyright 2022 Chen Jun
|
||||
// Licensed under the MIT License.
|
||||
|
||||
#ifndef PNP_SOLVER_HPP_
|
||||
#define PNP_SOLVER_HPP_
|
||||
|
||||
//#include <geometry_msgs/msg/point.hpp>
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
// STD
|
||||
#include <array>
|
||||
#include <vector>
|
||||
|
||||
#include "Blade.h"
|
||||
|
||||
namespace rm_power_rune
|
||||
{
|
||||
class PnPSolver
|
||||
{
|
||||
public:
|
||||
PnPSolver(
|
||||
const std::array<double, 9> & camera_matrix,
|
||||
const std::vector<double> & distortion_coefficients);
|
||||
|
||||
// Get 3d position
|
||||
bool solvePnP(const Blade & blade, cv::Mat & rvec, cv::Mat & tvec);
|
||||
|
||||
// Calculate the distance between armor center and image center
|
||||
float calculateDistanceToCenter(const cv::Point2f & image_point);
|
||||
|
||||
private:
|
||||
cv::Mat camera_matrix_;
|
||||
cv::Mat dist_coeffs_;
|
||||
|
||||
// Unit: mm
|
||||
static constexpr float SMALL_ARMOR_WIDTH = 135;
|
||||
static constexpr float SMALL_ARMOR_HEIGHT = 55;
|
||||
static constexpr float LARGE_ARMOR_WIDTH = 225;
|
||||
static constexpr float LARGE_ARMOR_HEIGHT = 55;
|
||||
|
||||
// Four vertices of armor in 3d
|
||||
std::vector<cv::Point3f> armor_points_;
|
||||
};
|
||||
|
||||
} // namespace rm_power_rune
|
||||
|
||||
#endif // PNP_SOLVER_HPP_
|
||||
|
|
@ -0,0 +1,65 @@
|
|||
#ifndef POWER_RUNE_RUNEDETECTOR_H
|
||||
#define POWER_RUNE_RUNEDETECTOR_H
|
||||
|
||||
// OpenCV
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/core/types.hpp>
|
||||
|
||||
// STD
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <chrono>
|
||||
|
||||
#include "Blade.h"
|
||||
#include "AdamFitter.h"
|
||||
|
||||
namespace rm_power_rune {
|
||||
class RuneDetector {
|
||||
public:
|
||||
RuneDetector(const int & bin_thres, const int & channel_thres, const int & color, const int & num_points);
|
||||
|
||||
std::vector<cv::Point> detect(const cv::Mat & input);
|
||||
|
||||
cv::Mat preprocessImage(const cv::Mat & input) const;
|
||||
std::pair<std::vector<FarEnd>, std::vector<NearEnd>> findRAndEnds(const cv::Mat & bin_img);
|
||||
static std::vector<Blade> matchEnds(std::vector<FarEnd> &far_ends, std::vector<NearEnd> &near_ends);
|
||||
void getAngularV(const std::vector<Blade>& blades);
|
||||
static void rotatePoints(const cv::Point& center, std::vector<cv::Point>& Points, double angle);
|
||||
|
||||
int binary_thres;
|
||||
int channel_thres;
|
||||
int detect_color;
|
||||
int num_points;
|
||||
|
||||
// For debug usage
|
||||
cv::Mat binary_img;
|
||||
|
||||
void drawEnds(cv::Mat & img);
|
||||
int drawResults(cv::Mat & img);
|
||||
|
||||
|
||||
private:
|
||||
int width_;
|
||||
int height_;
|
||||
|
||||
double distance_r_to_center_;
|
||||
double t_;
|
||||
double time_point_;
|
||||
|
||||
bool is_convergence_;
|
||||
std::vector<cv::Point> target_points_;
|
||||
|
||||
std::vector<double> vec_t_;
|
||||
std::vector<double> angular_v_;
|
||||
fitter::AdamFitter adam_fitter_;
|
||||
|
||||
cv::Point2f r_;
|
||||
std::pair<std::vector<FarEnd>, std::vector<NearEnd>> ends_;
|
||||
std::vector<FarEnd> far_ends_;
|
||||
std::vector<NearEnd> near_ends_;
|
||||
std::vector<Blade> blades_;
|
||||
std::vector<double> last_tilt_angles_;
|
||||
};
|
||||
}
|
||||
#endif //POWER_RUNE_RUNEDETECTOR_H
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,108 @@
|
|||
//
|
||||
// Created by sph on 2024/1/4.
|
||||
//
|
||||
|
||||
#include <iostream>
|
||||
#include "AdamFitter.h"
|
||||
|
||||
namespace fitter {
|
||||
AdamFitter::AdamFitter(double a, double omega, double phi, double b, int num_iterations, double beta1, double beta2,
|
||||
double learning_rate, double epsilon, double convergence_threshold, double m_a, double v_a,
|
||||
double m_omega, double v_omega, double m_phi, double v_phi, double m_b, double v_b) :
|
||||
a_(a),
|
||||
omega_(omega),
|
||||
phi_(phi),
|
||||
b_(b),
|
||||
num_iterations_(num_iterations),
|
||||
beta1_(beta1),
|
||||
beta2_(beta2),
|
||||
learning_rate_(learning_rate),
|
||||
epsilon_(epsilon),
|
||||
convergence_threshold_(convergence_threshold),
|
||||
m_a_(m_a),
|
||||
v_a_(v_a),
|
||||
m_omega_(m_omega),
|
||||
v_omega_(v_omega),
|
||||
m_phi_(m_phi),
|
||||
v_phi_(v_phi),
|
||||
m_b_(m_b),
|
||||
v_b_(v_b) {}
|
||||
|
||||
Eigen::ArrayXd fitter::AdamFitter::sine_function(const Eigen::ArrayXd &t, double a,
|
||||
double omega, double phi, double b) {
|
||||
Eigen::ArrayXd result = a * (omega * t + phi).array().sin() + b;
|
||||
return result;
|
||||
}
|
||||
|
||||
double AdamFitter::loss(const Eigen::ArrayXd &angular_velocity, const Eigen::ArrayXd &angular_velocity_pred) {
|
||||
double result = (angular_velocity - angular_velocity_pred).square().mean();
|
||||
return result;
|
||||
}
|
||||
|
||||
bool AdamFitter::Fitting(const Eigen::ArrayXd &t, const Eigen::ArrayXd &angular_velocity,
|
||||
std::vector<double> *result) {
|
||||
|
||||
for (int i = 0; i <= num_iterations_; i++) {
|
||||
//计算预测值、损失
|
||||
Eigen::ArrayXd angular_velocity_pred = sine_function(t, a_, omega_, phi_, b_);
|
||||
double current_loss = loss(angular_velocity, angular_velocity_pred);
|
||||
|
||||
//计算参数梯度
|
||||
Eigen::ArrayXd temp = 2 * (angular_velocity_pred - angular_velocity);
|
||||
Eigen::ArrayXd temp_angle = omega_ * t + phi_;
|
||||
double grad_a = (temp * temp_angle.sin()).mean();
|
||||
double grad_omega = (temp * a_ * t * temp_angle.cos()).mean();
|
||||
double grad_phi = (temp * a_ * temp_angle.cos()).mean();
|
||||
double grad_b = temp.mean();
|
||||
|
||||
//更新一阶矩
|
||||
m_a_ = beta1_ * m_a_ + (1 - beta1_) * grad_a;
|
||||
m_omega_ = beta1_ * m_omega_ + (1 - beta1_) * grad_omega;
|
||||
m_phi_ = beta1_ * m_phi_ + (1 - beta1_) * grad_phi;
|
||||
m_b_ = beta1_ * m_b_ + (1 - beta1_) * grad_b;
|
||||
|
||||
//更新二阶矩
|
||||
v_a_ = beta2_ * v_a_ + (1 - beta2_) * grad_a * grad_a;
|
||||
v_omega_ = beta2_ * v_omega_ + (1 - beta2_) * grad_omega * grad_omega;
|
||||
v_phi_ = beta2_ * v_phi_ + (1 - beta2_) * grad_phi * grad_phi;
|
||||
v_b_ = beta2_ * v_b_ + (1 - beta2_) * grad_b * grad_b;
|
||||
|
||||
//修正一阶矩和二阶矩的偏差
|
||||
double m_a_hat = m_a_ / (1 - pow(beta1_, (i + 1)));
|
||||
double m_omega_hat = m_omega_ / (1 - pow(beta1_, (i + 1)));
|
||||
double m_phi_hat = m_phi_ / (1 - pow(beta1_, (i + 1)));
|
||||
double m_b_hat = m_b_ / (1 - pow(beta1_, (i + 1)));
|
||||
double v_a_hat = v_a_ / (1 - pow(beta2_, (i + 1)));
|
||||
double v_omega_hat = v_omega_ / (1 - pow(beta2_, (i + 1)));
|
||||
double v_phi_hat = v_phi_ / (1 - pow(beta2_, (i + 1)));
|
||||
double v_b_hat = v_b_ / (1 - pow(beta2_, (i + 1)));
|
||||
|
||||
//更新参数
|
||||
a_ = std::min(std::max(a_ - learning_rate_ * m_a_hat / (std::sqrt(v_a_hat) + epsilon_), 0.780), 1.045);
|
||||
omega_ = std::min(
|
||||
std::max(omega_ - learning_rate_ * m_omega_hat / (std::sqrt(v_omega_hat) + epsilon_), 1.884),
|
||||
1.980);
|
||||
phi_ = std::min(std::max(phi_ - learning_rate_ * m_phi_hat / (std::sqrt(v_phi_hat) + epsilon_), 0.000),
|
||||
3.1415926);
|
||||
b_ -= learning_rate_ * m_b_hat / (std::sqrt(v_b_hat) + epsilon_);
|
||||
b_ = 0.8 * (b_ - learning_rate_ * m_b_hat / (std::sqrt(v_b_hat) + epsilon_)) + 0.2 * (2.090 - a_);
|
||||
//新的预测值
|
||||
Eigen::ArrayXd angular_velocity_hat = sine_function(t, a_, omega_, phi_, b_);
|
||||
|
||||
//判断收敛
|
||||
if (std::abs(current_loss - loss(angular_velocity, angular_velocity_hat)) < convergence_threshold_) {
|
||||
std::cout << "Converged after " << i + 1 << " iterations" << std::endl;
|
||||
result->emplace_back(a_);
|
||||
result->emplace_back(omega_);
|
||||
result->emplace_back(phi_);
|
||||
result->emplace_back(b_);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
result->emplace_back(a_);
|
||||
result->emplace_back(omega_);
|
||||
result->emplace_back(phi_);
|
||||
result->emplace_back(b_);
|
||||
return false;
|
||||
}
|
||||
} // fitter
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
// Copyright 2022 Chen Jun
|
||||
|
||||
#include "PnpSolver.hpp"
|
||||
|
||||
#include <opencv2/calib3d.hpp>
|
||||
#include <vector>
|
||||
|
||||
namespace rm_power_rune {
|
||||
PnPSolver::PnPSolver(
|
||||
const std::array<double, 9> &camera_matrix, const std::vector<double> &dist_coeffs)
|
||||
: camera_matrix_(cv::Mat(3, 3, CV_64F, const_cast<double *>(camera_matrix.data())).clone()),
|
||||
dist_coeffs_(cv::Mat(1, 5, CV_64F, const_cast<double *>(dist_coeffs.data())).clone()) {
|
||||
|
||||
//Todo: 世界坐标系待修改
|
||||
// Unit: m
|
||||
constexpr double small_half_y = SMALL_ARMOR_WIDTH / 2.0 / 1000.0;
|
||||
constexpr double small_half_z = SMALL_ARMOR_HEIGHT / 2.0 / 1000.0;
|
||||
|
||||
// Start from bottom left in clockwise order
|
||||
// Model coordinate: x forward, y left, z up
|
||||
armor_points_.emplace_back(0, small_half_y, -small_half_z);
|
||||
armor_points_.emplace_back(0, small_half_y, small_half_z);
|
||||
armor_points_.emplace_back(0, -small_half_y, small_half_z);
|
||||
armor_points_.emplace_back(0, -small_half_y, -small_half_z);
|
||||
}
|
||||
|
||||
bool PnPSolver::solvePnP(const Blade &blade, cv::Mat &rvec, cv::Mat &tvec) {
|
||||
std::vector<cv::Point2f> image_armor_points;
|
||||
|
||||
// Fill in image points
|
||||
image_armor_points.emplace_back(blade.bottom_left);
|
||||
image_armor_points.emplace_back(blade.top_left);
|
||||
image_armor_points.emplace_back(blade.bottom_right);
|
||||
image_armor_points.emplace_back(blade.bottom_left);
|
||||
|
||||
// Solve pnp
|
||||
auto object_points = armor_points_ ;
|
||||
return cv::solvePnP(
|
||||
object_points, image_armor_points, camera_matrix_, dist_coeffs_, rvec, tvec, false,
|
||||
cv::SOLVEPNP_IPPE);
|
||||
}
|
||||
|
||||
float PnPSolver::calculateDistanceToCenter(const cv::Point2f &image_point) {
|
||||
auto cx = static_cast<float>(camera_matrix_.at<double>(0, 2));
|
||||
auto cy = static_cast<float>(camera_matrix_.at<double>(1, 2));
|
||||
return static_cast<float>(cv::norm(image_point - cv::Point2f(cx, cy)));
|
||||
}
|
||||
|
||||
} // namespace rm_auto_aim
|
||||
|
|
@ -0,0 +1,385 @@
|
|||
#include "RuneDetector.h"
|
||||
|
||||
namespace rm_power_rune {
|
||||
RuneDetector::RuneDetector(const int &bin_thres, const int &channel_thres, const int &color, const int &num_points)
|
||||
: binary_thres(bin_thres), channel_thres(channel_thres),
|
||||
detect_color(color), num_points(num_points) {
|
||||
r_ = cv::Point2f(0, 0);
|
||||
width_ = 0;
|
||||
height_ = 0;
|
||||
|
||||
t_ = 0;
|
||||
is_convergence_ = false;
|
||||
|
||||
auto now = std::chrono::system_clock::now();
|
||||
std::chrono::duration<double> duration = now.time_since_epoch();
|
||||
time_point_ = duration.count();
|
||||
|
||||
distance_r_to_center_ = cv::norm(
|
||||
r_ - cv::Point2f(static_cast<float>(width_) / 2, static_cast<float>(height_) / 2));
|
||||
}
|
||||
|
||||
std::vector<cv::Point> rm_power_rune::RuneDetector::detect(const cv::Mat &input) {
|
||||
if (width_ == 0 && height_ == 0) {
|
||||
width_ = input.cols;
|
||||
height_ = input.rows;
|
||||
}
|
||||
|
||||
binary_img = preprocessImage(input);
|
||||
ends_ = findRAndEnds(binary_img);
|
||||
far_ends_ = ends_.first;
|
||||
near_ends_ = ends_.second;
|
||||
blades_ = matchEnds(far_ends_, near_ends_);
|
||||
getAngularV(blades_);
|
||||
|
||||
Eigen::ArrayXd t(num_points);
|
||||
Eigen::ArrayXd v;
|
||||
Eigen::ArrayXd y_pre;
|
||||
std::vector<double> result;
|
||||
|
||||
target_points_.clear();
|
||||
|
||||
if (!vec_t_.empty() || !angular_v_.empty()) {
|
||||
|
||||
t = Eigen::Map<Eigen::ArrayXd>(vec_t_.data(), static_cast<Eigen::Index>(vec_t_.size()));
|
||||
|
||||
v = Eigen::Map<Eigen::ArrayXd>(angular_v_.data(), static_cast<Eigen::Index>(angular_v_.size()));
|
||||
is_convergence_ = adam_fitter_.Fitting(t, v, &result);
|
||||
if (is_convergence_) {
|
||||
double a = result[0];
|
||||
double omega = result[1];
|
||||
double phi = result[2];
|
||||
double b = result[3];
|
||||
|
||||
double angle = (a * sin((omega * (vec_t_.back()) + phi)) + b)*0.1;
|
||||
|
||||
for(const auto& blade:blades_){
|
||||
if(!blade.is_activated){
|
||||
|
||||
target_points_.emplace_back(blade.top_left);
|
||||
target_points_.emplace_back(blade.top_right);
|
||||
target_points_.emplace_back(blade.bottom_right);
|
||||
target_points_.emplace_back(blade.bottom_left);
|
||||
|
||||
rotatePoints(r_, target_points_, -angle);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return {};
|
||||
}
|
||||
|
||||
cv::Mat rm_power_rune::RuneDetector::preprocessImage(const cv::Mat &input) const {
|
||||
|
||||
cv::Mat subtracted_img, gray_img, threshold_img;
|
||||
|
||||
cvtColor(input, gray_img, cv::COLOR_BGR2GRAY);
|
||||
threshold(gray_img, threshold_img, binary_thres, 255, cv::THRESH_BINARY);
|
||||
//通道相减 突出识别颜色
|
||||
std::vector<cv::Mat> channels;
|
||||
split(input, channels);
|
||||
if (detect_color == RED)
|
||||
subtract(channels[2], channels[0], subtracted_img);
|
||||
else if (detect_color == BLUE)
|
||||
subtract(channels[0], channels[2], subtracted_img);
|
||||
|
||||
//图像预处理参数 二值化、膨胀、形态学闭操作
|
||||
threshold(subtracted_img, subtracted_img, channel_thres, 255, cv::THRESH_BINARY);
|
||||
|
||||
cv::Mat bin_img;
|
||||
//与操作消除白色灯条与蓝色灯条相连部分
|
||||
bin_img = (subtracted_img & threshold_img);
|
||||
|
||||
|
||||
cv::Mat kernel = cv::getStructuringElement(0, cv::Size(2, 2));
|
||||
dilate(bin_img, bin_img, kernel, cv::Point(-1, -1), 1);
|
||||
|
||||
kernel = cv::getStructuringElement(0, cv::Size(7, 7));
|
||||
morphologyEx(bin_img, bin_img, cv::MORPH_CLOSE, kernel);
|
||||
|
||||
return bin_img;
|
||||
}
|
||||
|
||||
std::pair<std::vector<FarEnd>, std::vector<NearEnd>> RuneDetector::findRAndEnds(const cv::Mat &bin_img) {
|
||||
std::vector<FarEnd> far_ends;
|
||||
std::vector<NearEnd> near_ends;
|
||||
|
||||
using std::vector;
|
||||
|
||||
//轮廓及其层次结构
|
||||
vector<vector<cv::Point>> contours;
|
||||
vector<cv::Vec4i> hierarchy;
|
||||
|
||||
cv::findContours(bin_img, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);
|
||||
|
||||
int contour_index = 0;
|
||||
for (const auto &contour: contours) {
|
||||
|
||||
//只检测5个点以上且最外层轮廓
|
||||
if (contour.size() < 5) {
|
||||
contour_index++;
|
||||
continue;
|
||||
}
|
||||
if (hierarchy[contour_index][3] != -1) {
|
||||
contour_index++;
|
||||
continue;
|
||||
}
|
||||
|
||||
auto r_rect = cv::minAreaRect(contour);
|
||||
|
||||
float area_rect = r_rect.size.area();
|
||||
if (area_rect >= 10000.0 || area_rect < 80) {
|
||||
contour_index++;
|
||||
continue;
|
||||
}
|
||||
|
||||
float width_rect;
|
||||
float height_rect;
|
||||
//判断长宽比
|
||||
if (r_rect.size.width > r_rect.size.height) {
|
||||
width_rect = r_rect.size.width;
|
||||
height_rect = r_rect.size.height;
|
||||
} else {
|
||||
width_rect = r_rect.size.height;
|
||||
height_rect = r_rect.size.width;
|
||||
}
|
||||
|
||||
if (width_rect / height_rect < 4) {
|
||||
if (area_rect > 80 && area_rect < 600) { // find R
|
||||
if (height_rect / width_rect > 0.8) {
|
||||
if (r_.x == 0 && r_.y == 0) {
|
||||
r_ = r_rect.center;
|
||||
distance_r_to_center_ = cv::norm(
|
||||
r_ - cv::Point2f(static_cast<float>(width_) / 2, static_cast<float>(height_) / 2));
|
||||
} else {
|
||||
double distance = cv::norm(r_rect.center - cv::Point2f(static_cast<float>(width_) / 2,
|
||||
static_cast<float>(height_) / 2));
|
||||
if (abs(distance - distance_r_to_center_) < 100) {
|
||||
r_ = r_rect.center;
|
||||
distance_r_to_center_ = distance;
|
||||
} else std::cout << "R jump!" << std::endl;
|
||||
}
|
||||
}
|
||||
} else { //find ends
|
||||
// find NearEnd
|
||||
if (area_rect > 4000 && area_rect < 6500 && width_rect / height_rect > 0.9 &&
|
||||
width_rect / height_rect < 1.6) {
|
||||
if (hierarchy[contour_index][2] != -1) {
|
||||
auto near_end = NearEnd(r_rect, true, r_);
|
||||
near_ends.emplace_back(near_end);
|
||||
} else {
|
||||
auto near_end = NearEnd(r_rect, false, r_);
|
||||
near_ends.emplace_back(near_end);
|
||||
}
|
||||
}
|
||||
|
||||
// find FarEnd
|
||||
if (area_rect > 800 && area_rect < 1500 && width_rect / height_rect > 1.8 &&
|
||||
width_rect / height_rect < 3.5) {
|
||||
auto far_end = FarEnd(r_rect, r_);
|
||||
far_ends.emplace_back(far_end);
|
||||
}
|
||||
}
|
||||
}
|
||||
contour_index++;
|
||||
}
|
||||
return std::make_pair(far_ends, near_ends);
|
||||
}
|
||||
|
||||
std::vector<Blade>
|
||||
rm_power_rune::RuneDetector::matchEnds(std::vector<FarEnd> &far_ends, std::vector<NearEnd> &near_ends) {
|
||||
std::vector<Blade> blades;
|
||||
|
||||
if (far_ends.size() != near_ends.size()) {
|
||||
std::cout << "Number mismatch between far_ends and near_ends" << std::endl;
|
||||
}
|
||||
|
||||
std::sort(far_ends.begin(), far_ends.end(),
|
||||
[](const FarEnd &a, const FarEnd &b) { return a.tilt_angle < b.tilt_angle; });
|
||||
std::sort(near_ends.begin(), near_ends.end(),
|
||||
[](const NearEnd &a, const NearEnd &b) { return a.tilt_angle < b.tilt_angle; });
|
||||
|
||||
for (const auto &near_end: near_ends) {
|
||||
for (const auto &far_end: far_ends) {
|
||||
if ((near_end.tilt_angle < 3 && far_end.tilt_angle > 357) ||
|
||||
(far_end.tilt_angle < 3 && near_end.tilt_angle > 357)) {
|
||||
Blade blade(near_end, far_end);
|
||||
blades.emplace_back(blade);
|
||||
break;
|
||||
}
|
||||
if (abs(near_end.tilt_angle - far_end.tilt_angle) < 5) {
|
||||
Blade blade(near_end, far_end);
|
||||
blades.emplace_back(blade);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
return blades;
|
||||
}
|
||||
|
||||
void RuneDetector::getAngularV(const std::vector<Blade> &blades) {
|
||||
std::vector<double> delta_angles;
|
||||
|
||||
if (last_tilt_angles_.empty()) {
|
||||
for (const auto &blade: blades) {
|
||||
last_tilt_angles_.emplace_back(blade.tilt_angle);
|
||||
}
|
||||
auto now = std::chrono::system_clock::now();
|
||||
std::chrono::duration<double> duration = now.time_since_epoch();
|
||||
time_point_ = duration.count();
|
||||
t_ = 0;
|
||||
angular_v_.clear();
|
||||
vec_t_.clear();
|
||||
|
||||
} else {
|
||||
double delta_angle;
|
||||
for (double last_tilt_angle: last_tilt_angles_) {
|
||||
for (const auto &blade: blades) {
|
||||
if (last_tilt_angle > 355 && blade.tilt_angle < 5) {
|
||||
delta_angle = 360 - last_tilt_angle + blade.tilt_angle;
|
||||
delta_angles.push_back(delta_angle);
|
||||
} else if (abs(blade.tilt_angle - last_tilt_angle) < 5) {
|
||||
delta_angle =
|
||||
(blade.tilt_angle - last_tilt_angle) > 0 ? (blade.tilt_angle - last_tilt_angle) : 0.01;
|
||||
delta_angles.push_back(delta_angle);
|
||||
} else if (blades.size() == 1) {
|
||||
std::cout << "blade jump" << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
last_tilt_angles_.clear();
|
||||
for (const auto &blade: blades) {
|
||||
last_tilt_angles_.emplace_back(blade.tilt_angle);
|
||||
}
|
||||
double sum = 0;
|
||||
|
||||
for (auto delta_a: delta_angles) {
|
||||
sum += delta_a;
|
||||
}
|
||||
double delta_angle_avg = (sum / static_cast<double> (delta_angles.size())) * CV_PI / 180;
|
||||
|
||||
auto now = std::chrono::system_clock::now();
|
||||
std::chrono::duration<double> duration = now.time_since_epoch();
|
||||
double delta_t = duration.count() - time_point_;
|
||||
time_point_ = duration.count();
|
||||
t_ += delta_t;
|
||||
|
||||
double angular_v;
|
||||
angular_v = delta_angle_avg / delta_t < 2.09 ? delta_angle_avg / delta_t : 2.09;
|
||||
|
||||
if (vec_t_.size() < num_points) {
|
||||
vec_t_.emplace_back(t_);
|
||||
angular_v_.emplace_back(angular_v);
|
||||
} else {
|
||||
vec_t_.erase(vec_t_.begin());
|
||||
angular_v_.erase(angular_v_.begin());
|
||||
vec_t_.emplace_back(t_);
|
||||
angular_v_.emplace_back(angular_v);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void RuneDetector::rotatePoints(const cv::Point ¢er, std::vector<cv::Point> &Points, double angle) {
|
||||
// 对每个点执行旋转,其实对输入点顺序并无要求。。。仅是对输入的所有点绕center做相同角度的旋转
|
||||
Eigen::Vector2d eigenCenter(center.x, center.y);
|
||||
Eigen::Vector2d LeftTopPoint(Points[0].x, (Points[0].y));
|
||||
Eigen::Vector2d RightTopPoint(Points[1].x, (Points[1].y));
|
||||
Eigen::Vector2d RightLowPoint(Points[2].x, (Points[2].y));
|
||||
Eigen::Vector2d LeftLowTopPoint(Points[3].x, (Points[3].y));
|
||||
|
||||
Eigen::Transform<double, 2, Eigen::Affine> transform;
|
||||
transform = Eigen::Translation2d(eigenCenter) * Eigen::Rotation2Dd(angle) * Eigen::Translation2d(-eigenCenter);
|
||||
|
||||
LeftTopPoint = transform * LeftTopPoint;
|
||||
RightTopPoint = transform * RightTopPoint;
|
||||
RightLowPoint = transform * RightLowPoint;
|
||||
LeftLowTopPoint = transform * LeftLowTopPoint;
|
||||
|
||||
Points.clear();
|
||||
Points.emplace_back(LeftTopPoint.x(), LeftTopPoint.y());
|
||||
Points.emplace_back(RightTopPoint.x(), RightTopPoint.y());
|
||||
Points.emplace_back(RightLowPoint.x(), RightLowPoint.y());
|
||||
Points.emplace_back(LeftLowTopPoint.x(), LeftLowTopPoint.y());
|
||||
}
|
||||
|
||||
void RuneDetector::drawEnds(cv::Mat &img) {
|
||||
using std::vector;
|
||||
std::cout << "##################" << std::endl;
|
||||
//draw far_ends
|
||||
for (auto far_end: far_ends_) {
|
||||
vector<cv::Point> p(far_end.p, far_end.p + 4);
|
||||
vector<vector<cv::Point>> temp;
|
||||
temp.emplace_back(p);
|
||||
drawContours(img, temp, -1, cv::Scalar(0, 0, 255), 2);
|
||||
cv::circle(img, far_end.corner_point_0, 5, cv::Scalar(0, 255, 255));
|
||||
cv::circle(img, far_end.corner_point_1, 5, cv::Scalar(0, 255, 255));
|
||||
|
||||
cv::putText(img, "0", far_end.corner_point_0, 0, 0.6, cv::Scalar(0, 255, 255));
|
||||
cv::putText(img, "1", far_end.corner_point_1, 0, 0.6, cv::Scalar(0, 255, 255));
|
||||
std::cout << "far: " << far_end.tilt_angle << std::endl;
|
||||
}
|
||||
|
||||
//draw near_ends
|
||||
for (auto near_end: near_ends_) {
|
||||
vector<cv::Point> p(near_end.p, near_end.p + 4);
|
||||
vector<vector<cv::Point>> temp;
|
||||
temp.emplace_back(p);
|
||||
if (near_end.is_activated)
|
||||
drawContours(img, temp, -1, cv::Scalar(0, 255, 0), 2);
|
||||
else
|
||||
drawContours(img, temp, -1, cv::Scalar(0, 0, 255), 2);
|
||||
|
||||
cv::circle(img, near_end.corner_point_2, 5, cv::Scalar(0, 255, 255));
|
||||
cv::circle(img, near_end.corner_point_3, 5, cv::Scalar(0, 255, 255));
|
||||
|
||||
cv::putText(img, "2", near_end.corner_point_2, 0, 0.6, cv::Scalar(0, 255, 255));
|
||||
cv::putText(img, "3", near_end.corner_point_3, 0, 0.6, cv::Scalar(0, 255, 255));
|
||||
std::cout << "near: " << near_end.tilt_angle << std::endl;
|
||||
}
|
||||
std::cout << "##################" << std::endl;
|
||||
}
|
||||
|
||||
int RuneDetector::drawResults(cv::Mat &img) {
|
||||
using std::vector;
|
||||
|
||||
//draw R
|
||||
cv::circle(img, r_, 15, cv::Scalar(0, 255, 255));
|
||||
//std::cout << r_ << std::endl;
|
||||
|
||||
//drawEnds(img);
|
||||
|
||||
//draw blades
|
||||
for (const auto &blade: blades_) {
|
||||
vector<cv::Point> p;
|
||||
p.emplace_back(blade.top_left);
|
||||
p.emplace_back(blade.top_right);
|
||||
p.emplace_back(blade.bottom_right);
|
||||
p.emplace_back(blade.bottom_left);
|
||||
|
||||
vector<vector<cv::Point>> temp;
|
||||
temp.emplace_back(p);
|
||||
|
||||
if (blade.is_activated) {
|
||||
drawContours(img, temp, -1, cv::Scalar(0, 255, 0), 2);
|
||||
cv::circle(img, blade.center, 3, cv::Scalar(0, 255, 0), 2);
|
||||
} else {
|
||||
drawContours(img, temp, -1, cv::Scalar(0, 0, 255), 2);
|
||||
cv::circle(img, blade.center, 3, cv::Scalar(0, 0, 255), 2);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (is_convergence_ && !target_points_.empty()) {
|
||||
vector<vector<cv::Point>> a;
|
||||
a.emplace_back(target_points_);
|
||||
drawContours(img, a, -1, cv::Scalar(0, 255, 255), 2);
|
||||
}
|
||||
|
||||
imshow("RawImage", img);
|
||||
//imshow("BinaryImage", binary_img);
|
||||
|
||||
int key = cv::waitKey(6);
|
||||
return key;
|
||||
}
|
||||
}// namespace rm_power_rune
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#include "RuneDetector.h"
|
||||
|
||||
int main() {
|
||||
cv::VideoCapture cap(R"(../video/blue.mp4)");
|
||||
|
||||
if (!cap.isOpened()) {
|
||||
std::cout << "Error opening video stream or file" << std::endl;
|
||||
}
|
||||
|
||||
rm_power_rune::RuneDetector runeDetector(180, 100, rm_power_rune::BLUE, 180);
|
||||
|
||||
while (cap.isOpened()) {
|
||||
// Initialise frame matrix
|
||||
cv::Mat rawImages;
|
||||
|
||||
// Initialize a boolean to check if frames are there or not
|
||||
bool isSuccess = cap.read(rawImages);
|
||||
|
||||
// If frames are present, show it
|
||||
if (isSuccess) {
|
||||
// 开始计时
|
||||
auto startTime = std::chrono::steady_clock::now();
|
||||
|
||||
//display frames
|
||||
runeDetector.detect(rawImages);
|
||||
int key = runeDetector.drawResults(rawImages);
|
||||
if (key == 'q') break;
|
||||
|
||||
// 计算总帧时间
|
||||
auto endTime = std::chrono::steady_clock::now();
|
||||
auto elapsedTime = std::chrono::duration<double>(endTime - startTime).count();
|
||||
|
||||
// 如果时间间隔达到了设定的秒数，计算并输出帧率
|
||||
double fps = 1 / elapsedTime;
|
||||
std::cout << "FPS: " << fps << std::endl;
|
||||
} else {
|
||||
std::cout << "Video camera is disconnected" << std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
// Release the video capture object
|
||||
cap.release();
|
||||
cv::destroyAllWindows();
|
||||
return 0;
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 109 KiB |
Binary file not shown.
Loading…
Reference in New Issue