Merge pull request 'feat: replace CSV with SQLite realtime database' (#2) from feature/sqlite-realtime into develop

Reviewed-on: #2

imu_module/imu_reader.py

@@ -1,68 +1,92 @@
-import smbus2 #библиотека Python для работы с I2C шиной
+import smbus2
 import time
-import csv
+import sqlite3
-import struct
+import signal
+import sys
 
 # Константы
 MPU_ADDRESS = 0x68
-I2C_BUS = 1
+I2C_BUS = 2
+DB_PATH = '../inertial_data.db'
 
 # Регистры MPU-9250
-PWR_MGMT_1 = 0x6B      # управление питанием
+PWR_MGMT_1 = 0x6B
-ACCEL_XOUT_H = 0x3B    # начало данных акселерометра
+ACCEL_XOUT_H = 0x3B
-GYRO_XOUT_H = 0x43     # начало данных гироскопа
+GYRO_XOUT_H = 0x43
 
-# Масштабные коэффициенты (диапазон ±2g и ±250°/с)
+# Масштабные коэффициенты
-ACCEL_SCALE = 16384.0  # LSB/g
+ACCEL_SCALE = 16384.0
-GYRO_SCALE = 131.0     # LSB/(°/с)
+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
 
 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_csv(filename, duration_seconds):
+def save_to_db():
-    """Записать данные в CSV файл"""
+    global conn, bus, running
+    conn = init_db()
     bus = smbus2.SMBus(I2C_BUS)
     init_mpu(bus)
 
-    with open(filename, 'w', newline='') as f:
+    print("Запись данных IMU...")
-        writer = csv.writer(f)
+    t = 0.0
-        writer.writerow(['timestamp', 'ax', 'ay', 'az', 'gx', 'gy', 'gz'])
+    try:
-
+        while running:
-        t = 0.0
-        while t < duration_seconds:
             ax, ay, az, gx, gy, gz = read_imu(bus)
-            writer.writerow([round(t, 3), 
+            conn.execute(
-                           round(ax, 4), round(ay, 4), round(az, 4),
+                'INSERT INTO imu_data VALUES (?, ?, ?, ?, ?, ?, ?)',
-                           round(gx, 4), round(gy, 4), round(gz, 4)])
+                (round(t, 3), round(ax, 4), round(ay, 4), round(az, 4),
+                 round(gx, 4), round(gy, 4), round(gz, 4))
+            )
+            conn.commit()
             t += 0.01
             time.sleep(0.01)
+    finally:
+        bus.close()
+        conn.close()
+        print("IMU модуль остановлен")
 
-    bus.close()
+save_to_db()
-
-# Запуск — записываем 5 секунд
-save_to_csv('imu.csv', 5)
-print("Готово!")

lidar_module/lidar_reader.py

@@ -1,87 +1,104 @@
 import serial
-import csv
 import time
+import sqlite3
 import struct
+import signal
 
 # Константы
-SERIAL_PORT = '/dev/ttyS0'  # UART порт на MangoPi
+SERIAL_PORT = '/dev/ttyS0'
-BAUD_RATE = 230400          # скорость передачи LDS02RR
+BAUD_RATE = 230400
-DURATION = 5                # секунд записи
+DB_PATH = '../inertial_data.db'
 
 # Заголовок пакета LDS02RR
 HEADER = 0xFA
-PACKET_SIZE = 22            # размер пакета в байтах
+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
 
 def parse_packet(data):
     """Парсить один пакет данных лидара"""
-    if len(data) < PACKET_SIZE:
+    if len(data) < PACKET_SIZE or data[0] != HEADER:
         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] >> 8
+        quality = struct.unpack_from('<H', data, offset + 2)[0] >> 2
-
+        distance = distance_raw / 4.0
-        distance = distance_raw / 4.0  # в мм
+        angle = index * 4 + i
-        angle = index * 4 + i          # угол в градусах
-
         points.append((angle, distance, quality))
-
     return points
 
-def save_to_csv(filename, duration_seconds):
+def save_to_db():
-    """Читать данные лидара и записывать в CSV"""
+    """Основной цикл записи данных в БД"""
+    global conn, ser, running
+    conn = init_db()
+
     try:
         ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1)
-        print(f"Подключено к {SERIAL_PORT}")
+        print("Подключено к лидару")
     except Exception as e:
         print(f"Ошибка подключения: {e}")
+        conn.close()
         return
 
-    with open(filename, 'w', newline='') as f:
+    print("Запись данных лидара...")
-        writer = csv.writer(f)
+    buffer = bytearray()
-        writer.writerow(['timestamp', 'angle', 'distance_mm', 'quality'])
+    start_time = time.time()
 
-        start_time = time.time()
+    try:
-        buffer = bytearray()
+        while running:
-
-        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:
-                            writer.writerow([t, angle, 
+                            conn.execute(
-                                           round(distance, 1), 
+                                'INSERT INTO lidar_data VALUES (?, ?, ?, ?)',
-                                           quality])
+                                (t, angle, round(distance, 1), quality)
+                            )
+                        conn.commit()
                     buffer = buffer[PACKET_SIZE:]
                 else:
                     buffer.pop(0)
+    finally:
+        ser.close()
+        conn.close()
+        print("Лидар модуль остановлен")
 
-    ser.close()
+save_to_db()
-    print("Готово!")
-
-# Запуск
-save_to_csv('lidar.csv', DURATION)

processing/calculations.cpp

@@ -1,26 +1,35 @@
 #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;   // g
+    Vector3d acc;
-    Vector3d gyro;  // deg/s
+    Vector3d gyro;
 };
 
 struct LidarData {
     double t;
     double angle;
-    double distance; // mm
+    double distance;
 };
 
 struct State {
@@ -28,59 +37,91 @@ struct State {
     MatrixXd P;
 };
 
-// ЧТЕНИЕ CSV
+// ===== ЧТЕНИЕ ИЗ SQLite =====
 
-vector<IMUData> readIMU(const string& filename) {
+vector<IMUData> readIMU(sqlite3* db) {
     vector<IMUData> data;
-    ifstream file(filename);
+    sqlite3_stmt* stmt;
-    string line;
 
-    getline(file, 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);
-    while (getline(file, line)) {
-        stringstream ss(line);
-        string val;
 
+    while (sqlite3_step(stmt) == SQLITE_ROW) {
         IMUData d;
-
+        d.t = sqlite3_column_double(stmt, 0);
-        getline(ss, val, ','); d.t = stod(val);
+        d.acc.x() = sqlite3_column_double(stmt, 1);
-        getline(ss, val, ','); d.acc.x() = stod(val);
+        d.acc.y() = sqlite3_column_double(stmt, 2);
-        getline(ss, val, ','); d.acc.y() = stod(val);
+        d.acc.z() = sqlite3_column_double(stmt, 3);
-        getline(ss, val, ','); d.acc.z() = stod(val);
+        d.gyro.x() = sqlite3_column_double(stmt, 4);
-        getline(ss, val, ','); d.gyro.x() = stod(val);
+        d.gyro.y() = sqlite3_column_double(stmt, 5);
-        getline(ss, val, ','); d.gyro.y() = stod(val);
+        d.gyro.z() = sqlite3_column_double(stmt, 6);
-        getline(ss, val, ','); d.gyro.z() = stod(val);
-
         data.push_back(d);
     }
 
+    sqlite3_finalize(stmt);
     return data;
 }
 
-vector<LidarData> readLidar(const string& filename) {
+vector<LidarData> readLidar(sqlite3* db) {
     vector<LidarData> data;
-    ifstream file(filename);
+    sqlite3_stmt* stmt;
-    string line;
 
-    getline(file, line);
+    const char* sql = "SELECT timestamp, angle, distance_mm FROM lidar_data ORDER BY timestamp";
-
+    sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
-    while (getline(file, line)) {
-        stringstream ss(line);
-        string val;
 
+    while (sqlite3_step(stmt) == SQLITE_ROW) {
         LidarData d;
-
+        d.t = sqlite3_column_double(stmt, 0);
-        getline(ss, val, ','); d.t = stod(val);
+        d.angle = sqlite3_column_double(stmt, 1);
-        getline(ss, val, ','); d.angle = stod(val);
+        d.distance = sqlite3_column_double(stmt, 2);
-        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());
@@ -89,11 +130,8 @@ 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);
@@ -102,7 +140,6 @@ 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;
 
@@ -116,245 +153,167 @@ 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;
 }
 
-// KD-TREE
+// ===== ICP (без изменений) =====
 
 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) {
+        [axis](const Vector3d& a, const Vector3d& b) { return a[axis] < b[axis]; });
-            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) {
+    if (d < best_dist) { best_dist = d; best = node->point; }
-        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) {
+    if (diff * diff < best_dist) nearestSearch(far, target, best, best_dist);
-        nearestSearch(far, target, best, best_dist);
-    }
 }
 
-// ICP
+Matrix3d computeRotation(const vector<Vector3d>& src, const vector<Vector3d>& dst) {
-
+    Vector3d c1 = Vector3d::Zero(), c2 = Vector3d::Zero();
-Matrix3d computeRotation(const vector<Vector3d>& src,
+    for (size_t i = 0; i < src.size(); i++) { c1 += src[i]; c2 += dst[i]; }
-    const vector<Vector3d>& dst) {
+    c1 /= src.size(); c2 /= dst.size();
-
-    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,
+Vector3d computeTranslation(const vector<Vector3d>& src, const vector<Vector3d>& dst, const Matrix3d& R) {
-    const vector<Vector3d>& dst,
+    Vector3d c1 = Vector3d::Zero(), c2 = Vector3d::Zero();
-    const Matrix3d& R) {
+    for (size_t i = 0; i < src.size(); i++) { c1 += src[i]; c2 += dst[i]; }
-
+    c1 /= src.size(); c2 /= dst.size();
-    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,
+void ICP_fast(vector<Vector3d>& src, vector<Vector3d>& dst, Matrix3d& R, Vector3d& t) {
-    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;
+            Vector3d best; double best_dist = 1e9;
-            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() {
-    auto imu = readIMU("../imu_module/imu.csv");
+    signal(SIGTERM, shutdown);
-    auto lidar = readLidar("../lidar_module/lidar.csv");
+    signal(SIGINT, shutdown);
+
+    sqlite3* db;
+    sqlite3_open("../inertial_data.db", &db);
+    initDB(db);
 
     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;
 
-    for (size_t i = 1;i < imu.size();i++) {
+    cout << "Расчёты запущены..." << endl;
 
-        double dt = imu[i].t - imu[i - 1].t;
+    while (running) {
-        if (dt <= 0 || dt > 0.1) continue;
+        auto imu = readIMU(db);
+        auto lidar = readLidar(db);
 
-        predict(state, imu[i], dt);
+        // Обрабатываем только новые данные
+        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;
 
-        while (lidar_idx < lidar.size() &&
+            predict(state, imu[i], dt);
-            lidar[lidar_idx].t <= imu[i].t) {
 
-            curr_scan.push_back(lidarToWorld(state, lidar[lidar_idx]));
+            while (lidar_idx < lidar.size() &&
-            lidar_idx++;
+                   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;
             }
 
-            prev_scan = curr_scan;
+            if (curr_scan.size() > 200) {
-            curr_scan.clear();
+                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);
         }
 
-        Vector3d pos = state.x.segment<3>(0);
+        imu_idx = imu.size();
-        Vector3d ang = state.x.segment<3>(6);
 
-        traj << imu[i].t << ","
+        // Ждём 200 мс перед следующим циклом
-            << pos.x() << ","
+        this_thread::sleep_for(chrono::milliseconds(200));
-            << pos.y() << ","
-            << pos.z() << ","
-            << ang.x() << ","
-            << ang.y() << ","
-            << ang.z() << "\n";
     }
 
-    for (auto& p : prev_scan) {
+    sqlite3_close(db);
-        lidar_out << p.x() << ","
+    cout << "Расчёты остановлены" << endl;
-            << p.y() << ","
-            << p.z() << "\n";
-    }
-
-    cout << "Done\n";
     return 0;
 }

processing/visualization.py

@@ -1,55 +1,41 @@
 import open3d as o3d
 import numpy as np
-import pandas as pd
+import sqlite3
-import matplotlib.pyplot as plt
+import signal
+import sys
 import os
-from datetime import datetime
+import time
 
-# СОЗДАНИЕ УНИКАЛЬНОЙ ПАПКИ
+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")
 
-os.makedirs(frames_dir, exist_ok=True)
+def shutdown_all():
+    """Останавливает все процессы проекта"""
+    os.system("pkill -f imu_reader.py")
+    os.system("pkill -f lidar_reader.py")
+    os.system("pkill -f calculations")
 
-# ЗАГРУЗКА ДАННЫХ
+# ===== ЧТЕНИЕ ИЗ SQLite =====
 
-traj = pd.read_csv("trajectory.csv")
+def read_trajectory(conn):
-lidar = pd.read_csv("lidar_points.csv")
+    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:]
 
-trajectory = traj[['x', 'y', 'z']].values
+def read_lidar_points(conn):
-angles = traj[['roll', 'pitch', 'yaw']].values
+    cursor = conn.execute("SELECT x, y, z FROM lidar_points")
-lidar_points = lidar[['x', 'y', 'z']].values
+    rows = cursor.fetchall()
+    if not rows:
+        return np.zeros((0, 3))
+    return np.array(rows)
 
-# СОХРАНЕНИЕ 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([
@@ -69,36 +55,68 @@ def get_rotation_matrix(roll, pitch, yaw):
     ])
     return Rz @ Ry @ Rx
 
-# СОХРАНЕНИЕ КАДРОВ
+# ===== MAIN =====
 
-for i in range(len(trajectory)):
+def main():
+    conn = sqlite3.connect(DB_PATH)
 
+    # Создаём окно 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)
 
-    vis.poll_events()
+    print("Визуализация запущена. Закройте окно для остановки проекта.")
-    vis.update_renderer()
 
-    # сохраняем изображение
+    try:
-    img_path = os.path.join(frames_dir, f"frame_{i:04d}.png")
+        while True:
-    vis.capture_screen_image(img_path)
+            # Проверяем закрыто ли окно
+            if not vis.poll_events():
+                print("Окно закрыто — останавливаем проект...")
+                shutdown_all()
+                break
 
-# СОХРАНЕНИЕ ФИНАЛЬНОГО ВИДА
+            # Читаем новые данные из БД
+            trajectory, angles = read_trajectory(conn)
+            lidar_points = read_lidar_points(conn)
 
-vis.capture_screen_image(os.path.join(output_dir, "3d_view.png"))
+            # Обновляем облако точек лидара
+            if len(lidar_points) > 0:
+                pcd.points = o3d.utility.Vector3dVector(lidar_points)
+                vis.update_geometry(pcd)
 
-vis.destroy_window()
+            # Обновляем траекторию
+            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)
 
-print(f"Результаты сохранены в: {output_dir}")
+            # Обновляем позицию объекта
+            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()

run.sh

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