-
Notifications
You must be signed in to change notification settings - Fork 0
/
drop.cpp
80 lines (66 loc) · 3.15 KB
/
drop.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
#include <sensor_msgs/Imu.h>
#include <cmath>
//可以在Estimator里声明这个类
class detect(){
// Global variables to track frame drop statistics
int total_seq_diff = 0;
double total_time_diff = 0.0;
double total_displacement_norm = 0.0;
double total_rotation_norm = 0.0;
double global_threshold=0.0;//动态阈值系数,当且仅当
//手动在failuredetection处setzero
void setzero(){
total_seq_diff = 0;
total_time_diff = 0.0;
total_displacement_norm = 0.0;
total_rotation_norm = 0.0;
}
bool detect_frame_drop(const sensor_msgs::Imu& prev_msg, const sensor_msgs::Imu& curr_msg,bool initialized) {
if (initialized==false)
return false;
int seq_diff = curr_msg.header.seq - prev_msg.header.seq;
// 判断当前序列是否连续(已弃用,手动setzero)
// if(seq_diff==1){
// std::cout << "Frame drop detected!" << std::endl;
// total_seq_diff = 0;
// total_time_diff = 0.0;
// total_displacement_norm = 0.0;
// total_rotation_norm = 0.0;
// }
double time_diff = (curr_msg.header.stamp - prev_msg.header.stamp).toSec();
Eigen::Vector3d cur_position(curr_msg.linear_acceleration.x,curr_msg.linear_acceleration.y,curr_msg.linear_acceleration.z);
Eigen::Vector3d prev_position(prev_msg.linear_acceleration.x,prev_msg.linear_acceleration.y,prev_msg.linear_acceleration.z);
// Calculate displacement norm
// double displacement_norm = std::sqrt(
// std::pow(curr_msg.linear_acceleration.x - prev_msg.linear_acceleration.x, 2) +
// std::pow(curr_msg.linear_acceleration.y - prev_msg.linear_acceleration.y, 2) +
// std::pow(curr_msg.linear_acceleration.z - prev_msg.linear_acceleration.z, 2)
// );
double displacement_norm=(cur_position-prev_position).norm();
// Calculate rotation norm (using quaternions)
double rotation_norm = std::sqrt(
std::pow(curr_msg.orientation.x - prev_msg.orientation.x, 2) +
std::pow(curr_msg.orientation.y - prev_msg.orientation.y, 2) +
std::pow(curr_msg.orientation.z - prev_msg.orientation.z, 2) +
std::pow(curr_msg.orientation.w - prev_msg.orientation.w, 2)
);
// Update global variables
total_seq_diff += seq_diff;
total_time_diff += time_diff;
total_displacement_norm += displacement_norm;
total_rotation_norm += rotation_norm;
// Criteria for detecting a frame drop
bool is_frame_dropped = (seq_diff > 1 || time_diff > 0.1); // Example thresholds
global_threshold=5.0/displacement_norm;
// Output current stats
if (is_frame_dropped) {
ROS_INFO("Frame drop detected!");
ROS_INFO("Seq diff: %d, Time diff: %f seconds", seq_diff, time_diff.toSec());
ROS_INFO("Displacement norm: %f, Rotation norm: %f", displacement_norm, rotation_norm);
ROS_INFO("Total Seq diff: %d, Total Time diff: %f seconds", total_seq_diff, total_time_diff.toSec());
ROS_INFO("Total Displacement norm: %f, Total Rotation norm: %f", total_displacement_norm, total_rotation_norm);
ROS_INFO("dynamic_threshold:%f,and total=:%f", global_threshold,displacement_norm*global_threshold);
}
return is_frame_dropped;
}
}