imu_module/imu_reader.py

@@ -1,92 +1,68 @@
-import smbus2
+import smbus2 #библиотека Python для работы с I2C шиной
 import time
-import sqlite3
-import signal
-import sys
+import csv
+import struct
 
 # Константы
 MPU_ADDRESS = 0x68
-I2C_BUS = 2
-DB_PATH = '../inertial_data.db'
+I2C_BUS = 1
 
 # Регистры MPU-9250
-PWR_MGMT_1 = 0x6B
-ACCEL_XOUT_H = 0x3B
-GYRO_XOUT_H = 0x43
+PWR_MGMT_1 = 0x6B      # управление питанием
+ACCEL_XOUT_H = 0x3B    # начало данных акселерометра
+GYRO_XOUT_H = 0x43     # начало данных гироскопа
 
-# Масштабные коэффициенты
-ACCEL_SCALE = 16384.0
-GYRO_SCALE = 131.0
-
-# Глобальные переменные для завершения
-running = True
-conn = None
-bus = None
-
-def shutdown(signum, frame):
-    """Обработчик сигнала завершения"""
-    global running
-    print("Завершение IMU модуля...")
-    running = False
-
-# Перехватываем сигналы завершения
-signal.signal(signal.SIGTERM, shutdown)
-signal.signal(signal.SIGINT, shutdown)
-
-def init_db():
-    conn = sqlite3.connect(DB_PATH)
-    conn.execute('''CREATE TABLE IF NOT EXISTS imu_data (
-        timestamp REAL,
-        ax REAL, ay REAL, az REAL,
-        gx REAL, gy REAL, gz REAL
-    )''')
-    conn.execute('DELETE FROM imu_data')
-    conn.commit()
-    return conn
+# Масштабные коэффициенты (диапазон ±2g и ±250°/с)
+ACCEL_SCALE = 16384.0  # LSB/g
+GYRO_SCALE = 131.0     # LSB/(°/с)
 
 def init_mpu(bus):
+    """Разбудить датчик"""
     bus.write_byte_data(MPU_ADDRESS, PWR_MGMT_1, 0x00)
     time.sleep(0.1)
 
 def read_raw(bus, reg):
+    """Читать 16-битное значение из двух регистров"""
     high = bus.read_byte_data(MPU_ADDRESS, reg)
     low = bus.read_byte_data(MPU_ADDRESS, reg + 1)
     value = (high << 8) | low
+    # Перевод в знаковое число
     if value > 32767:
         value -= 65536
     return value
 
 def read_imu(bus):
+    """Читать и вернуть данные акселерометра и гироскопа"""
     ax = read_raw(bus, ACCEL_XOUT_H) / ACCEL_SCALE
     ay = read_raw(bus, ACCEL_XOUT_H + 2) / ACCEL_SCALE
     az = read_raw(bus, ACCEL_XOUT_H + 4) / ACCEL_SCALE
+
     gx = read_raw(bus, GYRO_XOUT_H) / GYRO_SCALE
     gy = read_raw(bus, GYRO_XOUT_H + 2) / GYRO_SCALE
     gz = read_raw(bus, GYRO_XOUT_H + 4) / GYRO_SCALE
+
     return ax, ay, az, gx, gy, gz
 
-def save_to_db():
-    global conn, bus, running
-    conn = init_db()
+def save_to_csv(filename, duration_seconds):
+    """Записать данные в CSV файл"""
     bus = smbus2.SMBus(I2C_BUS)
     init_mpu(bus)
 
-    print("Запись данных IMU...")
-    t = 0.0
-    try:
-        while running:
+    with open(filename, 'w', newline='') as f:
+        writer = csv.writer(f)
+        writer.writerow(['timestamp', 'ax', 'ay', 'az', 'gx', 'gy', 'gz'])
+
+        t = 0.0
+        while t < duration_seconds:
             ax, ay, az, gx, gy, gz = read_imu(bus)
-            conn.execute(
-                'INSERT INTO imu_data VALUES (?, ?, ?, ?, ?, ?, ?)',
-                (round(t, 3), round(ax, 4), round(ay, 4), round(az, 4),
-                 round(gx, 4), round(gy, 4), round(gz, 4))
-            )
-            conn.commit()
+            writer.writerow([round(t, 3), 
+                           round(ax, 4), round(ay, 4), round(az, 4),
+                           round(gx, 4), round(gy, 4), round(gz, 4)])
             t += 0.01
             time.sleep(0.01)
-    finally:
-        bus.close()
-        conn.close()
-        print("IMU модуль остановлен")
 
-save_to_db()
+    bus.close()
+
+# Запуск — записываем 5 секунд
+save_to_csv('imu.csv', 5)
+print("Готово!")

lidar_module/lidar_reader.py

@@ -1,104 +1,87 @@
 import serial
+import csv
 import time
-import sqlite3
 import struct
-import signal
 
 # Константы
-SERIAL_PORT = '/dev/ttyS0'
-BAUD_RATE = 230400
-DB_PATH = '../inertial_data.db'
+SERIAL_PORT = '/dev/ttyS0'  # UART порт на MangoPi
+BAUD_RATE = 230400          # скорость передачи LDS02RR
+DURATION = 5                # секунд записи
 
 # Заголовок пакета LDS02RR
 HEADER = 0xFA
-PACKET_SIZE = 22
-
-# Глобальные переменные для завершения
-running = True
-conn = None
-ser = None
-
-def shutdown(signum, frame):
-    """Обработчик сигнала завершения"""
-    global running
-    print("Завершение лидар модуля...")
-    running = False
-
-# Перехватываем сигналы завершения
-signal.signal(signal.SIGTERM, shutdown)
-signal.signal(signal.SIGINT, shutdown)
-
-def init_db():
-    """Создать таблицу если не существует и очистить"""
-    conn = sqlite3.connect(DB_PATH)
-    conn.execute('''CREATE TABLE IF NOT EXISTS lidar_data (
-        timestamp REAL,
-        angle REAL,
-        distance_mm REAL,
-        quality INTEGER
-    )''')
-    conn.execute('DELETE FROM lidar_data')
-    conn.commit()
-    return conn
+PACKET_SIZE = 22            # размер пакета в байтах
 
 def parse_packet(data):
     """Парсить один пакет данных лидара"""
-    if len(data) < PACKET_SIZE or data[0] != HEADER:
+    if len(data) < PACKET_SIZE:
         return None
+    
+    if data[0] != HEADER:
+        return None
+
+    # Индекс угла (0xA0 = 0°, 0xF9 = 359°)
     index = data[1] - 0xA0
     if index < 0 or index > 89:
         return None
+
+    # Скорость вращения
+    speed = struct.unpack_from('<H', data, 2)[0] / 64.0
+
     points = []
     for i in range(4):
         offset = 4 + i * 4
         distance_raw = struct.unpack_from('<H', data, offset)[0]
-        quality = struct.unpack_from('<H', data, offset + 2)[0] >> 2
-        distance = distance_raw / 4.0
-        angle = index * 4 + i
+        quality = struct.unpack_from('<H', data, offset + 2)[0] >> 8
+
+        distance = distance_raw / 4.0  # в мм
+        angle = index * 4 + i          # угол в градусах
+
         points.append((angle, distance, quality))
+
     return points
 
-def save_to_db():
-    """Основной цикл записи данных в БД"""
-    global conn, ser, running
-    conn = init_db()
-
+def save_to_csv(filename, duration_seconds):
+    """Читать данные лидара и записывать в CSV"""
     try:
         ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1)
-        print("Подключено к лидару")
+        print(f"Подключено к {SERIAL_PORT}")
     except Exception as e:
         print(f"Ошибка подключения: {e}")
-        conn.close()
         return
 
-    print("Запись данных лидара...")
-    buffer = bytearray()
-    start_time = time.time()
+    with open(filename, 'w', newline='') as f:
+        writer = csv.writer(f)
+        writer.writerow(['timestamp', 'angle', 'distance_mm', 'quality'])
 
-    try:
-        while running:
+        start_time = time.time()
+        buffer = bytearray()
+
+        while time.time() - start_time < duration_seconds:
             data = ser.read(PACKET_SIZE)
             if not data:
                 continue
+
             buffer.extend(data)
+
+            # Ищем заголовок пакета
             while len(buffer) >= PACKET_SIZE:
                 if buffer[0] == HEADER:
                     packet = bytes(buffer[:PACKET_SIZE])
                     points = parse_packet(packet)
+
                     if points:
                         t = round(time.time() - start_time, 3)
                         for angle, distance, quality in points:
-                            conn.execute(
-                                'INSERT INTO lidar_data VALUES (?, ?, ?, ?)',
-                                (t, angle, round(distance, 1), quality)
-                            )
-                        conn.commit()
+                            writer.writerow([t, angle, 
+                                           round(distance, 1), 
+                                           quality])
                     buffer = buffer[PACKET_SIZE:]
                 else:
                     buffer.pop(0)
-    finally:
-        ser.close()
-        conn.close()
-        print("Лидар модуль остановлен")
 
-save_to_db()
+    ser.close()
+    print("Готово!")
+
+# Запуск
+save_to_csv('lidar.csv', DURATION)

processing/calculations.cpp

@@ -1,35 +1,26 @@
 #include <iostream>
+#include <fstream>
+#include <sstream>
 #include <vector>
 #include <cmath>
 #include <algorithm>
-#include <chrono>
-#include <thread>
-#include <csignal>
-#include <sqlite3.h>
 #include <Eigen/Dense>
 
 using namespace std;
 using namespace Eigen;
 
-// ===== ФЛАГ ЗАВЕРШЕНИЯ =====
-volatile sig_atomic_t running = 1;
-
-void shutdown(int signum) {
-    running = 0;
-}
-
-// ===== СТРУКТУРЫ =====
+// СТРУКТУРЫ
 
 struct IMUData {
     double t;
-    Vector3d acc;
-    Vector3d gyro;
+    Vector3d acc;   // g
+    Vector3d gyro;  // deg/s
 };
 
 struct LidarData {
     double t;
     double angle;
-    double distance;
+    double distance; // mm
 };
 
 struct State {
@@ -37,91 +28,59 @@ struct State {
     MatrixXd P;
 };
 
-// ===== ЧТЕНИЕ ИЗ SQLite =====
+// ЧТЕНИЕ CSV
 
-vector<IMUData> readIMU(sqlite3* db) {
+vector<IMUData> readIMU(const string& filename) {
     vector<IMUData> data;
-    sqlite3_stmt* stmt;
+    ifstream file(filename);
+    string line;
 
-    const char* sql = "SELECT timestamp, ax, ay, az, gx, gy, gz FROM imu_data ORDER BY timestamp";
-    sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
+    getline(file, line);
+
+    while (getline(file, line)) {
+        stringstream ss(line);
+        string val;
 
-    while (sqlite3_step(stmt) == SQLITE_ROW) {
         IMUData d;
-        d.t = sqlite3_column_double(stmt, 0);
-        d.acc.x() = sqlite3_column_double(stmt, 1);
-        d.acc.y() = sqlite3_column_double(stmt, 2);
-        d.acc.z() = sqlite3_column_double(stmt, 3);
-        d.gyro.x() = sqlite3_column_double(stmt, 4);
-        d.gyro.y() = sqlite3_column_double(stmt, 5);
-        d.gyro.z() = sqlite3_column_double(stmt, 6);
+
+        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);
     }
 
-    sqlite3_finalize(stmt);
     return data;
 }
 
-vector<LidarData> readLidar(sqlite3* db) {
+vector<LidarData> readLidar(const string& filename) {
     vector<LidarData> data;
-    sqlite3_stmt* stmt;
+    ifstream file(filename);
+    string line;
 
-    const char* sql = "SELECT timestamp, angle, distance_mm FROM lidar_data ORDER BY timestamp";
-    sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
+    getline(file, line);
+
+    while (getline(file, line)) {
+        stringstream ss(line);
+        string val;
 
-    while (sqlite3_step(stmt) == SQLITE_ROW) {
         LidarData d;
-        d.t = sqlite3_column_double(stmt, 0);
-        d.angle = sqlite3_column_double(stmt, 1);
-        d.distance = sqlite3_column_double(stmt, 2);
+
+        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);
     }
 
-    sqlite3_finalize(stmt);
     return data;
 }
 
-// ===== ЗАПИСЬ В SQLite =====
-
-void initDB(sqlite3* db) {
-    const char* sql =
-        "CREATE TABLE IF NOT EXISTS trajectory ("
-        "timestamp REAL, x REAL, y REAL, z REAL,"
-        "roll REAL, pitch REAL, yaw REAL);"
-        "DELETE FROM trajectory;"
-        "CREATE TABLE IF NOT EXISTS lidar_points ("
-        "x REAL, y REAL, z REAL);"
-        "DELETE FROM lidar_points;";
-    sqlite3_exec(db, sql, nullptr, nullptr, nullptr);
-}
-
-void writeTraj(sqlite3* db, double t, Vector3d pos, Vector3d ang) {
-    sqlite3_stmt* stmt;
-    const char* sql = "INSERT INTO trajectory VALUES (?,?,?,?,?,?,?)";
-    sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
-    sqlite3_bind_double(stmt, 1, t);
-    sqlite3_bind_double(stmt, 2, pos.x());
-    sqlite3_bind_double(stmt, 3, pos.y());
-    sqlite3_bind_double(stmt, 4, pos.z());
-    sqlite3_bind_double(stmt, 5, ang.x());
-    sqlite3_bind_double(stmt, 6, ang.y());
-    sqlite3_bind_double(stmt, 7, ang.z());
-    sqlite3_step(stmt);
-    sqlite3_finalize(stmt);
-}
-
-void writeLidarPoint(sqlite3* db, Vector3d p) {
-    sqlite3_stmt* stmt;
-    const char* sql = "INSERT INTO lidar_points VALUES (?,?,?)";
-    sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
-    sqlite3_bind_double(stmt, 1, p.x());
-    sqlite3_bind_double(stmt, 2, p.y());
-    sqlite3_bind_double(stmt, 3, p.z());
-    sqlite3_step(stmt);
-    sqlite3_finalize(stmt);
-}
-
-// ===== ФИЗИКА (без изменений) =====
+// ПОВОРОТ
 
 Matrix3d rotationMatrix(double roll, double pitch, double yaw) {
     AngleAxisd rx(roll, Vector3d::UnitX());
@@ -130,8 +89,11 @@ Matrix3d rotationMatrix(double roll, double pitch, double yaw) {
     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);
@@ -140,6 +102,7 @@ void predict(State& s, const IMUData& imu, double dt) {
     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;
 
@@ -153,167 +116,245 @@ void predict(State& s, const IMUData& imu, double dt) {
 
     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;
+    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;
 }
 
-// ===== ICP (без изменений) =====
+// 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<Vector3d>& 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]; });
+        [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<Vector3d> left(pts.begin(), pts.begin() + mid);
     vector<Vector3d> 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; }
+    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);
+    if (diff * diff < best_dist) {
+        nearestSearch(far, target, best, best_dist);
+    }
 }
 
-Matrix3d computeRotation(const vector<Vector3d>& src, const vector<Vector3d>& dst) {
-    Vector3d c1 = Vector3d::Zero(), c2 = Vector3d::Zero();
-    for (size_t i = 0; i < src.size(); i++) { c1 += src[i]; c2 += dst[i]; }
-    c1 /= src.size(); c2 /= dst.size();
+// ICP
+
+Matrix3d computeRotation(const vector<Vector3d>& src,
+    const vector<Vector3d>& 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++)
+
+    for (size_t i = 0;i < src.size();i++) {
         H += (src[i] - c1) * (dst[i] - c2).transpose();
+    }
+
     JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
     return svd.matrixV() * svd.matrixU().transpose();
 }
 
-Vector3d computeTranslation(const vector<Vector3d>& src, const vector<Vector3d>& dst, const Matrix3d& R) {
-    Vector3d c1 = Vector3d::Zero(), c2 = Vector3d::Zero();
-    for (size_t i = 0; i < src.size(); i++) { c1 += src[i]; c2 += dst[i]; }
-    c1 /= src.size(); c2 /= dst.size();
+Vector3d computeTranslation(const vector<Vector3d>& src,
+    const vector<Vector3d>& 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<Vector3d> downsample(const vector<Vector3d>& pts, int step = 5) {
     vector<Vector3d> out;
-    for (size_t i = 0; i < pts.size(); i += step)
+    for (size_t i = 0;i < pts.size();i += step)
         out.push_back(pts[i]);
     return out;
 }
 
-void ICP_fast(vector<Vector3d>& src, vector<Vector3d>& dst, Matrix3d& R, Vector3d& t) {
+void ICP_fast(vector<Vector3d>& src,
+    vector<Vector3d>& dst,
+    Matrix3d& R, Vector3d& t) {
+
     R = Matrix3d::Identity();
     t = Vector3d::Zero();
+
     vector<Vector3d> dst_copy = dst;
     KDNode* tree = buildKDTree(dst_copy);
+
     for (int iter = 0; iter < 10; iter++) {
+
         vector<Vector3d> matched;
+
         for (auto& p : src) {
-            Vector3d best; double best_dist = 1e9;
+            Vector3d best;
+            double best_dist = 1e9;
+
             nearestSearch(tree, p, best, best_dist);
-            if (best_dist < 0.5) matched.push_back(best);
+
+            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;
+
+        for (auto& p : src)
+            p = dR * p + dt;
+
         R = dR * R;
         t = dR * t + dt;
     }
 }
 
-// ===== MAIN =====
+// MAIN
 
 int main() {
-    signal(SIGTERM, shutdown);
-    signal(SIGINT, shutdown);
-
-    sqlite3* db;
-    sqlite3_open("../inertial_data.db", &db);
-    initDB(db);
+    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<Vector3d> prev_scan, curr_scan;
+
     size_t lidar_idx = 0;
-    size_t imu_idx = 0;
 
-    cout << "Расчёты запущены..." << endl;
+    for (size_t i = 1;i < imu.size();i++) {
 
-    while (running) {
-        auto imu = readIMU(db);
-        auto lidar = readLidar(db);
+        double dt = imu[i].t - imu[i - 1].t;
+        if (dt <= 0 || dt > 0.1) continue;
 
-        // Обрабатываем только новые данные
-        for (size_t i = max((size_t)1, imu_idx); 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);
 
-            predict(state, imu[i], dt);
+        while (lidar_idx < lidar.size() &&
+            lidar[lidar_idx].t <= imu[i].t) {
 
-            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<Vector3d> src = downsample(curr_scan, 5);
-                    vector<Vector3d> dst = downsample(prev_scan, 5);
-                    Matrix3d R; Vector3d t;
-                    ICP_fast(src, dst, R, t);
-                    state.x.segment<3>(0) += t;
-                }
-                for (auto& p : curr_scan)
-                    writeLidarPoint(db, p);
-                prev_scan = curr_scan;
-                curr_scan.clear();
-            }
-
-            Vector3d pos = state.x.segment<3>(0);
-            Vector3d ang = state.x.segment<3>(6);
-            writeTraj(db, imu[i].t, pos, ang);
+            curr_scan.push_back(lidarToWorld(state, lidar[lidar_idx]));
+            lidar_idx++;
         }
 
-        imu_idx = imu.size();
+        if (curr_scan.size() > 200) {
 
-        // Ждём 200 мс перед следующим циклом
-        this_thread::sleep_for(chrono::milliseconds(200));
+            if (!prev_scan.empty()) {
+
+                vector<Vector3d> src = downsample(curr_scan, 5);
+                vector<Vector3d> 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";
     }
 
-    sqlite3_close(db);
-    cout << "Расчёты остановлены" << endl;
+    for (auto& p : prev_scan) {
+        lidar_out << p.x() << ","
+            << p.y() << ","
+            << p.z() << "\n";
+    }
+
+    cout << "Done\n";
     return 0;
 }

processing/visualization.py

@@ -1,41 +1,55 @@
 import open3d as o3d
 import numpy as np
-import sqlite3
-import signal
-import sys
+import pandas as pd
+import matplotlib.pyplot as plt
 import os
-import time
+from datetime import datetime
 
-DB_PATH = '../inertial_data.db'
+# СОЗДАНИЕ УНИКАЛЬНОЙ ПАПКИ
 
-# ===== ЗАВЕРШЕНИЕ ВСЕГО ПРОЕКТА =====
+timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
+output_dir = f"results/run_{timestamp}"
+frames_dir = os.path.join(output_dir, "frames")
 
-def shutdown_all():
-    """Останавливает все процессы проекта"""
-    os.system("pkill -f imu_reader.py")
-    os.system("pkill -f lidar_reader.py")
-    os.system("pkill -f calculations")
+os.makedirs(frames_dir, exist_ok=True)
 
-# ===== ЧТЕНИЕ ИЗ SQLite =====
+# ЗАГРУЗКА ДАННЫХ
 
-def read_trajectory(conn):
-    cursor = conn.execute(
-        "SELECT x, y, z, roll, pitch, yaw FROM trajectory ORDER BY timestamp"
-    )
-    rows = cursor.fetchall()
-    if not rows:
-        return np.zeros((0, 3)), np.zeros((0, 3))
-    data = np.array(rows)
-    return data[:, :3], data[:, 3:]
+traj = pd.read_csv("trajectory.csv")
+lidar = pd.read_csv("lidar_points.csv")
 
-def read_lidar_points(conn):
-    cursor = conn.execute("SELECT x, y, z FROM lidar_points")
-    rows = cursor.fetchall()
-    if not rows:
-        return np.zeros((0, 3))
-    return np.array(rows)
+trajectory = traj[['x', 'y', 'z']].values
+angles = traj[['roll', 'pitch', 'yaw']].values
+lidar_points = lidar[['x', 'y', 'z']].values
 
-# ===== ПОВОРОТ =====
+# СОХРАНЕНИЕ 2D ГРАФИКОВ
+
+plt.figure()
+plt.plot(trajectory[:,0], trajectory[:,1])
+plt.title("Trajectory (X-Y)")
+plt.xlabel("X")
+plt.ylabel("Y")
+plt.grid()
+plt.savefig(os.path.join(output_dir, "trajectory_plot.png"))
+plt.close()
+
+# OPEN3D
+
+pcd = o3d.geometry.PointCloud()
+pcd.points = o3d.utility.Vector3dVector(lidar_points)
+
+lines = [[i, i+1] for i in range(len(trajectory)-1)]
+line_set = o3d.geometry.LineSet()
+line_set.points = o3d.utility.Vector3dVector(trajectory)
+line_set.lines = o3d.utility.Vector2iVector(lines)
+
+vis = o3d.visualization.Visualizer()
+vis.create_window(visible=False)  # важно для сохранения кадров
+
+vis.add_geometry(pcd)
+vis.add_geometry(line_set)
+
+# ПОВОРОТ
 
 def get_rotation_matrix(roll, pitch, yaw):
     Rx = np.array([
@@ -55,68 +69,36 @@ def get_rotation_matrix(roll, pitch, yaw):
     ])
     return Rz @ Ry @ Rx
 
-# ===== MAIN =====
+# СОХРАНЕНИЕ КАДРОВ
 
-def main():
-    conn = sqlite3.connect(DB_PATH)
+for i in range(len(trajectory)):
 
-    # Создаём окно Open3D
-    vis = o3d.visualization.Visualizer()
-    vis.create_window(window_name="Инерциальное управление", width=1280, height=720)
-
-    pcd = o3d.geometry.PointCloud()
-    line_set = o3d.geometry.LineSet()
     frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.2)
 
+    pos = trajectory[i]
+    roll, pitch, yaw = angles[i]
+
+    R = get_rotation_matrix(roll, pitch, yaw)
+
+    frame.rotate(R, center=(0,0,0))
+    frame.translate(pos)
+
+    vis.clear_geometries()
     vis.add_geometry(pcd)
     vis.add_geometry(line_set)
     vis.add_geometry(frame)
 
-    print("Визуализация запущена. Закройте окно для остановки проекта.")
+    vis.poll_events()
+    vis.update_renderer()
 
-    try:
-        while True:
-            # Проверяем закрыто ли окно
-            if not vis.poll_events():
-                print("Окно закрыто — останавливаем проект...")
-                shutdown_all()
-                break
+    # сохраняем изображение
+    img_path = os.path.join(frames_dir, f"frame_{i:04d}.png")
+    vis.capture_screen_image(img_path)
 
-            # Читаем новые данные из БД
-            trajectory, angles = read_trajectory(conn)
-            lidar_points = read_lidar_points(conn)
+# СОХРАНЕНИЕ ФИНАЛЬНОГО ВИДА
 
-            # Обновляем облако точек лидара
-            if len(lidar_points) > 0:
-                pcd.points = o3d.utility.Vector3dVector(lidar_points)
-                vis.update_geometry(pcd)
+vis.capture_screen_image(os.path.join(output_dir, "3d_view.png"))
 
-            # Обновляем траекторию
-            if len(trajectory) > 1:
-                lines = [[i, i+1] for i in range(len(trajectory)-1)]
-                line_set.points = o3d.utility.Vector3dVector(trajectory)
-                line_set.lines = o3d.utility.Vector2iVector(lines)
-                vis.update_geometry(line_set)
+vis.destroy_window()
 
-            # Обновляем позицию объекта
-            if len(trajectory) > 0:
-                pos = trajectory[-1]
-                roll, pitch, yaw = angles[-1]
-                R = get_rotation_matrix(roll, pitch, yaw)
-                frame_new = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.2)
-                frame_new.rotate(R, center=(0, 0, 0))
-                frame_new.translate(pos)
-                frame.vertices = frame_new.vertices
-                frame.triangles = frame_new.triangles
-                vis.update_geometry(frame)
-
-            vis.update_renderer()
-
-            # Обновляем каждые 200 мс
-            time.sleep(0.2)
-
-    finally:
-        vis.destroy_window()
-        conn.close()
-
-main()
+print(f"Результаты сохранены в: {output_dir}")

run.sh

@@ -1,26 +1,22 @@
 #!/bin/bash
+echo "Запуск системы..."
 
-# Переходим в папку проекта
-cd "$(dirname "$0")"
-
-echo "Запуск проекта..."
-
-# Запускаем модули сбора данных в фоне
+# Запуск модуля акселерометра
+echo "Запуск IMU модуля..."
 python3 imu_module/imu_reader.py &
-IMU_PID=$!
 
+# Запуск модуля лидара
+echo "Запуск лидар модуля..."
 python3 lidar_module/lidar_reader.py &
-LIDAR_PID=$!
 
-# Запускаем расчёты в фоне
-processing/calculations &
-CALC_PID=$!
-
-# Запускаем визуализацию (она управляет завершением)
-python3 processing/visualization.py
-
-# Когда визуализация закрыта — останавливаем всё
-echo "Остановка проекта..."
-kill $IMU_PID $LIDAR_PID $CALC_PID 2>/dev/null
+# Ждём пока данные запишутся
 wait
-echo "Проект остановлен"
+
+# Компиляция и запуск расчётов
+echo "Запуск расчётов..."
+g++ processing/calculations.cpp -o processing/calculations
+./processing/calculations
+
+# Запуск визуализации
+echo "Запуск визуализации..."
+python3 processing/visualization.py