Compare commits
No commits in common. "feature/sqlite-realtime" and "main" have entirely different histories.
feature/sq
...
main
|
|
@ -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("Готово!")
|
||||
|
|
@ -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)
|
||||
|
|
@ -1,319 +0,0 @@
|
|||
#include <iostream>
|
||||
#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;
|
||||
};
|
||||
|
||||
struct LidarData {
|
||||
double t;
|
||||
double angle;
|
||||
double distance;
|
||||
};
|
||||
|
||||
struct State {
|
||||
VectorXd x;
|
||||
MatrixXd P;
|
||||
};
|
||||
|
||||
// ===== ЧТЕНИЕ ИЗ SQLite =====
|
||||
|
||||
vector<IMUData> readIMU(sqlite3* db) {
|
||||
vector<IMUData> data;
|
||||
sqlite3_stmt* stmt;
|
||||
|
||||
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 (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);
|
||||
data.push_back(d);
|
||||
}
|
||||
|
||||
sqlite3_finalize(stmt);
|
||||
return data;
|
||||
}
|
||||
|
||||
vector<LidarData> readLidar(sqlite3* db) {
|
||||
vector<LidarData> data;
|
||||
sqlite3_stmt* stmt;
|
||||
|
||||
const char* sql = "SELECT timestamp, angle, distance_mm FROM lidar_data ORDER BY timestamp";
|
||||
sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
|
||||
|
||||
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);
|
||||
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());
|
||||
AngleAxisd ry(pitch, Vector3d::UnitY());
|
||||
AngleAxisd rz(yaw, Vector3d::UnitZ());
|
||||
return (rz * ry * rx).toRotationMatrix();
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// ===== 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) { 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; }
|
||||
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);
|
||||
}
|
||||
|
||||
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();
|
||||
Matrix3d H = Matrix3d::Zero();
|
||||
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();
|
||||
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)
|
||||
out.push_back(pts[i]);
|
||||
return out;
|
||||
}
|
||||
|
||||
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;
|
||||
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() {
|
||||
signal(SIGTERM, shutdown);
|
||||
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;
|
||||
|
||||
vector<Vector3d> prev_scan, curr_scan;
|
||||
size_t lidar_idx = 0;
|
||||
size_t imu_idx = 0;
|
||||
|
||||
cout << "Расчёты запущены..." << endl;
|
||||
|
||||
while (running) {
|
||||
auto imu = readIMU(db);
|
||||
auto lidar = readLidar(db);
|
||||
|
||||
// Обрабатываем только новые данные
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
imu_idx = imu.size();
|
||||
|
||||
// Ждём 200 мс перед следующим циклом
|
||||
this_thread::sleep_for(chrono::milliseconds(200));
|
||||
}
|
||||
|
||||
sqlite3_close(db);
|
||||
cout << "Расчёты остановлены" << endl;
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,122 +0,0 @@
|
|||
import open3d as o3d
|
||||
import numpy as np
|
||||
import sqlite3
|
||||
import signal
|
||||
import sys
|
||||
import os
|
||||
import time
|
||||
|
||||
DB_PATH = '../inertial_data.db'
|
||||
|
||||
# ===== ЗАВЕРШЕНИЕ ВСЕГО ПРОЕКТА =====
|
||||
|
||||
def shutdown_all():
|
||||
"""Останавливает все процессы проекта"""
|
||||
os.system("pkill -f imu_reader.py")
|
||||
os.system("pkill -f lidar_reader.py")
|
||||
os.system("pkill -f calculations")
|
||||
|
||||
# ===== ЧТЕНИЕ ИЗ 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:]
|
||||
|
||||
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)
|
||||
|
||||
# ===== ПОВОРОТ =====
|
||||
|
||||
def get_rotation_matrix(roll, pitch, yaw):
|
||||
Rx = np.array([
|
||||
[1, 0, 0],
|
||||
[0, np.cos(roll), -np.sin(roll)],
|
||||
[0, np.sin(roll), np.cos(roll)]
|
||||
])
|
||||
Ry = np.array([
|
||||
[np.cos(pitch), 0, np.sin(pitch)],
|
||||
[0, 1, 0],
|
||||
[-np.sin(pitch), 0, np.cos(pitch)]
|
||||
])
|
||||
Rz = np.array([
|
||||
[np.cos(yaw), -np.sin(yaw), 0],
|
||||
[np.sin(yaw), np.cos(yaw), 0],
|
||||
[0, 0, 1]
|
||||
])
|
||||
return Rz @ Ry @ Rx
|
||||
|
||||
# ===== MAIN =====
|
||||
|
||||
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)
|
||||
|
||||
vis.add_geometry(pcd)
|
||||
vis.add_geometry(line_set)
|
||||
vis.add_geometry(frame)
|
||||
|
||||
print("Визуализация запущена. Закройте окно для остановки проекта.")
|
||||
|
||||
try:
|
||||
while True:
|
||||
# Проверяем закрыто ли окно
|
||||
if not vis.poll_events():
|
||||
print("Окно закрыто — останавливаем проект...")
|
||||
shutdown_all()
|
||||
break
|
||||
|
||||
# Читаем новые данные из БД
|
||||
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)
|
||||
|
||||
# Обновляем траекторию
|
||||
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)
|
||||
|
||||
# Обновляем позицию объекта
|
||||
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()
|
||||
34
run.sh
34
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
|
||||
Loading…
Reference in New Issue
Block a user