From 272c38e684e594493bf3e42af243713855921176 Mon Sep 17 00:00:00 2001 From: varnashova88 Date: Sun, 19 Apr 2026 17:07:07 +0000 Subject: [PATCH] =?UTF-8?q?=D0=9E=D0=B1=D0=BD=D0=BE=D0=B2=D0=B8=D1=82?= =?UTF-8?q?=D1=8C=20processing/calculations.cpp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- processing/calculations.cpp | 360 ++++++++++++++++++++++++++++++++++++ 1 file changed, 360 insertions(+) diff --git a/processing/calculations.cpp b/processing/calculations.cpp index e69de29..c4e700a 100644 --- a/processing/calculations.cpp +++ b/processing/calculations.cpp @@ -0,0 +1,360 @@ +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace Eigen; + +// СТРУКТУРЫ + +struct IMUData { + double t; + Vector3d acc; // g + Vector3d gyro; // deg/s +}; + +struct LidarData { + double t; + double angle; + double distance; // mm +}; + +struct State { + VectorXd x; + MatrixXd P; +}; + +// ЧТЕНИЕ CSV + +vector readIMU(const string& filename) { + vector data; + ifstream file(filename); + string line; + + getline(file, line); + + while (getline(file, line)) { + stringstream ss(line); + string val; + + IMUData d; + + getline(ss, val, ','); d.t = stod(val); + getline(ss, val, ','); d.acc.x() = stod(val); + getline(ss, val, ','); d.acc.y() = stod(val); + getline(ss, val, ','); d.acc.z() = stod(val); + getline(ss, val, ','); d.gyro.x() = stod(val); + getline(ss, val, ','); d.gyro.y() = stod(val); + getline(ss, val, ','); d.gyro.z() = stod(val); + + data.push_back(d); + } + + return data; +} + +vector readLidar(const string& filename) { + vector data; + ifstream file(filename); + string line; + + getline(file, line); + + while (getline(file, line)) { + stringstream ss(line); + string val; + + LidarData d; + + getline(ss, val, ','); d.t = stod(val); + getline(ss, val, ','); d.angle = stod(val); + getline(ss, val, ','); d.distance = stod(val); + + data.push_back(d); + } + + return data; +} + +// ПОВОРОТ + +Matrix3d rotationMatrix(double roll, double pitch, double yaw) { + AngleAxisd rx(roll, Vector3d::UnitX()); + AngleAxisd ry(pitch, Vector3d::UnitY()); + AngleAxisd rz(yaw, Vector3d::UnitZ()); + return (rz * ry * rx).toRotationMatrix(); +} + +// IMU PREDICT + +void predict(State& s, const IMUData& imu, double dt) { + VectorXd& x = s.x; + + Vector3d pos = x.segment<3>(0); + Vector3d vel = x.segment<3>(3); + Vector3d angles = x.segment<3>(6); + + Vector3d acc = imu.acc * 9.81; + Vector3d gyro = imu.gyro * M_PI / 180.0; + + Matrix3d R = rotationMatrix(angles[0], angles[1], angles[2]); + + Vector3d g(0, 0, 9.81); + Vector3d acc_world = R * acc - g; + + vel += acc_world * dt; + pos += vel * dt + 0.5 * acc_world * dt * dt; + angles += gyro * dt; + + x.segment<3>(0) = pos; + x.segment<3>(3) = vel; + x.segment<3>(6) = angles; + + MatrixXd F = MatrixXd::Identity(15, 15); + F.block<3, 3>(0, 3) = Matrix3d::Identity() * dt; + + MatrixXd Q = MatrixXd::Identity(15, 15) * 0.01; + + s.P = F * s.P * F.transpose() + Q; +} + +// ЛИДАР → МИР + +Vector3d lidarToWorld(const State& s, const LidarData& l) { + double angle = l.angle * M_PI / 180.0; + double dist = l.distance / 1000.0; + + Vector3d local; + local << dist * cos(angle), dist* sin(angle), 0; + + Vector3d pos = s.x.segment<3>(0); + Vector3d ang = s.x.segment<3>(6); + + Matrix3d R = rotationMatrix(ang[0], ang[1], ang[2]); + + return pos + R * local; +} + +// KD-TREE + +struct KDNode { + Vector3d point; + KDNode* left; + KDNode* right; + int axis; + + KDNode(Vector3d p, int ax) : point(p), left(nullptr), right(nullptr), axis(ax) {} +}; + +KDNode* buildKDTree(vector& pts, int depth = 0) { + if (pts.empty()) return nullptr; + + int axis = depth % 3; + + sort(pts.begin(), pts.end(), + [axis](const Vector3d& a, const Vector3d& b) { + return a[axis] < b[axis]; + }); + + int mid = pts.size() / 2; + + KDNode* node = new KDNode(pts[mid], axis); + + vector left(pts.begin(), pts.begin() + mid); + vector right(pts.begin() + mid + 1, pts.end()); + + node->left = buildKDTree(left, depth + 1); + node->right = buildKDTree(right, depth + 1); + + return node; +} + +void nearestSearch(KDNode* node, const Vector3d& target, + Vector3d& best, double& best_dist) { + if (!node) return; + + double d = (node->point - target).squaredNorm(); + if (d < best_dist) { + best_dist = d; + best = node->point; + } + + int axis = node->axis; + + KDNode* near = target[axis] < node->point[axis] ? node->left : node->right; + KDNode* far = target[axis] < node->point[axis] ? node->right : node->left; + + nearestSearch(near, target, best, best_dist); + + double diff = target[axis] - node->point[axis]; + if (diff * diff < best_dist) { + nearestSearch(far, target, best, best_dist); + } +} + +// ICP + +Matrix3d computeRotation(const vector& src, + const vector& dst) { + + Vector3d c1 = Vector3d::Zero(); + Vector3d c2 = Vector3d::Zero(); + + for (size_t i = 0;i < src.size();i++) { + c1 += src[i]; + c2 += dst[i]; + } + + c1 /= src.size(); + c2 /= dst.size(); + + Matrix3d H = Matrix3d::Zero(); + + for (size_t i = 0;i < src.size();i++) { + H += (src[i] - c1) * (dst[i] - c2).transpose(); + } + + JacobiSVD svd(H, ComputeFullU | ComputeFullV); + return svd.matrixV() * svd.matrixU().transpose(); +} + +Vector3d computeTranslation(const vector& src, + const vector& dst, + const Matrix3d& R) { + + Vector3d c1 = Vector3d::Zero(); + Vector3d c2 = Vector3d::Zero(); + + for (size_t i = 0;i < src.size();i++) { + c1 += src[i]; + c2 += dst[i]; + } + + c1 /= src.size(); + c2 /= dst.size(); + + return c2 - R * c1; +} + +vector downsample(const vector& pts, int step = 5) { + vector out; + for (size_t i = 0;i < pts.size();i += step) + out.push_back(pts[i]); + return out; +} + +void ICP_fast(vector& src, + vector& dst, + Matrix3d& R, Vector3d& t) { + + R = Matrix3d::Identity(); + t = Vector3d::Zero(); + + vector dst_copy = dst; + KDNode* tree = buildKDTree(dst_copy); + + for (int iter = 0; iter < 10; iter++) { + + vector matched; + + for (auto& p : src) { + Vector3d best; + double best_dist = 1e9; + + nearestSearch(tree, p, best, best_dist); + + if (best_dist < 0.5) + matched.push_back(best); + } + + if (matched.size() < 10) break; + + Matrix3d dR = computeRotation(src, matched); + Vector3d dt = computeTranslation(src, matched, dR); + + for (auto& p : src) + p = dR * p + dt; + + R = dR * R; + t = dR * t + dt; + } +} + +// MAIN + +int main() { + auto imu = readIMU("../imu_module/imu.csv"); + auto lidar = readLidar("../lidar_module/lidar.csv"); + + State state; + state.x = VectorXd::Zero(15); + state.P = MatrixXd::Identity(15, 15) * 0.1; + + ofstream traj("trajectory.csv"); + traj << "timestamp,x,y,z,roll,pitch,yaw\n"; + + ofstream lidar_out("lidar_points.csv"); + lidar_out << "x,y,z\n"; + + vector prev_scan, curr_scan; + + size_t lidar_idx = 0; + + for (size_t i = 1;i < imu.size();i++) { + + double dt = imu[i].t - imu[i - 1].t; + if (dt <= 0 || dt > 0.1) continue; + + predict(state, imu[i], dt); + + while (lidar_idx < lidar.size() && + lidar[lidar_idx].t <= imu[i].t) { + + curr_scan.push_back(lidarToWorld(state, lidar[lidar_idx])); + lidar_idx++; + } + + if (curr_scan.size() > 200) { + + if (!prev_scan.empty()) { + + vector src = downsample(curr_scan, 5); + vector dst = downsample(prev_scan, 5); + + Matrix3d R; + Vector3d t; + + ICP_fast(src, dst, R, t); + + state.x.segment<3>(0) += t; + } + + prev_scan = curr_scan; + curr_scan.clear(); + } + + Vector3d pos = state.x.segment<3>(0); + Vector3d ang = state.x.segment<3>(6); + + traj << imu[i].t << "," + << pos.x() << "," + << pos.y() << "," + << pos.z() << "," + << ang.x() << "," + << ang.y() << "," + << ang.z() << "\n"; + } + + for (auto& p : prev_scan) { + lidar_out << p.x() << "," + << p.y() << "," + << p.z() << "\n"; + } + + cout << "Done\n"; + return 0; +} \ No newline at end of file