48 lines
1.0 KiB
C++
48 lines
1.0 KiB
C++
|
// 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_
|