add wust typr mpc and mutipule x
This commit is contained in:
cyy_mac
344
wust_vision-main/tasks/auto_sniper/auto_sniper.cpp
Normal file
344
wust_vision-main/tasks/auto_sniper/auto_sniper.cpp
Normal file
@@ -0,0 +1,344 @@
|
||||
#include "3rdparty/angles.h"
|
||||
//#ifdef USE_ROS2
|
||||
#ifdef FUCK
|
||||
#include "auto_sniper.hpp"
|
||||
#include "ros2/tf.hpp"
|
||||
|
||||
#include <Eigen/src/Core/Matrix.h>
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <open3d/utility/Eigen.h>
|
||||
#include <optional>
|
||||
#include <rclcpp/logger.hpp>
|
||||
#include <rclcpp/logging.hpp>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <open3d/Open3D.h>
|
||||
#include <rclcpp/node.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include "k1_solver.hpp"
|
||||
#include "offset_helper.hpp"
|
||||
#include "tasks/type_common.hpp"
|
||||
#include "tasks/utils/config.hpp"
|
||||
#include "voxel_map.hpp"
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <sensor_msgs/point_cloud2_iterator.hpp>
|
||||
#include <visualization_msgs/msg/marker.hpp>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
namespace wust_vision::auto_sniper {
|
||||
|
||||
struct AutoSniper::Impl {
|
||||
Impl(
|
||||
rclcpp::Node& node,
|
||||
std::shared_ptr<wust_vl::common::utils::MotionBufferGeneric<Motion, 1024>> motion_buffer
|
||||
) {
|
||||
node_ = &node;
|
||||
motion_buffer_ = motion_buffer;
|
||||
tf_ = TF::create(*node_);
|
||||
auto config = YAML::LoadFile(AUTO_SNIPER_CONFIG);
|
||||
auto map_config = config["map"];
|
||||
auto min_pos_v = map_config["min_pos"].as<std::vector<double>>();
|
||||
auto min_pos = Eigen::Vector3d(min_pos_v[0], min_pos_v[1], min_pos_v[2]);
|
||||
auto max_pos_v = map_config["max_pos"].as<std::vector<double>>();
|
||||
auto max_pos = Eigen::Vector3d(max_pos_v[0], max_pos_v[1], max_pos_v[2]);
|
||||
voxel_map_ = std::make_shared<SlidingVoxelMap<3, Cell>>(
|
||||
map_config["voxel_size"].as<double>(),
|
||||
min_pos,
|
||||
max_pos,
|
||||
true
|
||||
);
|
||||
auto solver_config = config["solver"];
|
||||
vis_cloud_ = std::make_shared<open3d::geometry::PointCloud>();
|
||||
k1_solver_ = K1BallisticSolver::create(
|
||||
solver_config["k1"].as<double>(),
|
||||
solver_config["g"].as<double>()
|
||||
);
|
||||
target_armor_z_ = solver_config["target_armor_z"].as<double>();
|
||||
offset_helper_ = OffsetHelper::create(config["offset_helper"]);
|
||||
pointcloud_sub_ = node_->create_subscription<sensor_msgs::msg::PointCloud2>(
|
||||
"/cloud_registered",
|
||||
rclcpp::SensorDataQoS(),
|
||||
std::bind(&AutoSniper::Impl::pointCloudCallback, this, std::placeholders::_1)
|
||||
);
|
||||
|
||||
odometry_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(
|
||||
"/Odometry",
|
||||
10,
|
||||
std::bind(&AutoSniper::Impl::odomCallback, this, std::placeholders::_1)
|
||||
);
|
||||
traj_pub_ =
|
||||
node_->create_publisher<visualization_msgs::msg::Marker>("bullet_trajectory", 10);
|
||||
}
|
||||
void start() {
|
||||
if (run_flag_) {
|
||||
return;
|
||||
}
|
||||
run_flag_ = true;
|
||||
vis_thread_ = wust_vl::common::concurrency::MonitoredThread::create(
|
||||
"AutoSniperVisualizer",
|
||||
[this](wust_vl::common::concurrency::MonitoredThread::Ptr self) {
|
||||
this->visualizeLoop(self);
|
||||
}
|
||||
);
|
||||
wust_vl::common::concurrency::ThreadManager::instance().registerThread(vis_thread_);
|
||||
}
|
||||
void doDebug() {}
|
||||
void pushInput(CommonFrame& frame) {}
|
||||
wust_vl::common::concurrency::MonitoredThread::Ptr getThread() {
|
||||
return vis_thread_;
|
||||
}
|
||||
GimbalCmd solve(double bullet_speed) noexcept {
|
||||
GimbalCmd cmd;
|
||||
if (!target_pos_in_map_.has_value()) {
|
||||
cmd.appear = false;
|
||||
return cmd;
|
||||
}
|
||||
Eigen::Vector3d target_pos_in_self = self_in_map_.inverse() * target_pos_in_map_.value();
|
||||
target_pos_in_self.z() = target_armor_z_;
|
||||
auto pitch = k1_solver_->solvePitch(target_pos_in_self, bullet_speed);
|
||||
if (!pitch.has_value()) {
|
||||
cmd.appear = false;
|
||||
std::cout << "no pitch" << std::endl;
|
||||
return cmd;
|
||||
}
|
||||
double yaw =
|
||||
angles::normalize_angle(std::atan2(target_pos_in_self.y(), target_pos_in_self.x()));
|
||||
auto traj = k1_solver_->computeTrajectory(
|
||||
self_in_map_.translation(),
|
||||
target_pos_in_map_.value(),
|
||||
bullet_speed,
|
||||
0.01
|
||||
);
|
||||
double yaw_deg = angles::to_degrees(yaw);
|
||||
double pitch_deg = angles::to_degrees(pitch.value());
|
||||
publishTrajectoryMarker(traj);
|
||||
|
||||
auto control_pitch = pitch_deg + offset_helper_->getPitchOffset(target_pos_in_self.norm());
|
||||
|
||||
auto control_yaw = yaw_deg + offset_helper_->getYawOffset(target_pos_in_self.norm());
|
||||
if (auto last_att = motion_buffer_->get_last()) {
|
||||
control_yaw += angles::to_degrees(last_att->data.yaw);
|
||||
}
|
||||
cmd.appear = true;
|
||||
cmd.target_pitch = control_pitch;
|
||||
cmd.target_yaw = control_yaw;
|
||||
cmd.appear = true;
|
||||
cmd.yaw = control_yaw;
|
||||
cmd.pitch = control_pitch;
|
||||
cmd.distance = target_pos_in_self.norm();
|
||||
cmd.enable_pitch_diff = 0.5;
|
||||
cmd.enable_yaw_diff = 0.5;
|
||||
static int count = 0;
|
||||
count++;
|
||||
if (count % 100 == 0) {
|
||||
std::cout << cmd.yaw << " " << cmd.pitch << std::endl;
|
||||
}
|
||||
return cmd;
|
||||
}
|
||||
void publishTrajectoryMarker(const std::vector<Eigen::Vector3d>& traj) {
|
||||
static auto last_pub_time = std::chrono::steady_clock::now();
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_pub_time);
|
||||
if (elapsed.count() < 33) {
|
||||
return;
|
||||
}
|
||||
last_pub_time = now;
|
||||
|
||||
if (traj.empty())
|
||||
return;
|
||||
visualization_msgs::msg::Marker marker;
|
||||
marker.header.frame_id = target_frame_;
|
||||
marker.header.stamp = node_->now();
|
||||
marker.ns = "bullet";
|
||||
marker.id = 0;
|
||||
marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
|
||||
marker.action = visualization_msgs::msg::Marker::ADD;
|
||||
marker.scale.x = 0.1;
|
||||
marker.color.r = 0.0;
|
||||
marker.color.g = 1.0;
|
||||
marker.color.b = 1.0;
|
||||
marker.color.a = 1.0;
|
||||
marker.lifetime = rclcpp::Duration::from_seconds(1.0);
|
||||
for (auto& p: traj) {
|
||||
geometry_msgs::msg::Point pt;
|
||||
pt.x = p.x();
|
||||
pt.y = p.y();
|
||||
pt.z = p.z();
|
||||
marker.points.push_back(pt);
|
||||
}
|
||||
|
||||
traj_pub_->publish(marker);
|
||||
}
|
||||
void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
|
||||
auto T = tf_->getTransform(target_frame_, "gimbal_yaw", msg->header.stamp);
|
||||
|
||||
if (!T.has_value()) {
|
||||
return;
|
||||
}
|
||||
self_in_map_ = T.value().cast<double>();
|
||||
// Eigen::Isometry3f
|
||||
// Eigen::Vector4f p(
|
||||
// msg->pose.pose.position.x,
|
||||
// msg->pose.pose.position.y,
|
||||
// msg->pose.pose.position.z,
|
||||
// 1.0f
|
||||
// );
|
||||
|
||||
// auto p_target = T.value() * p;
|
||||
// self_pos_ = Eigen::Vector3d(p_target.x(), p_target.y(), p_target.z());
|
||||
}
|
||||
|
||||
void visualizeLoop(wust_vl::common::concurrency::MonitoredThread::Ptr self) {
|
||||
while (!self->isAlive()) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
|
||||
auto vis_cloud = std::make_shared<open3d::geometry::PointCloud>();
|
||||
std::atomic<bool> picking = false;
|
||||
auto future = std::async(std::launch::async, [&, self]() {
|
||||
while (self->isAlive() && run_flag_) {
|
||||
picking = true;
|
||||
pickBlocking(vis_cloud);
|
||||
picking = false;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||||
}
|
||||
});
|
||||
|
||||
while (self->isAlive() && run_flag_) {
|
||||
self->heartbeat();
|
||||
if (!picking) {
|
||||
std::lock_guard<std::mutex> lock(cloud_mutex_);
|
||||
*vis_cloud = *vis_cloud_;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||||
}
|
||||
if (future.valid()) {
|
||||
future.wait();
|
||||
}
|
||||
}
|
||||
|
||||
void pickBlocking(std::shared_ptr<open3d::geometry::PointCloud> pcd) {
|
||||
open3d::visualization::VisualizerWithEditing vis;
|
||||
vis.CreateVisualizerWindow("Pick points (close window when done)", 1280, 720);
|
||||
vis.AddGeometry(pcd);
|
||||
vis.Run();
|
||||
|
||||
auto picked = vis.GetPickedPoints();
|
||||
if (!picked.empty()) {
|
||||
Eigen::Vector3d picked_pos = pcd->points_[picked.back()];
|
||||
target_pos_in_map_ = Eigen::Vector3d(picked_pos);
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
rclcpp::get_logger("awm"),
|
||||
" Target pos: " << picked_pos.transpose()
|
||||
<< " self pos: " << self_in_map_.translation().transpose()
|
||||
);
|
||||
}
|
||||
|
||||
vis.DestroyVisualizerWindow();
|
||||
}
|
||||
|
||||
void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
|
||||
auto T = tf_->getTransform(target_frame_, msg->header.frame_id, msg->header.stamp);
|
||||
|
||||
if (!T.has_value()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const size_t size = msg->width * msg->height;
|
||||
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
|
||||
|
||||
std::vector<Eigen::Vector3d> new_points;
|
||||
new_points.reserve(size);
|
||||
|
||||
for (size_t i = 0; i < size; ++i) {
|
||||
Eigen::Vector4f p(*iter_x, *iter_y, *iter_z, 1.0f);
|
||||
|
||||
p = T.value() * p;
|
||||
|
||||
if (std::isfinite(p.x()) && std::isfinite(p.y()) && std::isfinite(p.z())) {
|
||||
new_points.emplace_back(p.head<3>().cast<double>());
|
||||
}
|
||||
|
||||
++iter_x;
|
||||
++iter_y;
|
||||
++iter_z;
|
||||
}
|
||||
for (const auto& p: new_points) {
|
||||
auto idx = voxel_map_->worldToIndex(p);
|
||||
if (idx > 0) {
|
||||
voxel_map_->grid[idx].v = 1;
|
||||
}
|
||||
}
|
||||
std::vector<Eigen::Vector3d> pointcloud;
|
||||
for (int i = 0; i < voxel_map_->grid.size(); i++) {
|
||||
if (voxel_map_->grid[i].v == 1) {
|
||||
pointcloud.emplace_back(voxel_map_->indexToWorld(i));
|
||||
}
|
||||
}
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(cloud_mutex_);
|
||||
|
||||
vis_cloud_->points_ = pointcloud;
|
||||
}
|
||||
}
|
||||
|
||||
std::string target_frame_ = "map";
|
||||
|
||||
rclcpp::Node* node_;
|
||||
std::optional<Eigen::Vector3d> target_pos_in_map_ = std::nullopt;
|
||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
|
||||
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr traj_pub_;
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;
|
||||
TF::Ptr tf_;
|
||||
std::shared_ptr<open3d::geometry::PointCloud> vis_cloud_;
|
||||
struct Cell {
|
||||
uint8_t v = 0;
|
||||
};
|
||||
SlidingVoxelMap<3, Cell>::Ptr voxel_map_;
|
||||
K1BallisticSolver::Ptr k1_solver_;
|
||||
OffsetHelper::Ptr offset_helper_;
|
||||
wust_vl::common::concurrency::MonitoredThread::Ptr vis_thread_;
|
||||
bool run_flag_ = false;
|
||||
std::mutex cloud_mutex_;
|
||||
double target_armor_z_ = 0.0;
|
||||
Eigen::Isometry3d self_in_map_ = Eigen::Isometry3d::Identity();
|
||||
std::shared_ptr<wust_vl::common::utils::MotionBufferGeneric<Motion, 1024>> motion_buffer_;
|
||||
};
|
||||
AutoSniper::AutoSniper(
|
||||
rclcpp::Node& node,
|
||||
std::shared_ptr<wust_vl::common::utils::MotionBufferGeneric<Motion, 1024>> motion_buffer
|
||||
) {
|
||||
_impl = std::make_unique<Impl>(node, motion_buffer);
|
||||
}
|
||||
AutoSniper::~AutoSniper() {
|
||||
_impl.reset();
|
||||
}
|
||||
void AutoSniper::start() {
|
||||
_impl->start();
|
||||
}
|
||||
void AutoSniper::doDebug() {
|
||||
_impl->doDebug();
|
||||
}
|
||||
void AutoSniper::pushInput(CommonFrame& frame) {
|
||||
_impl->pushInput(frame);
|
||||
}
|
||||
|
||||
wust_vl::common::concurrency::MonitoredThread::Ptr AutoSniper::getThread() {
|
||||
return _impl->getThread();
|
||||
}
|
||||
GimbalCmd AutoSniper::solve(double bullet_speed) {
|
||||
return _impl->solve(bullet_speed);
|
||||
}
|
||||
} // namespace wust_vision::auto_sniper
|
||||
#endif
|
||||
32
wust_vision-main/tasks/auto_sniper/auto_sniper.hpp
Normal file
32
wust_vision-main/tasks/auto_sniper/auto_sniper.hpp
Normal file
@@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
#include "tasks/imodule.hpp"
|
||||
#include "tasks/type_common.hpp"
|
||||
#include <memory>
|
||||
#include <rclcpp/node.hpp>
|
||||
namespace wust_vision {
|
||||
namespace auto_sniper {
|
||||
class AutoSniper: public IModule {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<AutoSniper>;
|
||||
AutoSniper(
|
||||
rclcpp::Node& node,
|
||||
std::shared_ptr<wust_vl::common::utils::MotionBufferGeneric<Motion, 1024>> motion_buffer
|
||||
);
|
||||
static Ptr create(
|
||||
rclcpp::Node& node,
|
||||
std::shared_ptr<wust_vl::common::utils::MotionBufferGeneric<Motion, 1024>> motion_buffer
|
||||
) {
|
||||
return std::make_shared<AutoSniper>(node, motion_buffer);
|
||||
}
|
||||
~AutoSniper();
|
||||
void start() override;
|
||||
void doDebug() override;
|
||||
void pushInput(CommonFrame& frame) override;
|
||||
GimbalCmd solve(double bullet_speed) override;
|
||||
wust_vl::common::concurrency::MonitoredThread::Ptr getThread() override;
|
||||
struct Impl;
|
||||
std::unique_ptr<Impl> _impl;
|
||||
};
|
||||
} // namespace auto_sniper
|
||||
|
||||
} // namespace wust_vision
|
||||
106
wust_vision-main/tasks/auto_sniper/k1_solver.hpp
Normal file
106
wust_vision-main/tasks/auto_sniper/k1_solver.hpp
Normal file
@@ -0,0 +1,106 @@
|
||||
#include <Eigen/Dense>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
|
||||
class K1BallisticSolver {
|
||||
public:
|
||||
using Ptr = std::unique_ptr<K1BallisticSolver>;
|
||||
K1BallisticSolver(double k1 = 0.05, double g = 9.81): k1_(k1), g_(g) {}
|
||||
static Ptr create(double k1 = 0.05, double g = 9.81) {
|
||||
return std::make_unique<K1BallisticSolver>(k1, g);
|
||||
}
|
||||
std::optional<double> solvePitch(const Eigen::Vector3d& target_pos, double v0) const {
|
||||
double x = std::hypot(target_pos.x(), target_pos.y());
|
||||
double z = target_pos.z();
|
||||
|
||||
if (x < 1e-6)
|
||||
return std::nullopt;
|
||||
|
||||
auto heightError = [&](double pitch) {
|
||||
double cos_theta = std::cos(pitch);
|
||||
double sin_theta = std::sin(pitch);
|
||||
|
||||
double denom = v0 * cos_theta;
|
||||
if (denom <= 1e-6)
|
||||
return 1e6;
|
||||
|
||||
double t = -std::log(1.0 - k1_ * x / denom) / k1_;
|
||||
|
||||
if (!std::isfinite(t))
|
||||
return 1e6;
|
||||
|
||||
double z_pred =
|
||||
((v0 * sin_theta + g_ / k1_) / k1_) * (1.0 - std::exp(-k1_ * t)) - (g_ / k1_) * t;
|
||||
|
||||
return z_pred - z;
|
||||
};
|
||||
|
||||
double left = -0.3;
|
||||
double right = 0.6;
|
||||
|
||||
for (int i = 0; i < 60; ++i) {
|
||||
double mid = 0.5 * (left + right);
|
||||
double err = heightError(mid);
|
||||
|
||||
if (err > 0)
|
||||
right = mid;
|
||||
else
|
||||
left = mid;
|
||||
}
|
||||
|
||||
return 0.5 * (left + right);
|
||||
}
|
||||
std::vector<Eigen::Vector3d> computeTrajectory(
|
||||
const Eigen::Vector3d& start,
|
||||
const Eigen::Vector3d& target,
|
||||
double v0,
|
||||
double dt = 0.01 // 每个离散点的时间间隔
|
||||
) {
|
||||
std::vector<Eigen::Vector3d> traj;
|
||||
|
||||
Eigen::Vector3d diff = target - start;
|
||||
auto pitch_opt = solvePitch(diff, v0);
|
||||
if (!pitch_opt.has_value())
|
||||
return traj;
|
||||
|
||||
double pitch = pitch_opt.value();
|
||||
double yaw = std::atan2(diff.y(), diff.x());
|
||||
|
||||
double k1 = k1_;
|
||||
double g = g_;
|
||||
|
||||
double vx = v0 * std::cos(pitch) * std::cos(yaw);
|
||||
double vy = v0 * std::cos(pitch) * std::sin(yaw);
|
||||
double vz = v0 * std::sin(pitch);
|
||||
|
||||
double t = 0.0;
|
||||
Eigen::Vector3d pos = start;
|
||||
|
||||
while (true) {
|
||||
double exp_kt = std::exp(-k1 * t);
|
||||
pos.x() = start.x() + (vx / k1) * (1 - exp_kt);
|
||||
pos.y() = start.y() + (vy / k1) * (1 - exp_kt);
|
||||
pos.z() = start.z() + ((vz + g / k1) / k1) * (1 - exp_kt) - (g / k1) * t;
|
||||
|
||||
traj.push_back(pos);
|
||||
|
||||
t += dt;
|
||||
|
||||
double dx = std::abs(pos.x() - start.x());
|
||||
double dy = std::abs(pos.y() - start.y());
|
||||
double horizontal_dist = std::sqrt(dx * dx + dy * dy);
|
||||
double target_dist = std::sqrt(diff.x() * diff.x() + diff.y() * diff.y());
|
||||
|
||||
if (horizontal_dist >= target_dist) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return traj;
|
||||
}
|
||||
|
||||
private:
|
||||
double k1_;
|
||||
double g_;
|
||||
};
|
||||
87
wust_vision-main/tasks/auto_sniper/offset_helper.hpp
Normal file
87
wust_vision-main/tasks/auto_sniper/offset_helper.hpp
Normal file
@@ -0,0 +1,87 @@
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include <vector>
|
||||
#include <yaml-cpp/node/node.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
namespace wust_vision::auto_sniper {
|
||||
|
||||
class OffsetHelper {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<OffsetHelper>;
|
||||
struct OffsetPoint {
|
||||
double distance;
|
||||
double yaw;
|
||||
double pitch;
|
||||
};
|
||||
OffsetHelper(const YAML::Node& config) {
|
||||
data_.clear();
|
||||
for (auto& v: config["offset_table"]) {
|
||||
OffsetPoint p;
|
||||
p.distance = v["distance"].as<double>();
|
||||
p.yaw = v["yaw"].as<double>();
|
||||
p.pitch = v["pitch"].as<double>();
|
||||
|
||||
data_.push_back(p);
|
||||
}
|
||||
order_ = config["order"].as<int>();
|
||||
yaw_base_offset = config["yaw_base_offset"].as<double>();
|
||||
pitch_base_offset = config["pitch_base_offset"].as<double>();
|
||||
fit();
|
||||
}
|
||||
static Ptr create(const YAML::Node& config) {
|
||||
return std::make_shared<OffsetHelper>(config);
|
||||
}
|
||||
void fit() {
|
||||
int n = data_.size();
|
||||
Eigen::MatrixXd A(n, order_ + 1);
|
||||
|
||||
Eigen::VectorXd y_yaw(n);
|
||||
Eigen::VectorXd y_pitch(n);
|
||||
|
||||
for (int i = 0; i < n; ++i) {
|
||||
double x = data_[i].distance;
|
||||
|
||||
double v = 1;
|
||||
for (int j = 0; j <= order_; ++j) {
|
||||
A(i, j) = v;
|
||||
v *= x;
|
||||
}
|
||||
|
||||
y_yaw(i) = data_[i].yaw;
|
||||
y_pitch(i) = data_[i].pitch;
|
||||
}
|
||||
|
||||
yaw_coeff_ = A.colPivHouseholderQr().solve(y_yaw);
|
||||
pitch_coeff_ = A.colPivHouseholderQr().solve(y_pitch);
|
||||
}
|
||||
|
||||
double getYawOffset(double distance) const {
|
||||
return yaw_base_offset + eval(yaw_coeff_, distance);
|
||||
}
|
||||
double getPitchOffset(double distance) const {
|
||||
return pitch_base_offset + eval(pitch_coeff_, distance);
|
||||
}
|
||||
|
||||
double eval(const Eigen::VectorXd& coeff, double x) const {
|
||||
double y = 0;
|
||||
double v = 1;
|
||||
|
||||
for (int i = 0; i < coeff.size(); ++i) {
|
||||
y += coeff[i] * v;
|
||||
v *= x;
|
||||
}
|
||||
|
||||
return y;
|
||||
}
|
||||
std::vector<OffsetPoint> data_;
|
||||
|
||||
Eigen::VectorXd yaw_coeff_;
|
||||
Eigen::VectorXd pitch_coeff_;
|
||||
double yaw_base_offset = 0;
|
||||
double pitch_base_offset = 0;
|
||||
int order_ = 2;
|
||||
};
|
||||
|
||||
} // namespace wust_vision::auto_sniper
|
||||
348
wust_vision-main/tasks/auto_sniper/voxel_map.hpp
Normal file
348
wust_vision-main/tasks/auto_sniper/voxel_map.hpp
Normal file
@@ -0,0 +1,348 @@
|
||||
#pragma once
|
||||
|
||||
#include "3rdparty/ankerl/unordered_dense.h"
|
||||
#include <Eigen/Dense>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
namespace wust_vision::auto_sniper {
|
||||
|
||||
template<int Dim>
|
||||
struct VoxelKey {
|
||||
std::array<int, Dim> data {};
|
||||
|
||||
int& operator[](int i) noexcept {
|
||||
return data[i];
|
||||
}
|
||||
const int& operator[](int i) const noexcept {
|
||||
return data[i];
|
||||
}
|
||||
|
||||
// ----- x -----
|
||||
int& x() noexcept {
|
||||
static_assert(Dim >= 1, "x requires Dim >= 1");
|
||||
return data[0];
|
||||
}
|
||||
const int& x() const noexcept {
|
||||
static_assert(Dim >= 1, "x requires Dim >= 1");
|
||||
return data[0];
|
||||
}
|
||||
|
||||
// ----- y -----
|
||||
int& y() noexcept {
|
||||
static_assert(Dim >= 2, "y requires Dim >= 2");
|
||||
return data[1];
|
||||
}
|
||||
const int& y() const noexcept {
|
||||
static_assert(Dim >= 2, "y requires Dim >= 2");
|
||||
return data[1];
|
||||
}
|
||||
|
||||
// ----- z -----
|
||||
int& z() noexcept {
|
||||
static_assert(Dim >= 3, "z requires Dim >= 3");
|
||||
return data[2];
|
||||
}
|
||||
const int& z() const noexcept {
|
||||
static_assert(Dim >= 3, "z requires Dim >= 3");
|
||||
return data[2];
|
||||
}
|
||||
bool operator==(const VoxelKey& other) const noexcept {
|
||||
for (int i = 0; i < Dim; ++i)
|
||||
if (data[i] != other.data[i])
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool operator!=(const VoxelKey& other) const noexcept {
|
||||
return !(*this == other);
|
||||
}
|
||||
|
||||
VoxelKey& operator+=(const VoxelKey& other) noexcept {
|
||||
for (int i = 0; i < Dim; ++i)
|
||||
data[i] += other.data[i];
|
||||
return *this;
|
||||
}
|
||||
|
||||
VoxelKey& operator-=(const VoxelKey& other) noexcept {
|
||||
for (int i = 0; i < Dim; ++i)
|
||||
data[i] -= other.data[i];
|
||||
return *this;
|
||||
}
|
||||
|
||||
VoxelKey& operator*=(int scalar) noexcept {
|
||||
for (int i = 0; i < Dim; ++i)
|
||||
data[i] *= scalar;
|
||||
return *this;
|
||||
}
|
||||
|
||||
VoxelKey& operator/=(int scalar) noexcept {
|
||||
for (int i = 0; i < Dim; ++i)
|
||||
data[i] /= scalar;
|
||||
return *this;
|
||||
}
|
||||
|
||||
friend VoxelKey operator+(VoxelKey lhs, const VoxelKey& rhs) noexcept {
|
||||
lhs += rhs;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
friend VoxelKey operator-(VoxelKey lhs, const VoxelKey& rhs) noexcept {
|
||||
lhs -= rhs;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
friend VoxelKey operator*(VoxelKey lhs, int scalar) noexcept {
|
||||
lhs *= scalar;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
friend VoxelKey operator*(int scalar, VoxelKey rhs) noexcept {
|
||||
rhs *= scalar;
|
||||
return rhs;
|
||||
}
|
||||
|
||||
friend VoxelKey operator/(VoxelKey lhs, int scalar) noexcept {
|
||||
lhs /= scalar;
|
||||
return lhs;
|
||||
}
|
||||
constexpr VoxelKey cwiseMin(const VoxelKey& other) const noexcept {
|
||||
VoxelKey out {};
|
||||
for (int i = 0; i < Dim; ++i)
|
||||
out.data[i] = data[i] < other.data[i] ? data[i] : other.data[i];
|
||||
return out;
|
||||
}
|
||||
|
||||
constexpr VoxelKey cwiseMax(const VoxelKey& other) const noexcept {
|
||||
VoxelKey out {};
|
||||
for (int i = 0; i < Dim; ++i)
|
||||
out.data[i] = data[i] > other.data[i] ? data[i] : other.data[i];
|
||||
return out;
|
||||
}
|
||||
};
|
||||
|
||||
template<int Dim, typename Cell>
|
||||
class SlidingVoxelMap {
|
||||
static_assert(Dim == 2 || Dim == 3, "Dim must be 2 or 3");
|
||||
|
||||
public:
|
||||
using Ptr = std::shared_ptr<SlidingVoxelMap>;
|
||||
using Key = VoxelKey<Dim>;
|
||||
using EigenPoint = Eigen::Matrix<double, Dim, 1>;
|
||||
|
||||
SlidingVoxelMap(double voxel_size_, const EigenPoint& size_, const EigenPoint& center_):
|
||||
voxel_size(voxel_size_),
|
||||
size(size_),
|
||||
center(center_) {
|
||||
EigenPoint half = size * 0.5f;
|
||||
|
||||
min_key = worldToKey(center - half);
|
||||
max_key = worldToKey(center + half);
|
||||
|
||||
for (int i = 0; i < Dim; ++i) {
|
||||
dims[i] = max_key[i] - min_key[i] + 1;
|
||||
offset[i] = 0;
|
||||
}
|
||||
|
||||
center_key = worldToKey(center);
|
||||
|
||||
size_t N = 1;
|
||||
for (int i = 0; i < Dim; ++i)
|
||||
N *= static_cast<size_t>(dims[i]);
|
||||
|
||||
grid.resize(N);
|
||||
}
|
||||
SlidingVoxelMap(
|
||||
double voxel_size_,
|
||||
const EigenPoint& min_pos,
|
||||
const EigenPoint& max_pos,
|
||||
bool /*dummy*/
|
||||
):
|
||||
voxel_size(voxel_size_) {
|
||||
min_key = worldToKey(min_pos);
|
||||
max_key = worldToKey(max_pos);
|
||||
|
||||
for (int i = 0; i < Dim; ++i) {
|
||||
if (max_key[i] < min_key[i])
|
||||
std::swap(max_key[i], min_key[i]);
|
||||
|
||||
dims[i] = max_key[i] - min_key[i] + 1;
|
||||
offset[i] = 0;
|
||||
}
|
||||
|
||||
for (int i = 0; i < Dim; ++i)
|
||||
center_key[i] = (min_key[i] + max_key[i]) / 2;
|
||||
|
||||
EigenPoint min_world = keyToWorld(min_key);
|
||||
EigenPoint max_world = keyToWorld(max_key);
|
||||
|
||||
for (int i = 0; i < Dim; ++i) {
|
||||
center[i] = (min_world[i] + max_world[i]) * 0.5f;
|
||||
size[i] = dims[i] * voxel_size;
|
||||
}
|
||||
|
||||
size_t N = 1;
|
||||
for (int i = 0; i < Dim; ++i)
|
||||
N *= static_cast<size_t>(dims[i]);
|
||||
|
||||
grid.resize(N);
|
||||
}
|
||||
|
||||
static Ptr create(double voxel_size, const EigenPoint& size, const EigenPoint& center) {
|
||||
return std::make_shared<SlidingVoxelMap>(voxel_size, size, center);
|
||||
}
|
||||
|
||||
size_t gridSize() const noexcept {
|
||||
return grid.size();
|
||||
}
|
||||
inline int worldToIndex(const EigenPoint& p) const noexcept {
|
||||
return keyToIndex(worldToKey(p));
|
||||
}
|
||||
|
||||
inline Key worldToKey(const EigenPoint& p) const noexcept {
|
||||
Key k;
|
||||
const double inv = 1.0f / voxel_size;
|
||||
for (int i = 0; i < Dim; ++i)
|
||||
k.data[i] = static_cast<int>(std::floor(p[i] * inv + 1e-6f));
|
||||
return k;
|
||||
}
|
||||
|
||||
inline EigenPoint keyToWorld(const Key& k) const noexcept {
|
||||
EigenPoint p;
|
||||
for (int i = 0; i < Dim; ++i)
|
||||
p[i] = (k.data[i] + 0.5f) * voxel_size;
|
||||
return p;
|
||||
}
|
||||
|
||||
inline EigenPoint indexToWorld(int idx) const noexcept {
|
||||
return keyToWorld(indexToKey(idx));
|
||||
}
|
||||
|
||||
inline int keyToIndex(const Key& k) const noexcept {
|
||||
int idx = 0;
|
||||
int stride = 1;
|
||||
|
||||
for (int d = Dim - 1; d >= 0; --d) {
|
||||
int delta = k[d] - center_key[d] + (dims[d] >> 1);
|
||||
|
||||
if (delta < 0 || delta >= dims[d])
|
||||
return -1;
|
||||
|
||||
int r = delta + offset[d];
|
||||
|
||||
if (r >= dims[d])
|
||||
r -= dims[d];
|
||||
else if (r < 0)
|
||||
r += dims[d];
|
||||
|
||||
idx += r * stride;
|
||||
stride *= dims[d];
|
||||
}
|
||||
|
||||
return idx;
|
||||
}
|
||||
|
||||
inline Key indexToKey(int idx) const noexcept {
|
||||
Key k;
|
||||
|
||||
for (int d = Dim - 1; d >= 0; --d) {
|
||||
int r = idx % dims[d];
|
||||
idx /= dims[d];
|
||||
|
||||
int delta = r - offset[d];
|
||||
|
||||
if (delta < 0)
|
||||
delta += dims[d];
|
||||
else if (delta >= dims[d])
|
||||
delta -= dims[d];
|
||||
|
||||
k[d] = center_key[d] + delta - (dims[d] >> 1);
|
||||
}
|
||||
|
||||
return k;
|
||||
}
|
||||
template<typename ClearFunc>
|
||||
void slideTo(const Key& new_center_key, ClearFunc clear_func) {
|
||||
Key shift;
|
||||
for (int d = 0; d < Dim; ++d)
|
||||
shift[d] = new_center_key[d] - center_key[d];
|
||||
|
||||
for (int d = 0; d < Dim; ++d) {
|
||||
if (std::abs(shift[d]) >= dims[d]) {
|
||||
for (size_t i = 0; i < grid.size(); ++i)
|
||||
clear_func(i);
|
||||
|
||||
offset = {};
|
||||
center_key = new_center_key;
|
||||
return;
|
||||
}
|
||||
}
|
||||
for (int axis = 0; axis < Dim; ++axis) {
|
||||
int s = shift[axis];
|
||||
if (s == 0)
|
||||
continue;
|
||||
|
||||
int steps = std::abs(s);
|
||||
int dir = s > 0 ? 1 : -1;
|
||||
|
||||
for (int step = 0; step < steps; ++step) {
|
||||
int slice = (offset[axis] + dir * step + dims[axis]) % dims[axis];
|
||||
|
||||
clearSlice(axis, slice, clear_func);
|
||||
}
|
||||
|
||||
offset[axis] = (offset[axis] + s + dims[axis]) % dims[axis];
|
||||
}
|
||||
|
||||
center_key = new_center_key;
|
||||
EigenPoint half = size * 0.5f;
|
||||
center = keyToWorld(center_key);
|
||||
min_key = worldToKey(center - half);
|
||||
max_key = worldToKey(center + half);
|
||||
}
|
||||
|
||||
template<typename ClearFunc>
|
||||
void clearSlice(int axis, int slice, ClearFunc clear_func) {
|
||||
if constexpr (Dim == 3) {
|
||||
int dx = dims[0];
|
||||
int dy = dims[1];
|
||||
int dz = dims[2];
|
||||
|
||||
if (axis == 0) {
|
||||
for (int y = 0; y < dy; ++y)
|
||||
for (int z = 0; z < dz; ++z) {
|
||||
int idx = (slice * dy + y) * dz + z;
|
||||
clear_func(idx);
|
||||
}
|
||||
} else if (axis == 1) {
|
||||
for (int x = 0; x < dx; ++x)
|
||||
for (int z = 0; z < dz; ++z) {
|
||||
int idx = (x * dy + slice) * dz + z;
|
||||
clear_func(idx);
|
||||
}
|
||||
} else {
|
||||
for (int x = 0; x < dx; ++x)
|
||||
for (int y = 0; y < dy; ++y) {
|
||||
int idx = (x * dy + y) * dz + slice;
|
||||
clear_func(idx);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
double voxel_size;
|
||||
|
||||
Key dims;
|
||||
Key offset;
|
||||
|
||||
std::vector<Cell> grid;
|
||||
|
||||
Key center_key;
|
||||
EigenPoint center;
|
||||
EigenPoint size;
|
||||
Key min_key;
|
||||
Key max_key;
|
||||
};
|
||||
|
||||
} // namespace wust_vision::auto_sniper
|
||||
Reference in New Issue
Block a user