Skip to content

Commit

Permalink
🔧 移除数据组件冗余的构造函数，将大部分构造操作移动至对应的工厂函数
Browse files Browse the repository at this point in the history
  • Loading branch information
@zhaoxi-scut @TooPretty0108
zhaoxi-scut authored and TooPretty0108 committed Feb 14, 2025
1 parent a8ec59b commit 516ab61
Show file tree
Hide file tree
Showing 14 changed files with 248 additions and 314 deletions.
5 changes: 1 addition & 4 deletions extra/combo/include/rmvl/combo/armor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ class RMVL_EXPORTS_W_DES Armor final : public combo

//! @cond
Armor() = default;
Armor(LightBlob::ptr, LightBlob::ptr, const ImuData &, double, float, float, float, float, float, float, float, ArmorSizeType);
Armor(float, float, float, float);
//! @endcond

/**
Expand Down Expand Up @@ -152,9 +152,6 @@ class RMVL_EXPORTS_W_DES Armor final : public combo
RMVL_W const cv::Vec2f &getPose() const { return _pose; }

private:
//! 用来确定装甲板的种类 (大装甲或者小装甲)
ArmorSizeType matchArmorType();

float _combo_ratio{}; //!< 组合特征宽高比
float _width_ratio{}; //!< 左右灯条宽度比
float _length_ratio{}; //!< 左右灯条长度比
Expand Down
141 changes: 81 additions & 60 deletions extra/combo/src/armor/armor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,37 @@ RobotType to_robot_type(const StateType &type)
return RobotType::UNKNOWN;
}

/**
* @brief 获取装甲板的位姿
*
* @param[in] cam_matrix 相机内参，用于解算相机外参
* @param[in] distCoeffs 相机畸变参数，用于解算相机外参
* @param[in] imu_data IMU 数据
* @param[in] type 装甲板类型
* @param[in] corners 装甲板角点
* @return CameraExtrinsics - 相机外参
*/
static CameraExtrinsics calculateExtrinsic(const cv::Matx33f &cameraMatrix, const cv::Matx51f &distCoeffs, const ImuData &imu_data,
ArmorSizeType type, const std::vector<cv::Point2f> &corners)
{
cv::Vec3f rvec; // 旋转向量
cv::Vec3f tvec; // 平移向量
CameraExtrinsics extrinsic; // 存储相机外参
type == ArmorSizeType::SMALL
? cv::solvePnP(para::armor_param.SMALL_ARMOR, corners, cameraMatrix, distCoeffs, rvec, tvec, false, cv::SOLVEPNP_IPPE)
: cv::solvePnP(para::armor_param.BIG_ARMOR, corners, cameraMatrix, distCoeffs, rvec, tvec, false, cv::SOLVEPNP_IPPE);
// 变换为 IMU 坐标系下
cv::Matx33f rmat;
cv::Rodrigues(rvec, rmat);
cv::Matx33f gyro_rmat;
cv::Vec3f gyro_tvec;
Armor::cameraConvertToImu(rmat, tvec, imu_data, gyro_rmat, gyro_tvec);
extrinsic.R(gyro_rmat);
extrinsic.tvec(gyro_tvec);

return extrinsic;
}

std::shared_ptr<Armor> Armor::make_combo(LightBlob::ptr p_left, LightBlob::ptr p_right, const ImuData &imu_data,
double tick, ArmorSizeType armor_size_type)
{
Expand Down Expand Up @@ -150,81 +181,71 @@ std::shared_ptr<Armor> Armor::make_combo(LightBlob::ptr p_left, LightBlob::ptr p
return nullptr;
}

return std::make_shared<Armor>(p_left, p_right, imu_data, tick,
width_ratio, length_ratio, corner_angle, match_error,
combo_height, combo_width, combo_ratio, armor_size_type);
}

/**
* @brief 获取装甲板的位姿
*
* @param[in] cam_matrix 相机内参，用于解算相机外参
* @param[in] distCoeffs 相机畸变参数，用于解算相机外参
* @param[in] imu_data IMU 数据
* @param[in] type 装甲板类型
* @param[in] corners 装甲板角点
* @return CameraExtrinsics - 相机外参
*/
static CameraExtrinsics calculateExtrinsic(const cv::Matx33f &cameraMatrix, const cv::Matx51f &distCoeffs, const ImuData &imu_data,
ArmorSizeType type, const std::vector<cv::Point2f> &corners)
{
cv::Vec3f rvec; // 旋转向量
cv::Vec3f tvec; // 平移向量
CameraExtrinsics extrinsic; // 存储相机外参
type == ArmorSizeType::SMALL
? cv::solvePnP(para::armor_param.SMALL_ARMOR, corners, cameraMatrix, distCoeffs, rvec, tvec, false, cv::SOLVEPNP_IPPE)
: cv::solvePnP(para::armor_param.BIG_ARMOR, corners, cameraMatrix, distCoeffs, rvec, tvec, false, cv::SOLVEPNP_IPPE);
// 变换为 IMU 坐标系下
cv::Matx33f rmat;
cv::Rodrigues(rvec, rmat);
cv::Matx33f gyro_rmat;
cv::Vec3f gyro_tvec;
Armor::cameraConvertToImu(rmat, tvec, imu_data, gyro_rmat, gyro_tvec);
extrinsic.R(gyro_rmat);
extrinsic.tvec(gyro_tvec);

return extrinsic;
}

void Armor::setType(RobotType stat) { _state["robot"] = to_string(stat); }
auto retval = std::make_shared<Armor>(width_r, length_r, corner_angle, match_error);

Armor::Armor(LightBlob::ptr p_left, LightBlob::ptr p_right, const ImuData &imu_data, double tick,
float width_r, float length_r, float corner_angle, float match_error,
float combo_h, float combo_w, float combo_r, ArmorSizeType armor_size_type)
: _width_ratio(width_r), _length_ratio(length_r), _corner_angle(corner_angle), _match_error(match_error)
{
_height = combo_h;
_width = combo_w;
_combo_ratio = combo_r;
retval->_height = combo_height;
retval->_width = combo_width;
retval->_combo_ratio = combo_ratio;
// 获取装甲板中心
_center = (p_left->center() + p_right->center()) / 2.f;
retval->_center = (p_left->center() + p_right->center()) / 2.f;
// 获取相对角度
_relative_angle = calculateRelativeAngle(para::camera_param.cameraMatrix, _center);
retval->_relative_angle = calculateRelativeAngle(para::camera_param.cameraMatrix, retval->_center);
// 获取当前装甲板对应的 IMU 位置信息
_imu_data = imu_data;
retval->_imu_data = imu_data;
// 装甲板角度
_angle = (p_left->angle() + p_right->angle()) / 2.f;
retval->_angle = (p_left->angle() + p_right->angle()) / 2.f;
// 匹配大小装甲板
ArmorSizeType armor_size{};
if (armor_size_type == ArmorSizeType::UNKNOWN)
armor_size = matchArmorType();
{
if (_svm != nullptr)
{
cv::Matx15f test_sample = {combo_ratio, // 装甲板长宽比
width_ratio, // 灯条宽度比
length_ratio}; // 灯条长度比
auto res = _svm->predict(test_sample);
armor_size = (res == 1) ? ArmorSizeType::SMALL : ArmorSizeType::BIG;
}
// 装甲板长宽比例符合
if (combo_ratio < para::armor_param.MAX_SMALL_COMBO_RATIO)
armor_size = ArmorSizeType::SMALL;
else if (combo_ratio > para::armor_param.MIN_BIG_COMBO_RATIO)
armor_size = ArmorSizeType::BIG;
// 错位角判断
if (abs(corner_angle) < para::armor_param.MAX_SMALL_CORNER_ANGLE)
armor_size = ArmorSizeType::SMALL;
else if (abs(corner_angle) > para::armor_param.MIN_BIG_CORNER_ANGLE)
armor_size = ArmorSizeType::BIG;
// 宽度比例判断
if (width_ratio < para::armor_param.BIG_SMALL_WIDTH_RATIO)
armor_size = ArmorSizeType::SMALL; // 装甲板长宽比例较小
else
armor_size = ArmorSizeType::BIG;
}
else
armor_size = armor_size_type;
_state["armor_size"] = to_string(armor_size);
retval->_state["armor_size"] = to_string(armor_size);
// 更新角点
_corners = {p_left->getBottomPoint(), // 左下
p_left->getTopPoint(), // 左上
p_right->getTopPoint(), // 右上
p_right->getBottomPoint()}; // 右下
retval->_corners = {p_left->getBottomPoint(), // 左下
p_left->getTopPoint(), // 左上
p_right->getTopPoint(), // 右上
p_right->getBottomPoint()}; // 右下
// 计算相机外参
_extrinsic = calculateExtrinsic(para::camera_param.cameraMatrix, para::camera_param.distCoeffs, imu_data, armor_size, _corners);
const auto &rmat = _extrinsic.R();
_pose = cv::normalize(cv::Vec2f(rmat(0, 2), rmat(2, 2)));
retval->_extrinsic = calculateExtrinsic(para::camera_param.cameraMatrix, para::camera_param.distCoeffs, imu_data, armor_size, retval->_corners);
const auto &rmat = retval->_extrinsic.R();
retval->_pose = cv::normalize(cv::Vec2f(rmat(0, 2), rmat(2, 2)));
// 设置组合体的特征容器
_features = {p_left, p_right};
_tick = tick;
retval->_features = {p_left, p_right};
retval->_tick = tick;

return retval;
}

void Armor::setType(RobotType stat) { _state["robot"] = to_string(stat); }

Armor::Armor(float width_r, float length_r, float corner_angle, float match_error)
: _width_ratio(width_r), _length_ratio(length_r), _corner_angle(corner_angle), _match_error(match_error) {}

combo::ptr Armor::clone(double tick)
{
auto retval = std::make_shared<Armor>(*this);
Expand Down
27 changes: 0 additions & 27 deletions extra/combo/src/armor/armor_cal.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,33 +19,6 @@
namespace rm
{

ArmorSizeType Armor::matchArmorType()
{
if (_svm != nullptr)
{
cv::Matx15f test_sample = {_combo_ratio, // 装甲板长宽比
_width_ratio, // 灯条宽度比
_length_ratio}; // 灯条长度比
auto res = _svm->predict(test_sample);
return res == 1 ? ArmorSizeType::SMALL : ArmorSizeType::BIG;
}
// 装甲板长宽比例符合
if (_combo_ratio < para::armor_param.MAX_SMALL_COMBO_RATIO)
return ArmorSizeType::SMALL;
else if (_combo_ratio > para::armor_param.MIN_BIG_COMBO_RATIO)
return ArmorSizeType::BIG;
// 错位角判断
if (abs(_corner_angle) < para::armor_param.MAX_SMALL_CORNER_ANGLE)
return ArmorSizeType::SMALL;
else if (abs(_corner_angle) > para::armor_param.MIN_BIG_CORNER_ANGLE)
return ArmorSizeType::BIG;
// 宽度比例判断
if (_width_ratio < para::armor_param.BIG_SMALL_WIDTH_RATIO)
return ArmorSizeType::SMALL; // 装甲板长宽比例较小
else
return ArmorSizeType::BIG;
}

void Armor::imuConvertToCamera(const cv::Matx33f &gyro_rmat, const cv::Vec3f &gyro_tvec,
const ImuData &imu_data, cv::Matx33f &cam_rmat, cv::Vec3f &cam_tvec)
{
Expand Down
21 changes: 3 additions & 18 deletions extra/feature/include/rmvl/feature/light_blob.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,20 +25,10 @@ namespace rm
*/
class RMVL_EXPORTS_W_DES LightBlob : public feature
{
cv::RotatedRect _rotated_rect; //!< 旋转矩形
cv::Point2f _top; //!< 上顶点
cv::Point2f _bottom; //!< 下顶点

public:
using ptr = std::shared_ptr<LightBlob>;
using const_ptr = std::shared_ptr<const LightBlob>;

//! @cond
LightBlob() = default;
LightBlob(const cv::Point2f &, const cv::Point2f &, float);
LightBlob(const std::vector<cv::Point> &, cv::RotatedRect &, float, float);
//! @endcond

//! 获取灯条顶端点
RMVL_W inline const cv::Point2f &getTopPoint() const { return _top; }
//! 获取灯条底端点
Expand All @@ -60,7 +50,7 @@ class RMVL_EXPORTS_W_DES LightBlob : public feature
* @param[in] width 灯条宽度
* @return 若构造成功则返回 LightBlob 的共享指针，否则返回 nullptr
*/
RMVL_W static inline ptr make_feature(const cv::Point2f &top, const cv::Point2f &bottom, float width) { return std::make_shared<LightBlob>(top, bottom, width); }
RMVL_W static ptr make_feature(const cv::Point2f &top, const cv::Point2f &bottom, float width);

/**
* @brief 从另一个特征进行构造
Expand All @@ -72,13 +62,8 @@ class RMVL_EXPORTS_W_DES LightBlob : public feature
RMVL_FEATURE_CAST(LightBlob)

private:
/**
* @brief 计算准确的特征信息
*
* @param[in] lw_ratio 长宽比
* @param[in] contour 轮廓点
*/
void calcAccurateInfo(float lw_ratio, const std::vector<cv::Point> &contour);
cv::Point2f _top; //!< 上顶点
cv::Point2f _bottom; //!< 下顶点
};

//! @} light_blob
Expand Down
27 changes: 7 additions & 20 deletions extra/feature/include/rmvl/feature/pilot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,20 +25,10 @@ namespace rm
*/
class RMVL_EXPORTS_W_DES Pilot : public feature
{
private:
cv::RotatedRect _rotated_rect; //!< 旋转矩形
cv::Point2f _left; //!< 左顶点
cv::Point2f _right; //!< 右顶点

public:
using ptr = std::shared_ptr<Pilot>;
using const_ptr = std::shared_ptr<const Pilot>;

//! @cond
Pilot(const std::vector<cv::Point> &, cv::RotatedRect &, float, float);
Pilot(const float &, const float &, const cv::Point2f &, const float &, const std::vector<cv::Point2f> &);
//! @endcond

/**
* @brief 构造接口
*
Expand All @@ -51,17 +41,14 @@ class RMVL_EXPORTS_W_DES Pilot : public feature
/**
* @brief 构造 Pilot
*
* @param[in] ref_width 参考宽度
* @param[in] ref_height 参考高度
* @param[in] ref_center 参考中心点
* @param[in] ref_angle 参考角度
* @param[in] ref_corners 参考角点
* @return 返回 Pilot 的共享指针
* @param[in] w 参考宽度
* @param[in] h 参考高度
* @param[in] center 参考中心点
* @param[in] angle 参考角度
* @param[in] corners 参考角点
* @return Pilot 的共享指针
*/
RMVL_W static inline ptr make_feature(float ref_width, float ref_height, const cv::Point2f &ref_center, float ref_angle, const std::vector<cv::Point2f> &ref_corners)
{
return std::make_shared<Pilot>(ref_width, ref_height, ref_center, ref_angle, ref_corners);
}
RMVL_W static ptr make_feature(float w, float h, const cv::Point2f &center, float angle, const std::vector<cv::Point2f> &corners);

/**
* @brief 从另一个特征进行构造
Expand Down
12 changes: 1 addition & 11 deletions extra/feature/include/rmvl/feature/rune_center.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,27 +22,17 @@ namespace rm
//! 神符中心特征
class RMVL_EXPORTS_W_DES RuneCenter : public feature
{
private:
std::vector<cv::Point> _contour; //!< 轮廓点集
cv::RotatedRect _rotated_rect; //!< 旋转矩形
float _ratio{}; //!< 长宽比

public:
using ptr = std::shared_ptr<RuneCenter>;
using const_ptr = std::shared_ptr<const RuneCenter>;

//! @cond
RuneCenter(const std::vector<cv::Point> &, cv::RotatedRect &);
RuneCenter(const cv::Point2f &);
//! @endcond

/**
* @brief 使用特征中心点构造 RuneCenter 的构造接口
*
* @param[in] center 特征中心点
* @return 如果成功，返回 RuneCenter 的共享指针，否则返回 nullptr
*/
RMVL_W static inline ptr make_feature(const cv::Point2f &center) { return std::make_shared<RuneCenter>(center); }
RMVL_W static ptr make_feature(const cv::Point2f &center);

/**
* @brief 使用轮廓和层次结构构造 RuneCenter 的构造接口
Expand Down
10 changes: 3 additions & 7 deletions extra/feature/include/rmvl/feature/rune_target.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@ class RMVL_EXPORTS_W_DES RuneTarget : public feature

//! @cond
RuneTarget() = default;
RuneTarget(const std::vector<cv::Point> &, const cv::RotatedRect &, bool is_active);
RuneTarget(const cv::Point &center, bool is_active);
//! @endcond

Expand All @@ -48,7 +47,7 @@ class RMVL_EXPORTS_W_DES RuneTarget : public feature
* @param[in] is_active 是否激活？
* @return 如果成功，返回 RuneTarget 的共享指针，否则返回 nullptr
*/
RMVL_W static ptr make_feature(const cv::Point &center, bool is_active);
RMVL_W static ptr make_feature(const cv::Point &center, bool is_active) { return std::make_shared<RuneTarget>(center, is_active); }

/**
* @brief 从另一个特征进行构造
Expand All @@ -65,11 +64,8 @@ class RMVL_EXPORTS_W_DES RuneTarget : public feature
RMVL_W inline float getRadius() const { return _radius; }

private:
std::vector<cv::Point> _contour; //!< 轮廓点集
cv::RotatedRect _rotated_rect; //!< 旋转矩形
float _ratio{}; //!< 长宽比
bool _is_active{}; //!< 是否处于激活状态
float _radius{}; //!< 半径
bool _is_active{}; //!< 是否处于激活状态
float _radius{}; //!< 半径
};

//! @} rune_target
Expand Down
Loading

0 comments on commit 516ab61

Please sign in to comment.