0. Overview
일단 내가 그려본 프로그램 순서도를 확인해보겠다.
간단히 이야기해보자면 다음과 같다.
(1) relocalization_callback()
relo_buf에 points_msg를 저장한다.
(2) restart_callback()
restart를 위한 초기화 작업을 한다.
(3) feature_callback()
feature 데이터를 받아와 feature_buf에 저장한다.
(4) imu_callback()
IMU 데이터를 지속적으로 받으며, 받은 데이터는 buffer에 저장하고, predict()를 통해 매순간 PVQ를 계산한다.
(5) predict()
PVQ를 계산한다. 수식은 다음과 같다.
(6) process()
Visual-Odometry Thread 로써의 역할을 한다.
(7) getMeasurements()
IMU 데이터의 타이밍과 카메라 프레임의 타이밍을 맞춘다.
(8) estimator.processIMU()
데이터를 처리하고 pre_integtation()을 위한 IntegrationBase.push_back() 함수를 실행한다.
(9) IntegrationBase.push_back()
데이터를 buffer에 저장하고 propagate()를 실행한다.
(10) IntegrationBase.propagate()
데이터를 처리하고 midPointIntegration()을 실행한다.
(11) IntegrationBase.midPointIntegration()
파라미터로 전달된 데이터들을 이용해 jacobian과 covariance를 구한다.
여기서 구해진 jacobian과 covariance는 차후에 쓰이게 된다.
(12) update()
계산된 내용을 update 한다.
1. main
먼저 main 함수를 확인해서 어떤 함수들이 동작하는지 확인하자.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
|
int main(int argc, char **argv)
{
ros::init(argc, argv, "vins_estimator");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
readParameters(n);
estimator.setParameter();
#ifdef EIGEN_DONT_PARALLELIZE
ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
ROS_WARN("waiting for image and imu...");
registerPub(n);
ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);
ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);
std::thread measurement_process{process};
ros::spin();
return 0;
}
|
|
ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);
ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);
std::thread measurement_process{process};
ros::spin();
(1) imu_callback : imu 센서값을 수신한다.
(2) feature_callback : feature 값을 수신한다.
(3) restart_callback : restart 신호를 수신한다.
(4) relocalization_callback : relocalization 신호 값을 수신한다.
(5) process : 수신한 센서값들을 이용하여 process를 진행한다.
2. imu_callback
imu 센서 값을 수신하면서 어떤 과정을 진행하는지 살펴보자.
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
|
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
if (imu_msg->header.stamp.toSec() <= last_imu_t)
{
ROS_WARN("imu message in disorder!");
return;
}
last_imu_t = imu_msg->header.stamp.toSec();
m_buf.lock();
imu_buf.push(imu_msg);
m_buf.unlock();
con.notify_one();
last_imu_t = imu_msg->header.stamp.toSec();
{
std::lock_guard<std::mutex> lg(m_state);
predict(imu_msg);
std_msgs::Header header = imu_msg->header;
header.frame_id = "world";
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
}
}
| cs |
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
if (imu_msg->header.stamp.toSec() <= last_imu_t)
{
ROS_WARN("imu message in disorder!");
return;
}
last_imu_t = imu_msg->header.stamp.toSec();
먼저 IMU 센서의 시간 체크를 통해 잘못된 데이터가 들어오진 않는지 확인한다.
m_buf.lock();
imu_buf.push(imu_msg);
m_buf.unlock();
con.notify_one();
뮤텍스(std::mutex m_buf;)를 이용하는 부분이다.
뮤텍스는 여러 쓰레드에서 공유하는 변수가 있을 때, lock()을 걸어서 해당 부분에서만 쓰레드를 진행하게 된다. 그리고 unlock()과 notify_one()을 통해 쓰레드의 동작을 허용한다.
자세한 내용은 다음 링크를 참조한다.
https://modoocode.com/270
last_imu_t = imu_msg->header.stamp.toSec();
다시 시간을 조정해준다.
{
std::lock_guard<std::mutex> lg(m_state);
predict(imu_msg);
std_msgs::Header header = imu_msg->header;
header.frame_id = "world";
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
}
다시 뮤텍스 작업을 진행한다. 이 때에는 lock_guard를 이용하게 된다.
predict 함수는 #09 의 predict() 부분을 참고하면 알 수 있겠지만,
high frequency로 받아지는 imu 센서 데이터에 대해 아래의 수식을 이용해 순간의 PVQ를 계산하고 계산된 Odometry를 Publish 하는 내용이다.
pubLatestOdometry는 vins_estimator/src/utility/visualization.cpp 에서 확인할 수 있으며,
단순히 Publish를 위한 내용이다.
3. feature_callback()
feature 부분에서 측정된 특징점 포인트클라우드 데이터들이 feature_buf에 저장되게 된다.
1
2
3
4
5
6
7
8
9
10
11
12
13
|
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
if (!init_feature)
{
//skip the first detected feature, which doesn't contain optical flow speed
init_feature = 1;
return;
}
m_buf.lock();
feature_buf.push(feature_msg);
m_buf.unlock();
con.notify_one();
}
| cs |
4. restart_callback()
restart 메세지가 수신되면 데이터들을 초기화시켜주게 된다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
|
void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
{
if (restart_msg->data == true)
{
ROS_WARN("restart the estimator!");
m_buf.lock();
while(!feature_buf.empty())
feature_buf.pop();
while(!imu_buf.empty())
imu_buf.pop();
m_buf.unlock();
m_estimator.lock();
estimator.clearState();
estimator.setParameter();
m_estimator.unlock();
current_time = -1;
last_imu_t = 0;
}
return;
}
| cs |
5. relocalization_callback()
사용되는 부분을 자세히 확인 해봐야 겠지만, relocalization 한 내용을 relo_buf에 저장하게 된다.
1
2
3
4
5
6
7
|
void relocalization_callback(const sensor_msgs::PointCloudConstPtr &points_msg)
{
//printf("relocalization callback! \n");
m_buf.lock();
relo_buf.push(points_msg);
m_buf.unlock();
}
| cs |
6. predict()
predict 함수는 high frequency로 전달되는 IMU 데이터를 처리하기 위한 부분으로써, vins_estimator/estimator_node.cpp의 imu_callback 함수에서 사용되는 것을 확인할 수 있다.신경쓸 점은, 논문에서는 Euler integration을 사용했기 때문에 필요한 수식이 다음과 같았지만,
실제 코드에서는 mid-point integration이 사용되었기 때문에 다음의 수식을 이용하게 된다.
predict 함수의 코드는 다음에서 확인할 수 있다. (vins_estimator/estimator_node.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
|
void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
double t = imu_msg->header.stamp.toSec();
if (init_imu)
{
latest_time = t;
init_imu = 0;
return;
}
double dt = t - latest_time;
latest_time = t;
double dx = imu_msg->linear_acceleration.x;
double dy = imu_msg->linear_acceleration.y;
double dz = imu_msg->linear_acceleration.z;
Eigen::Vector3d linear_acceleration{dx, dy, dz};
double rx = imu_msg->angular_velocity.x;
double ry = imu_msg->angular_velocity.y;
double rz = imu_msg->angular_velocity.z;
Eigen::Vector3d angular_velocity{rx, ry, rz};
Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;
Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
tmp_V = tmp_V + dt * un_acc;
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
| cs |
void predict(const sensor_msgs::ImuConstPtr &imu_msg)
해당 함수는 파라미터로 imu 메세지를 받는다.
double t = imu_msg->header.stamp.toSec();
if (init_imu)
{
latest_time = t;
init_imu = 0;
return;
}
double dt = t - latest_time;
latest_time = t;
그리고 메세지의 시간을 이용해 이전의 메세지로부터 얼마나 시간이 걸렸는지 구한다.
double dx = imu_msg->linear_acceleration.x;
double dy = imu_msg->linear_acceleration.y;
double dz = imu_msg->linear_acceleration.z;
Eigen::Vector3d linear_acceleration{dx, dy, dz};
double rx = imu_msg->angular_velocity.x;
double ry = imu_msg->angular_velocity.y;
double rz = imu_msg->angular_velocity.z;
Eigen::Vector3d angular_velocity{rx, ry, rz};
그리고 메세지의 각 값들을 Eigen::Vector3d 형태로 만들어준다.
Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;
이 부분은 아래 수식에서 위의 가속도 평균을 구할 때, 우항의 괄호 안 내용 중 왼쪽의 이전 가속도 값에 대한 수식이 된다. 아마 estimator.g는 중력가속도인 것 같다.
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
해당 부분은 위 수식에서 회전 가속도의 평균을 구하는 내용이다.
tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);
tmp_Q를 업데이트 한다.
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;
위 수식의 가속도 평균에서 우항의 괄호 안의 오른쪽에 존재하는 현재 수신한 가속도 값이다.
Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
이를 이용해 최종적으로 위 수식의 가속도 평균 계산이 된다.
tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
tmp_V = tmp_V + dt * un_acc;
아래의 수식과 같이 계산을 진행한다.
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
현재의 센서 데이터를 이전의 센서 데이터 변수에 넣어준다.
이렇게 구해진, tmp_P, tmp_V, tmp_Q 가 알파, 베타, 감마 가 된다.
7. process()
process() 함수는 Visual-Inertial Odometry 에 대한 내용을 총괄하는 thread로써,
IMU pre-integration, Feature, Relocalization, Restart 에 대한 내용 모두 포함 되게 된다.
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
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
|
// thread: visual-inertial odometry
void process()
{
while (true)
{
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
std::unique_lock<std::mutex> lk(m_buf);
con.wait(lk, [&]
{
return (measurements = getMeasurements()).size() != 0;
});
lk.unlock();
m_estimator.lock();
for (auto &measurement : measurements)
{
auto img_msg = measurement.second;
double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
for (auto &imu_msg : measurement.first)
{
double t = imu_msg->header.stamp.toSec();
double img_t = img_msg->header.stamp.toSec() + estimator.td;
if (t <= img_t)
{
if (current_time < 0)
current_time = t;
double dt = t - current_time;
ROS_ASSERT(dt >= 0);
current_time = t;
dx = imu_msg->linear_acceleration.x;
dy = imu_msg->linear_acceleration.y;
dz = imu_msg->linear_acceleration.z;
rx = imu_msg->angular_velocity.x;
ry = imu_msg->angular_velocity.y;
rz = imu_msg->angular_velocity.z;
estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
//printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);
}
else
{
double dt_1 = img_t - current_time;
double dt_2 = t - img_t;
current_time = img_t;
ROS_ASSERT(dt_1 >= 0);
ROS_ASSERT(dt_2 >= 0);
ROS_ASSERT(dt_1 + dt_2 > 0);
double w1 = dt_2 / (dt_1 + dt_2);
double w2 = dt_1 / (dt_1 + dt_2);
dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
//printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);
}
}
// set relocalization frame
sensor_msgs::PointCloudConstPtr relo_msg = NULL;
while (!relo_buf.empty())
{
relo_msg = relo_buf.front();
relo_buf.pop();
}
if (relo_msg != NULL)
{
vector<Vector3d> match_points;
double frame_stamp = relo_msg->header.stamp.toSec();
for (unsigned int i = 0; i < relo_msg->points.size(); i++)
{
Vector3d u_v_id;
u_v_id.x() = relo_msg->points[i].x;
u_v_id.y() = relo_msg->points[i].y;
u_v_id.z() = relo_msg->points[i].z;
match_points.push_back(u_v_id);
}
Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);
Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);
Matrix3d relo_r = relo_q.toRotationMatrix();
int frame_index;
frame_index = relo_msg->channels[0].values[7];
estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
}
ROS_DEBUG("processing vision data with stamp %f \n", img_msg->header.stamp.toSec());
TicToc t_s;
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{
int v = img_msg->channels[0].values[i] + 0.5;
int feature_id = v / NUM_OF_CAM;
int camera_id = v % NUM_OF_CAM;
double x = img_msg->points[i].x;
double y = img_msg->points[i].y;
double z = img_msg->points[i].z;
double p_u = img_msg->channels[1].values[i];
double p_v = img_msg->channels[2].values[i];
double velocity_x = img_msg->channels[3].values[i];
double velocity_y = img_msg->channels[4].values[i];
ROS_ASSERT(z == 1);
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
image[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
estimator.processImage(image, img_msg->header);
double whole_t = t_s.toc();
printStatistics(estimator, whole_t);
std_msgs::Header header = img_msg->header;
header.frame_id = "world";
pubOdometry(estimator, header);
pubKeyPoses(estimator, header);
pubCameraPose(estimator, header);
pubPointCloud(estimator, header);
pubTF(estimator, header);
pubKeyframe(estimator);
if (relo_msg != NULL)
pubRelocalization(estimator);
//ROS_ERROR("end: %f, at %f", img_msg->header.stamp.toSec(), ros::Time::now().toSec());
}
m_estimator.unlock();
m_buf.lock();
m_state.lock();
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
update();
m_state.unlock();
m_buf.unlock();
}
}
| cs |
process 함수는 보다시피 visual-inertial odometry를 진행하는 thread 이다.
while (true)
{
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
일단 measurements를 선언하는데, getMeasurements() 함수에서 리턴되는 변수를 받기 위함이라고 보면 된다.
std::unique_lock<std::mutex> lk(m_buf);
con.wait(lk, [&]
{
return (measurements = getMeasurements()).size() != 0;
});
lk.unlock();
이 부분은 뮤텍스와 조건변수를 이용하는 부분인데, 뮤텍스와 조건변수에 대한 개념이 잡혀있지 않으면 이해가 어려울 수 있다. 일단 참고하기 좋은 링크를 남기겠다.
( 링크 참조 : https://modoocode.com/270 )
참고로 con 조건변수는 estimator_node.cpp에 전역변수로 선언되어 있다.
std::condition_variable con;
뮤텍스의 쓰임새는 이렇다. 여러개의 쓰레드가 존재할 때, 공유하는 변수가 있을 수 있다. 그리고 각각 쓰레드에서 해당 변수를 동시다발적으로 변경하려 하면, 당연히 제대로 된 값이 나오기 힘들다. 그래서 뮤텍스를 사용하여 나머지 쓰레드들의 활동을 멈추고 하나의 쓰레드만이 작동하도록 만들 수 있다.
조건변수의 쓰임은 이렇다. 뮤텍스는 특정한 조건이 생겼을 때 발동되어야 효율이 높아진다. 이러한 조건을 주는 방법은 여러가지가 있겠지만, 여기서는 wait 함수를 사용하였다.
즉, wait함수가 수행되고 measurements에 데이터가 있다면 뮤텍스(ik)는 lock상태(하나의 쓰레드만 작동하는 상태)가 된다. 여기서는 getMeasurements() 함수를 작동시켜 리턴된 measurements가 존재한다면 lock 상태가 되도록 만들었으며, 해당 부분이 작동한 후에는 반드시 unlock()을 해주어야 다른 쓰레드들이 원활히 동작할 수 있게 된다.
참고로 measurements의 값이 존재하지 않는다면, 쓰레드는 con.wait에서 계속 기다리는 상태가 된다.
이후에는 m_estimator.lock()을 작동시키게 된다. 선언은 전역변수에 존재한다.
std::mutex m_estimator;
이제 해당 쓰레드 과정만이 수행되는 것이다.
참고로 getMeasurements() 함수는 IMU 센서의 측정 데이터를 이미지 프레임의 타이밍에 맞게 맞추는 역할을 한다.
for (auto &measurement : measurements)
{
measurements를 가져와 반복문을 돌린다. 약간 의아한 것은 난 당연히 getMeasurements()를 통해 리턴된 measurements는 하나의 벡터라고 생각했는데, 반복문을 넣은 것을 보니 아닐 수도 있나보다.
auto img_msg = measurement.second;
이미지 정보를 가져온다.
double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
for (auto &imu_msg : measurement.first)
{
double t = imu_msg->header.stamp.toSec();
double img_t = img_msg->header.stamp.toSec() + estimator.td;
if (t <= img_t)
{
if (current_time < 0)
current_time = t;
double dt = t - current_time;
ROS_ASSERT(dt >= 0);
current_time = t;
dx = imu_msg->linear_acceleration.x;
dy = imu_msg->linear_acceleration.y;
dz = imu_msg->linear_acceleration.z;
rx = imu_msg->angular_velocity.x;
ry = imu_msg->angular_velocity.y;
rz = imu_msg->angular_velocity.z;
estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
//printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);
}
만약 IMU 센서 데이터의 시간이 feature + td 의 시간보다 작다면 IMU 센서데이터를 그대로 대입한 후 processIMU를 진행한다.
else
{
double dt_1 = img_t - current_time;
double dt_2 = t - img_t;
current_time = img_t;
ROS_ASSERT(dt_1 >= 0);
ROS_ASSERT(dt_2 >= 0);
ROS_ASSERT(dt_1 + dt_2 > 0);
double w1 = dt_2 / (dt_1 + dt_2);
double w2 = dt_1 / (dt_1 + dt_2);
dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
//printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);
}
만약 IMU 센서 데이터의 시간이 feature + td 의 시간보다 늦다면
double dt_1 = img_t - current_time;
를 통해 이미지의 시간에서 이 전의 IMU 센서의 시간을 빼준다. 그리고
double dt_2 = t - img_t;
를 통해 현재 imu 센서의 시간에서 이미지 센서의 시간을 빼준다.
그림을 그려보자면 다음과 같다.
current_time = img_t;
그리고 이미지 센서의 시간이 current_time이 된다.
double w1 = dt_2 / (dt_1 + dt_2);
double w2 = dt_1 / (dt_1 + dt_2);
그리고 위 부분을 이용해 가중치를 구한다.
이렇게 구한 가중치를 이용해 가속도, 회전속도 값들을 수정해주게 된다.
dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
보면 알다시피, 이 전의 값이 더 작은 가중치와 곱해지고, 현재의 값이 더 큰 가중치와 곱해지는 것을 알 수 있다.
// set relocalization frame
sensor_msgs::PointCloudConstPtr relo_msg = NULL;
while (!relo_buf.empty())
{
relo_msg = relo_buf.front();
relo_buf.pop();
}
relo_buf에 있는 내용을 relo_msg에 모두 담아주게 된다.
if (relo_msg != NULL)
{
vector<Vector3d> match_points;
double frame_stamp = relo_msg->header.stamp.toSec();
for (unsigned int i = 0; i < relo_msg->points.size(); i++)
{
Vector3d u_v_id;
u_v_id.x() = relo_msg->points[i].x;
u_v_id.y() = relo_msg->points[i].y;
u_v_id.z() = relo_msg->points[i].z;
match_points.push_back(u_v_id);
}
Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);
Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);
Matrix3d relo_r = relo_q.toRotationMatrix();
int frame_index;
frame_index = relo_msg->channels[0].values[7];
estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
}
u_v_id와 relo_t, relo_q, relo_r 을 구해주게 된다.
u_v_id는 확실히 points인데, channels.values 의 의미는 확실히 모르겠다.
roll, pitch, yaw, quaternion 인 것 같긴 한데, 확실히는 모르겠다.
ROS_DEBUG("processing vision data with stamp %f \n", img_msg->header.stamp.toSec());
TicToc t_s;
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{
int v = img_msg->channels[0].values[i] + 0.5;
int feature_id = v / NUM_OF_CAM;
int camera_id = v % NUM_OF_CAM;
double x = img_msg->points[i].x;
double y = img_msg->points[i].y;
double z = img_msg->points[i].z;
double p_u = img_msg->channels[1].values[i];
double p_v = img_msg->channels[2].values[i];
double velocity_x = img_msg->channels[3].values[i];
double velocity_y = img_msg->channels[4].values[i];
ROS_ASSERT(z == 1);
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
image[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
estimator.processImage(image, img_msg->header);
img_msg는 pointCloud 데이터이므로, channels에 따라 여러 값을 지닌다.
역시나 channel과 value에 대해서는 자세히 모르겠다.
하지만 일단 중요한 부분은 image[feature_id] 에 id와 xyz_uv_velocity(7x1 Matrix) 데이터들을 저장한다는 것이다. 그리고 image를 이용하여 processImage 함수를 실행한다.
double whole_t = t_s.toc();
printStatistics(estimator, whole_t);
std_msgs::Header header = img_msg->header;
header.frame_id = "world";
pubOdometry(estimator, header);
pubKeyPoses(estimator, header);
pubCameraPose(estimator, header);
pubPointCloud(estimator, header);
pubTF(estimator, header);
pubKeyframe(estimator);
if (relo_msg != NULL)
pubRelocalization(estimator);
//ROS_ERROR("end: %f, at %f", img_msg->header.stamp.toSec(), ros::Time::now().toSec());
}
위 과정을 통해 얻어진 데이터들을 publish 한다.
m_estimator.unlock();
m_buf.lock();
m_state.lock();
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
update();
m_state.unlock();
m_buf.unlock();
}
update()를 해준다.
8. getMeasurements()
getMeasurements() 함수는 IMU 센서의 측정 데이터를 이미지 프레임의 타이밍에 맞게 맞추는 역할을 하며, pre-integration을 위해서는 이미지 프레임 bk와 bk+1 사이의 데이터들이 필요하기 때문에, pre-integration의 전초역할을 한다.함수를 살펴보면 다음과 같다. (vins_estimator/estimator_node.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
|
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>
getMeasurements()
{
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
while (true)
{
if (imu_buf.empty() || feature_buf.empty())
return measurements;
if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
{
//ROS_WARN("wait for imu, only should happen at the beginning");
sum_of_wait++;
return measurements;
}
if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))
{
ROS_WARN("throw img, only should happen at the beginning");
feature_buf.pop();
continue;
}
sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();
feature_buf.pop();
std::vector<sensor_msgs::ImuConstPtr> IMUs;
while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
{
IMUs.emplace_back(imu_buf.front());
imu_buf.pop();
}
IMUs.emplace_back(imu_buf.front());
if (IMUs.empty())
ROS_WARN("no imu between two image");
measurements.emplace_back(IMUs, img_msg);
}
return measurements;
}
| cs |
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>
getMeasurements()
먼저 함수는 IMU 데이터와 특징점 검출을 통해 얻은 Vision 데이터를 파라미터로 받는다.
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
IMU 데이터와 Vision 데이터를 담을 벡터 변수 measurements를 만든다.
while (true)
{
if (imu_buf.empty() || feature_buf.empty())
return measurements;
일단 각 buffer가 비어있는지 확인한다. 하나라도 비어있다면 아무 정보도 담기지 않은 measurements를 return한다.
if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
{
//ROS_WARN("wait for imu, only should happen at the beginning");
sum_of_wait++;
return measurements;
}
imu_buf.back()은 imu의 마지막 데이터를 의미하고, feature_buf.front()는 feature의 첫 데이터를 의미한다. estimator.td 는 두 feature frame 간의 시간인듯 하다.
즉, feature + td 보다 imu의 마지막 데이터를 받은 시간이 빠르다면, 아직 충분한 imu 데이터를 수신하지 못했다는 내용이므로, sum_of_wait을 증가시키고 아무 값 없는 measurements를 return한다.
if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))
{
ROS_WARN("throw img, only should happen at the beginning");
feature_buf.pop();
continue;
}
만약 imu의 첫 데이터가 feature + td 보다 늦게 받아졌다면 feature_buf에서 가장 오래된 데이터를 하나 없앰으로써 feature의 시간을 증가시킨다.
sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();
feature_buf.pop();
img_msg에 feature_buf.front()를 담아준 뒤, feature_buf 에서 front는 빼준다.
std::vector<sensor_msgs::ImuConstPtr> IMUs;
while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
{
IMUs.emplace_back(imu_buf.front());
imu_buf.pop();
}
IMUs.emplace_back(imu_buf.front());
if (IMUs.empty())
ROS_WARN("no imu between two image");
measurements.emplace_back(IMUs, img_msg);
}
return measurements;
이제 IMUs 변수에 값들을 넣어준 뒤, 최종적으로 measurements에 IMUs와 img_msg를 넣어준다.
그리고 measurements를 반환한다.
9. processIMU()
8번에서 얻어낸 measurtments로 유효한 데이터가 존재하는지 판단한 후,IMU의 데이터를 처리하고 pre_integration을 위한 준비를 하는 과정이다.
linear_acceleration과 angular_velocity 데이터를 받아
Rs, Ps, Vs를 얻어낸다.
processIMU 함수는 vins_estimator/src/estimator.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
|
void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
if (!first_imu)
{
first_imu = true;
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
if (!pre_integrations[frame_count])
{
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
}
if (frame_count != 0)
{
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
//if(solver_flag != NON_LINEAR)
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
dt_buf[frame_count].push_back(dt);
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;
}
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
| cs |
if (!first_imu)
{
first_imu = true;
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
초기의 imu 값을 받는다.
if (!pre_integrations[frame_count])
{
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
}
만약에 IntergrationBase 객체가 생성되지 않았다면, 만들어준다.
IntegrationBase 클래스는 integration_base.h에 존재하며, IMU 센서와 관련된 다양한 연산을 수행한다.
if (frame_count != 0)
{
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
dt_buf[frame_count].push_back(dt);
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);
frame_count가 0이 아니라면 push_back 함수를 통해 각 값들을 넣어준다.
참고로 dt는 두 프레임간의 시간이다.
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;
위 코드는 다음 수식을 의미한다.
Ps, Vs, Rs 가 위 수식의 알파, 베타, 감마가 된다.
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
현재 값을 이전 값으로 만든다.
여기서 다양한 데이터 형태를 볼 수 있는데, 정리하자면 다음과 같다.
Rs, Ps, Vs는 IMU pre-integration으로 얻어진 IMU(world)의 P, V, Q 이다.
여기서 나오는 값들은 bias correction이 되지 않은 상태이다.
dt_buf, linear_acceleration_buf, angular_velocity_buf 는 IMU measurements와 frame의 buffer이다.
pre_integrations[frame_count]는 frame_count 프레임에서 IMU pre-integration 과 관련된 값들 (F matrix, Q matrix, J matrix, etc)을 갖고 있다.
10. push_back()
dt_buf, acc_buf, gyr_buf에 IMU 센서 데이터를 저장하고, 저장된 데이터들을 이용해 propagate()를 진행한다.
1
2
3
4
5
6
7
|
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
dt_buf.push_back(dt);
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
propagate(dt, acc, gyr);
}
| cs |
11. propagate()
propagate은 bk와 bk_1 사이에서 time i 에서 time i+1 단위로 PVQ propagation과 error transmission 목적으로 사용된다.
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
|
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
dt = _dt;
acc_1 = _acc_1;
gyr_1 = _gyr_1;
Vector3d result_delta_p;
Quaterniond result_delta_q;
Vector3d result_delta_v;
Vector3d result_linearized_ba;
Vector3d result_linearized_bg;
midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
linearized_ba, linearized_bg,
result_delta_p, result_delta_q, result_delta_v,
result_linearized_ba, result_linearized_bg, 1);
//checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,
// linearized_ba, linearized_bg);
delta_p = result_delta_p;
delta_q = result_delta_q;
delta_v = result_delta_v;
linearized_ba = result_linearized_ba;
linearized_bg = result_linearized_bg;
delta_q.normalize();
sum_dt += dt;
acc_0 = acc_1;
gyr_0 = gyr_1;
}
| cs |
시간 dt와 가속도 acc, 회전 가속도 gyr를 파라미터로 받아
midPointIntegration() 함수를 통하여 PVQ와 bias를 갱신시킨 후,
갱신된 변수들을 과거 변수로 만드는 과정을 수행한다.
12. midPointIntegration()
midPointIntegration은 bk와 bk+1 사이에서의 PVQ와 bias를 구하기 위한 함수로써,vins_estimator/src/factor/integration_base.h 에서 찾아볼 수 있다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
|
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
{
//ROS_INFO("midpoint integration");
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
| cs |
내용은 아래 수식이다.
Error Transfer Matrix
error transfer matrix를 구하는 이유는 두가지가 있다.
첫 번째로는 알파, 베타, 감마를 계산할 때, 다른 bias 값을 제공하기 위해서이고,
두 번째로는 back-end optimization part에서 information matrix를 제공하기 위해서이다.
우리는 기본적으로 다음 단계의 에러를 계산할 때, 이전 단계의 에러를 이용한다.
그리고 Gaussian distribution 모델을 이용해 노이즈를 표현하므로,
식을 다음과 같이 나타낼 수 있다.
dzk+1과 dzk는 다음 단계의 에러와 이전 단계의 에러가 되고, 이를 matrix 형태로 나타내면 다음과 같다.
Jacobian's iterative formula는 다음과 같다.
이 형태는 VINS에 많이 나타나는데, 하나는 여기에 나타나있고, 나머지 세 개는 back-end optimization에서 나타난다.
covatiance iteration formula는 다음과 같다.
이 covariance matrix 는 back-end optimization의 IMU part에서 information matrix로 쓰인다.
noise를 나타내는 diagonal covariance matrix Q 는 다음과 같다.
해당 부분은 위 코드의 뒷 내용으로, Error Transfer Matrix 를 구하는 과정이다.
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
|
if(update_jacobian)
{
Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
Vector3d a_0_x = _acc_0 - linearized_ba;
Vector3d a_1_x = _acc_1 - linearized_ba;
Matrix3d R_w_x, R_a_0_x, R_a_1_x;
R_w_x<<0, -w_x(2), w_x(1),
w_x(2), 0, -w_x(0),
-w_x(1), w_x(0), 0;
R_a_0_x<<0, -a_0_x(2), a_0_x(1),
a_0_x(2), 0, -a_0_x(0),
-a_0_x(1), a_0_x(0), 0;
R_a_1_x<<0, -a_1_x(2), a_1_x(1),
a_1_x(2), 0, -a_1_x(0),
-a_1_x(1), a_1_x(0), 0;
MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +
-0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +
-0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
F.block<3, 3>(6, 6) = Matrix3d::Identity();
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
F.block<3, 3>(9, 9) = Matrix3d::Identity();
F.block<3, 3>(12, 12) = Matrix3d::Identity();
//cout<<"A"<<endl<<A<<endl;
MatrixXd V = MatrixXd::Zero(15,18);
V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt;
V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt;
V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;
//step_jacobian = F;
//step_V = V;
jacobian = F * jacobian;
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
}
| cs |
Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
Vector3d a_0_x = _acc_0 - linearized_ba;
Vector3d a_1_x = _acc_1 - linearized_ba;
Matrix3d R_w_x, R_a_0_x, R_a_1_x;
R_w_x<<0, -w_x(2), w_x(1),
w_x(2), 0, -w_x(0),
-w_x(1), w_x(0), 0;
R_a_0_x<<0, -a_0_x(2), a_0_x(1),
a_0_x(2), 0, -a_0_x(0),
-a_0_x(1), a_0_x(0), 0;
R_a_1_x<<0, -a_1_x(2), a_1_x(1),
a_1_x(2), 0, -a_1_x(0),
-a_1_x(1), a_1_x(0), 0;
위 내용은 아래 수식에서 각 요소를 정의하기 위한 값들을 만드는 과정이다.
MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +
-0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +
-0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
F.block<3, 3>(6, 6) = Matrix3d::Identity();
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
F.block<3, 3>(9, 9) = Matrix3d::Identity();
F.block<3, 3>(12, 12) = Matrix3d::Identity();
위 내용은 아래 수식의 우항의 F matrix를 나타내며, 각 요소는 3x3 크기이다.
MatrixXd V = MatrixXd::Zero(15,18);
V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt;
V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt;
V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;
위 내용은 아래 수식의 오른쪽 매트릭스 V를 의미하며, V 행렬안의 각 요소는 3x3 크기이다.
jacobian = F * jacobian;
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
위 내용은 아래 수식을 나타낸다. 차후에 사용하기 위해 jacobian과 covariance를 업데이트 시킨다.
여기에서의 jacobian과 covariance는 차후에 사용되게 된다.
13. setReloFrame()
해당 부분은 pose_graph 노드에서 relocation 신호를 받으면 진행하는 내용이므로,지금은 넘어가도록 하자.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
|
void Estimator::setReloFrame(double _frame_stamp, int _frame_index, vector<Vector3d> &_match_points, Vector3d _relo_t, Matrix3d _relo_r)
{
relo_frame_stamp = _frame_stamp;
relo_frame_index = _frame_index;
match_points.clear();
match_points = _match_points;
prev_relo_t = _relo_t;
prev_relo_r = _relo_r;
for(int i = 0; i < WINDOW_SIZE; i++)
{
if(relo_frame_stamp == Headers[i].stamp.toSec())
{
relo_frame_local_index = i;
relocalization_info = 1;
for (int j = 0; j < SIZE_POSE; j++)
relo_Pose[j] = para_Pose[i][j];
}
}
}
| cs |
14. processImage()
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
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
|
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)
{
ROS_DEBUG("new image coming ------------------------------------------");
ROS_DEBUG("Adding feature points %lu", image.size());
if (f_manager.addFeatureCheckParallax(frame_count, image, td))
marginalization_flag = MARGIN_OLD;
else
marginalization_flag = MARGIN_SECOND_NEW;
ROS_DEBUG("this frame is--------------------%s", marginalization_flag ? "reject" : "accept");
ROS_DEBUG("%s", marginalization_flag ? "Non-keyframe" : "Keyframe");
ROS_DEBUG("Solving %d", frame_count);
ROS_DEBUG("number of feature: %d", f_manager.getFeatureCount());
Headers[frame_count] = header;
ImageFrame imageframe(image, header.stamp.toSec());
imageframe.pre_integration = tmp_pre_integration;
all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));
tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
if(ESTIMATE_EXTRINSIC == 2)
{
ROS_INFO("calibrating extrinsic param, rotation movement is needed");
if (frame_count != 0)
{
vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
Matrix3d calib_ric;
if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
{
ROS_WARN("initial extrinsic rotation calib success");
ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
ric[0] = calib_ric;
RIC[0] = calib_ric;
ESTIMATE_EXTRINSIC = 1;
}
}
}
if (solver_flag == INITIAL)
{
if (frame_count == WINDOW_SIZE)
{
bool result = false;
if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
{
result = initialStructure();
initial_timestamp = header.stamp.toSec();
}
if(result)
{
solver_flag = NON_LINEAR;
solveOdometry();
slideWindow();
f_manager.removeFailures();
ROS_INFO("Initialization finish!");
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
else
slideWindow();
}
else
frame_count++;
}
else
{
TicToc t_solve;
solveOdometry();
ROS_DEBUG("solver costs: %fms", t_solve.toc());
if (failureDetection())
{
ROS_WARN("failure detection!");
failure_occur = 1;
clearState();
setParameter();
ROS_WARN("system reboot!");
return;
}
TicToc t_margin;
slideWindow();
f_manager.removeFailures();
ROS_DEBUG("marginalization costs: %fms", t_margin.toc());
// prepare output of VINS
key_poses.clear();
for (int i = 0; i <= WINDOW_SIZE; i++)
key_poses.push_back(Ps[i]);
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
}
| cs |
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)
{
processImage()는 process()에서 호출되며, 다음과 같은 파라미터를 받게 된다.
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
image[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
estimator.processImage(image, img_msg->header);
ROS_DEBUG("new image coming ------------------------------------------");
ROS_DEBUG("Adding feature points %lu", image.size());
if (f_manager.addFeatureCheckParallax(frame_count, image, td))
marginalization_flag = MARGIN_OLD;
else
marginalization_flag = MARGIN_SECOND_NEW;
15. addFeatureCheckParallax()
addFeatureCheckParallax() 에서는 특징점간의 시차를 확인하게 된다.
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
|
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
ROS_DEBUG("input feature: %d", (int)image.size());
ROS_DEBUG("num of feature: %d", getFeatureCount());
double parallax_sum = 0;
int parallax_num = 0;
last_track_num = 0;
for (auto &id_pts : image)
{
FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
int feature_id = id_pts.first;
auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
{
return it.feature_id == feature_id;
});
if (it == feature.end())
{
feature.push_back(FeaturePerId(feature_id, frame_count));
feature.back().feature_per_frame.push_back(f_per_fra);
}
else if (it->feature_id == feature_id)
{
it->feature_per_frame.push_back(f_per_fra);
last_track_num++;
}
}
if (frame_count < 2 || last_track_num < 20)
return true;
for (auto &it_per_id : feature)
{
if (it_per_id.start_frame <= frame_count - 2 &&
it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
{
parallax_sum += compensatedParallax2(it_per_id, frame_count);
parallax_num++;
}
}
if (parallax_num == 0)
{
return true;
}
else
{
ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
return parallax_sum / parallax_num >= MIN_PARALLAX;
}
}
| cs |
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
ROS_DEBUG("input feature: %d", (int)image.size());
ROS_DEBUG("num of feature: %d", getFeatureCount());
double parallax_sum = 0;
int parallax_num = 0;
last_track_num = 0;
16. getFeatureCount()
getFeatureCount() 에서는 각 프레임당 특징점이 검출된 횟수를 세는데,
이 때, 프레임에서 검출된 특징점은 2개 이상이어야 하며, WINDOW_SIZE-2 보다 작은 숫자가 start_frame이 되어야 한다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
|
int FeatureManager::getFeatureCount()
{
int cnt = 0;
for (auto &it : feature)
{
it.used_num = it.feature_per_frame.size();
if (it.used_num >= 2 && it.start_frame < WINDOW_SIZE - 2)
{
cnt++;
}
}
return cnt;
}
| cs |
end. update()
최종적으로 얻어진 내용들을 update한다.그리고 predict 함수를 실행해준다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
|
void update()
{
TicToc t_predict;
latest_time = current_time;
tmp_P = estimator.Ps[WINDOW_SIZE];
tmp_Q = estimator.Rs[WINDOW_SIZE];
tmp_V = estimator.Vs[WINDOW_SIZE];
tmp_Ba = estimator.Bas[WINDOW_SIZE];
tmp_Bg = estimator.Bgs[WINDOW_SIZE];
acc_0 = estimator.acc_0;
gyr_0 = estimator.gyr_0;
queue<sensor_msgs::ImuConstPtr> tmp_imu_buf = imu_buf;
for (sensor_msgs::ImuConstPtr tmp_imu_msg; !tmp_imu_buf.empty(); tmp_imu_buf.pop())
predict(tmp_imu_buf.front());
}
| cs |
댓글 없음:
댓글 쓰기