VINS-Mono源码分析7— pose_graph2
在上一篇博文中，大概分析了一下VINS-Mono回环检测和重定位的代码实现，这里主要分析
四自由度的位姿图优化。关于这部分的原理可以参考VINS-Mono论文第8部分的A、B和C以及崔神的文章《VINS论文推导及代码解析》中的7.4节。接下来开始进行代码分析，
PoseGraph::PoseGraph()
{
......
t_optimization = std::thread(&PoseGraph::optimize4DoF, this);
......
}
在pose_graph_node.cpp中，定义posegraph变量时，就开启了t_optimization线程，在这个线程中执行optimize4DoF函数，
void PoseGraph::optimize4DoF()
{
while(true)
{
int cur_index = -1;
int first_looped_index = -1;
m_optimize_buf.lock();
while(!optimize_buf.empty())
{
cur_index = optimize_buf.front();
first_looped_index = earliest_loop_index;
optimize_buf.pop();
}
m_optimize_buf.unlock();
if (cur_index != -1)
{
printf("optimize pose graph \n");
TicToc tmp_t;
m_keyframelist.lock();
KeyFrame* cur_kf = getKeyFrame(cur_index);
int max_length = cur_index + 1;
double t_array[max_length][3];
Quaterniond q_array[max_length];
double euler_array[max_length][3];
double sequence_array[max_length];
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(0.1);
ceres::LocalParameterization* angle_local_parameterization =
AngleLocalParameterization::Create();
list<KeyFrame*>::iterator it;
int i = 0;
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
if ((*it)->index < first_looped_index)
continue;
(*it)->local_index = i;
Quaterniond tmp_q;
Matrix3d tmp_r;
Vector3d tmp_t;
(*it)->getVioPose(tmp_t, tmp_r);
tmp_q = tmp_r;
t_array[i][0] = tmp_t(0);
t_array[i][1] = tmp_t(1);
t_array[i][2] = tmp_t(2);
q_array[i] = tmp_q;
Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());
euler_array[i][0] = euler_angle.x();
euler_array[i][1] = euler_angle.y();
euler_array[i][2] = euler_angle.z();
sequence_array[i] = (*it)->sequence;
problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
if ((*it)->index == first_looped_index || (*it)->sequence == 0)
{
problem.SetParameterBlockConstant(euler_array[i]);
problem.SetParameterBlockConstant(t_array[i]);
}
for (int j = 1; j < 5; j++)
{
if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])
{
Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());
Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);
relative_t = q_array[i-j].inverse() * relative_t;
double relative_yaw = euler_array[i][0] - euler_array[i-j][0];
ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
relative_yaw, euler_conncected.y(), euler_conncected.z());
problem.AddResidualBlock(cost_function, NULL, euler_array[i-j],
t_array[i-j],
euler_array[i],
t_array[i]);
}
}
if((*it)->has_loop)
{
assert((*it)->loop_index >= first_looped_index);
int connected_index = getKeyFrame((*it)->loop_index)->local_index;
Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());
Vector3d relative_t;
relative_t = (*it)->getLoopRelativeT();
double relative_yaw = (*it)->getLoopRelativeYaw();
ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
relative_yaw, euler_conncected.y(), euler_conncected.z());
problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index],
t_array[connected_index],
euler_array[i],
t_array[i]);
}
if ((*it)->index == cur_index)
break;
i++;
}
m_keyframelist.unlock();
ceres::Solve(options, &problem, &summary);
m_keyframelist.lock();
i = 0;
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
if ((*it)->index < first_looped_index)
continue;
Quaterniond tmp_q;
tmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));
Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);
Matrix3d tmp_r = tmp_q.toRotationMatrix();
(*it)-> updatePose(tmp_t, tmp_r);
if ((*it)->index == cur_index)
break;
i++;
}
Vector3d cur_t, vio_t;
Matrix3d cur_r, vio_r;
cur_kf->getPose(cur_t, cur_r);
cur_kf->getVioPose(vio_t, vio_r);
m_drift.lock();
yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();
r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));
t_drift = cur_t - r_drift * vio_t;
m_drift.unlock();
it++;
for (; it != keyframelist.end(); it++)
{
Vector3d P;
Matrix3d R;
(*it)->getVioPose(P, R);
P = r_drift * P + t_drift;
R = r_drift * R;
(*it)->updatePose(P, R);
}
m_keyframelist.unlock();
updatePath();
}
std::chrono::milliseconds dura(2000);
std::this_thread::sleep_for(dura);
}
}
下面是序列边残差计算的代码
struct FourDOFError
{
FourDOFError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i)
:t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){}
template <typename T>
bool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const
{
T t_w_ij[3];
t_w_ij[0] = tj[0] - ti[0];
t_w_ij[1] = tj[1] - ti[1];
t_w_ij[2] = tj[2] - ti[2];
T w_R_i[9];
YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);
T i_R_w[9];
RotationMatrixTranspose(w_R_i, i_R_w);
T t_i_ij[3];
RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);
residuals[0] = (t_i_ij[0] - T(t_x));
residuals[1] = (t_i_ij[1] - T(t_y));
residuals[2] = (t_i_ij[2] - T(t_z));
residuals[3] = NormalizeAngle(yaw_j[0] - yaw_i[0] - T(relative_yaw));
return true;
}
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
const double relative_yaw, const double pitch_i, const double roll_i)
{
return (new ceres::AutoDiffCostFunction<
FourDOFError, 4, 1, 3, 1, 3>(
new FourDOFError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));
}
double t_x, t_y, t_z;
double relative_yaw, pitch_i, roll_i;
};
下面是回环边残差计算的代码
struct FourDOFWeightError
{
FourDOFWeightError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i)
:t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){
weight = 1;
}
template <typename T>
bool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const
{
T t_w_ij[3];
t_w_ij[0] = tj[0] - ti[0];
t_w_ij[1] = tj[1] - ti[1];
t_w_ij[2] = tj[2] - ti[2];
T w_R_i[9];
YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);
T i_R_w[9];
RotationMatrixTranspose(w_R_i, i_R_w);
T t_i_ij[3];
RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);
residuals[0] = (t_i_ij[0] - T(t_x)) * T(weight);
residuals[1] = (t_i_ij[1] - T(t_y)) * T(weight);
residuals[2] = (t_i_ij[2] - T(t_z)) * T(weight);
residuals[3] = NormalizeAngle((yaw_j[0] - yaw_i[0] - T(relative_yaw))) * T(weight) / T(10.0);
std::cout<<"回环边"<<"residuals[3]"<<std::endl;
return true;
}
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
const double relative_yaw, const double pitch_i, const double roll_i)
{
return (new ceres::AutoDiffCostFunction<
FourDOFWeightError, 4, 1, 3, 1, 3>(
new FourDOFWeightError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));
}
double t_x, t_y, t_z;
double relative_yaw, pitch_i, roll_i;
double weight;
};
void PoseGraph::updatePath()
{
m_keyframelist.lock();
list<KeyFrame*>::iterator it;
for (int i = 1; i <= sequence_cnt; i++)
{
path[i].poses.clear();
}
base_path.poses.clear();
posegraph_visualization->reset();
if (SAVE_LOOP_PATH)
{
ofstream loop_path_file_tmp(VINS_RESULT_PATH, ios::out);
loop_path_file_tmp.close();
}
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
Vector3d P;
Matrix3d R;
(*it)->getPose(P, R);
Quaterniond Q;
Q = R;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time((*it)->time_stamp);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;
pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;
pose_stamped.pose.position.z = P.z();
pose_stamped.pose.orientation.x = Q.x();
pose_stamped.pose.orientation.y = Q.y();
pose_stamped.pose.orientation.z = Q.z();
pose_stamped.pose.orientation.w = Q.w();
if((*it)->sequence == 0)
{
base_path.poses.push_back(pose_stamped);
base_path.header = pose_stamped.header;
}
else
{
path[(*it)->sequence].poses.push_back(pose_stamped);
path[(*it)->sequence].header = pose_stamped.header;
}
if (SAVE_LOOP_PATH)
{
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(0);
loop_path_file << (*it)->time_stamp * 1e9 << ",";
loop_path_file.precision(5);
loop_path_file << P.x() << ","
<< P.y() << ","
<< P.z() << ","
<< Q.w() << ","
<< Q.x() << ","
<< Q.y() << ","
<< Q.z() << ","
<< endl;
loop_path_file.close();
}
if (SHOW_S_EDGE)
{
......
}
if (SHOW_L_EDGE)
{
if ((*it)->has_loop && (*it)->sequence == sequence_cnt)
{
KeyFrame* connected_KF = getKeyFrame((*it)->loop_index);
Vector3d connected_P;
Matrix3d connected_R;
connected_KF->getPose(connected_P, connected_R);
(*it)->getPose(P, R);
if((*it)->sequence > 0)
{
posegraph_visualization->add_loopedge(P, connected_P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0));
}
}
}
}
publish();
m_keyframelist.unlock();
}
void PoseGraph::publish()
{
for (int i = 1; i <= sequence_cnt; i++)
{
if (1 || i == base_sequence)
{
pub_pg_path.publish(path[i]);
pub_path[i].publish(path[i]);
posegraph_visualization->publish_by(pub_pose_graph, path[sequence_cnt].header);
}
}
base_path.header.frame_id = "world";
pub_base_path.publish(base_path);
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
