代码阅读
- 0. line_geometry
- 1. general_line_factor.h
- 2. general_line_parameterization.h
- 3. plucker_line_parameterization
- 4. plucker_projection_factor.h
- 5. plucker_projection_factor.cpp
- 6. pose_local_parameterization
- 7. line_manager（比较重要，论文中创新部分）
- 8. line_optimization.cpp (仿真应用）
0. line_geometry
定义了一系列线特征几何操作。
- plk_to_pose
返回相机坐标系下的plk坐标。
template <typename T>
Eigen::Matrix<T, 6, 1> plk_to_pose(Eigen::Matrix<T, 6, 1> plk_w,
Eigen::Matrix<T, 3, 3> Rcw,
Eigen::Matrix<T, 3, 1> tcw) {
Eigen::Matrix<T, 3, 1> nw = plk_w.head(3);
Eigen::Matrix<T, 3, 1> vw = plk_w.tail(3);
Eigen::Matrix<T, 3, 1> nc = Rcw * nw + skew_symmetric(tcw) * Rcw * vw;
Eigen::Matrix<T, 3, 1> vc = Rcw * vw;
Eigen::Matrix<T, 6, 1> plk_c;
plk_c.head(3) = nc;
plk_c.tail(3) = vc;
return plk_c;
}
- plk_from_pose
返回世界坐标系下的plk坐标。
template <typename T>
Eigen::Matrix<T, 6, 1> plk_from_pose(Eigen::Matrix<T, 6, 1> plk_c,
Eigen::Matrix<T, 3, 3> Rcw,
Eigen::Matrix<T, 3, 1> tcw) {
Eigen::Matrix<T, 3, 3> Rwc = Rcw.transpose();
Eigen::Matrix<T, 3, 1> twc = -Rwc * tcw;
return plk_to_pose(plk_c, Rwc, twc);
}
参考如下( 注意图中右上角少了一个负号 )：
- pluckerOrigin
返回相机光心到直线的距离。
inline Eigen::Vector3d pluckerOrigin(const Vector6d &plk) {
const Eigen::Vector3d &n = plk.head<3>();
const Eigen::Vector3d &v = plk.tail<3>();
Eigen::Vector3d f = n.cross(v);
double g = v.squaredNorm();
Eigen::Vector3d p0 = 1. / g * f;
return p0;
}
其中 p 0 = n × v ∣ ∣ v ∣ ∣ 2 = ∣ ∣ n ∣ ∣ ∣ ∣ v ∣ ∣ \textbf{p}_0=\frac{\textbf{n}\times\textbf{v}}{||\textbf{v}||^2}=\frac{||\textbf{n}||}{||\textbf{v}||} p0=∣∣v∣∣2n×v=∣∣v∣∣∣∣n∣∣，包含了原点到直线的距离信息。
- orth_to_plk
正交参数化表示法转化成plk坐标。
template <typename T>
Eigen::Matrix<T, 6, 1> orth_to_plk( Eigen::Matrix<T, 4, 1> orth) {
Eigen::Matrix<T, 6, 1> plk;
Eigen::Matrix<T, 3, 1> theta = orth.head(3);
T phi = orth[3];
T s1 = ceres::sin(theta[0]);
T c1 = ceres::cos(theta[0]);
T s2 = ceres::sin(theta[1]);
T c2 = ceres::cos(theta[1]);
T s3 = ceres::sin(theta[2]);
T c3 = ceres::cos(theta[2]);
Matrix<T, 3, 3> R;
R <<
c2 * c3, s1 * s2 * c3 - c1 * s3, c1 * s2 * c3 + s1 * s3,
c2 * s3, s1 * s2 * s3 + c1 * c3, c1 * s2 * s3 - s1 * c3,
-s2, s1 * c2, c1 * c2;
T w1 = ceres::cos(phi);
T w2 = ceres::sin(phi);
T d = w1 / w2; // 原点到直线的距离
Matrix<T, 3, 1> u1 = R.col(0);
Matrix<T, 3, 1> u2 = R.col(1);
Matrix<T, 3, 1> n = w1 * u1;
Matrix<T, 3, 1> v = w2 * u2;
plk.head(3) = n;
plk.tail(3) = v;
return plk;
}
参考如下：
- plk_to_orth
plk坐标转换到正交参数表示。
Vector4d plk_to_orth(Vector6d plk) {
Vector4d orth;
Vector3d n = plk.head(3);
Vector3d v = plk.tail(3);
Vector3d u1 = n / n.norm();
Vector3d u2 = v / v.norm();
Vector3d u3 = u1.cross(u2);
// todo:: use SO3
orth[0] = atan2(u2(2), u3(2));
orth[1] = asin(-u1(2));
orth[2] = atan2(u1(1), u1(0));
Vector2d w(n.norm(), v.norm());
w = w / w.norm();
orth[3] = asin(w(1));
return orth;
}
- TrimLine
输入参数为两个线段端点坐标和直线的plk坐标，获取3d空间直线修整后的两端点坐标。
inline Vector6d TrimLine(const Eigen::Vector3d &spt, const Eigen::Vector3d &ept,
const Vector6d &plk) {
Eigen::Vector3d p11 = spt / spt.z(); //归一化
Eigen::Vector3d p21 = ept / ept.z(); //归一化
Eigen::Vector2d ln = (p11.cross(p21)).head(2); //法向量
ln = ln / ln.norm();
Eigen::Vector3d p12 = Eigen::Vector3d(p11(0) + ln(0), p11(1) + ln(1),
1.0); // 直线垂直方向上移动一个单位
Eigen::Vector3d p22 = Eigen::Vector3d(p21(0) + ln(0), p21(1) + ln(1), 1.0);
Eigen::Vector3d cam = Eigen::Vector3d(0, 0, 0);
Eigen::Vector4d pi1 = pi_from_ppp(cam, p11, p12); //三点确认一个平面 ax + by + cz + d = 0，返回Vector4d的平面方程 （a,b,c,d）
Eigen::Vector4d pi2 = pi_from_ppp(cam, p21, p22);
Eigen::Matrix4d Lc;
Eigen::Vector3d nc, vc;
nc = plk.head(3);
vc = plk.tail(3);
Lc << skew_symmetric(nc), vc, -vc.transpose(), 0; //获取直线法向量的对偶plk矩阵
Eigen::Vector4d e1 = Lc * pi1;
Eigen::Vector4d e2 = Lc * pi2;
e1 = e1 / e1(3);
e2 = e2 / e2(3);
Eigen::Vector3d pts_1(e1(0), e1(1), e1(2));
Eigen::Vector3d pts_2(e2(0), e2(1), e2(2));
Vector6d res;
res.head(3) = pts_1;
res.tail(3) = pts_2;
return res;
}
- line_to_orth
线段参数转正交参数表示，这里线段参数 前三维为线段上一点 P P P（光心到直线垂线交点），后三维为方向向量，所以和plk_to_orth基本一致。
Vector4d line_to_orth(Vector6d line) {
Vector4d orth;
Vector3d p = line.head(3);
Vector3d v = line.tail(3);
Vector3d n = p.cross(v);
Vector3d u1 = n / n.norm();
Vector3d u2 = v / v.norm();
Vector3d u3 = u1.cross(u2);
orth[0] = atan2(u2(2), u3(2));
orth[1] = asin(-u1(2));
orth[2] = atan2(u1(1), u1(0));
Vector2d w(n.norm(), v.norm());
w = w / w.norm();
orth[3] = asin(w(1));
return orth;
}
- orth_to_line
从正交表示转化为线段参数，注意这里的线段参数前三维是从相机光心到直线的垂线落点。
Vector6d orth_to_line(Vector4d orth) {
Vector6d line;
Vector3d theta = orth.head(3);
double phi = orth[3];
// todo:: SO3
double s1 = sin(theta[0]);
double c1 = cos(theta[0]);
double s2 = sin(theta[1]);
double c2 = cos(theta[1]);
double s3 = sin(theta[2]);
double c3 = cos(theta[2]);
Matrix3d R;
R <<
c2 * c3, s1 * s2 * c3 - c1 * s3, c1 * s2 * c3 + s1 * s3,
c2 * s3, s1 * s2 * s3 + c1 * c3, c1 * s2 * s3 - s1 * c3,
-s2, s1 * c2, c1 * c2;
double w1 = cos(phi);
double w2 = sin(phi);
double d = w1 / w2; // 原点到直线的距离
line.head(3) = -R.col(2) * d;
line.tail(3) = R.col(1);
return line;
}
- pi_from_ppp
三点确立一个平面，返回的是平面方程 a x + b y + c z + d = 0 ax+by+cz+d=0 ax+by+cz+d=0 的 [ a , b , c , d ] [a, b, c, d] [a,b,c,d]；
Vector4d pi_from_ppp(Vector3d x1, Vector3d x2, Vector3d x3) {
Vector4d pi;
pi << (x1 - x3).cross(x2 - x3), -x3.dot(
x1.cross(x2)); // d = - x3.dot( (x1-x3).cross( x2-x3 ) ) = - x3.dot( x1.cross( x2 ) )
return pi;
}
- pipi_plk
两个平面相交确立一条直线，返回plk坐标。
Vector6d pipi_plk(Vector4d pi1, Vector4d pi2) {
Vector6d plk;
Matrix4d dp = pi1 * pi2.transpose() - pi2 * pi1.transpose();
plk << dp(0, 3), dp(1, 3), dp(2, 3), -dp(1, 2), dp(0, 2), -dp(0, 1);
return plk;
}
参考如下：
- plucker_origin
获取光心到直线的垂直点。
Vector3d plucker_origin(Vector3d n, Vector3d v) {
return v.cross(n) / v.dot(v);
}
- point_to_pose & point_from_pose
将空间点的坐标进行两个坐标系下的转换。
- line_to_pose & line_from_pose
将空间直线的参数进行两个坐标系下的转换，其中线参数前3维是光心到直线的垂直点。
- plk_to_pose & plk_from_pose
将空间直线的plk坐标进行两个坐标系下的转换
1. general_line_factor.h
以优化两帧位姿的GeneralLineErrorTwoCameraTerm为例。
namespace general_line {
class GeneralLineErrorTwoCameraTerm {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GeneralLineErrorTwoCameraTerm(const Eigen::Vector4d &obs_t_,
const Eigen::Vector3d &x_dir,
const Eigen::Matrix3d &R_c_l_)
: obs_t(obs_t_), x_dir_(x_dir), R_c_l(R_c_l_) {}
- obs_t ：目标帧（target frame）的线段特征量测，包括两个端点的归一化坐标。
- x_dir ：线段局部坐标系原点在主相机坐标系（host frame）下的坐标。即
- R_c_l ：从局部参数空间到相机坐标系的变换矩阵，即 R P C \textbf{R}^C_P RPC 或者 R L C \textbf{R}^C_L RLC
template<typename T>
// theta, rho: line parameter
// phi: mahata angle
bool operator()(const T *const line_ptr, const T *const T_w_i_h_ptr,
const T *const T_w_i_t_ptr, const T *const T_i_c_ptr,
T *residual_ptr) const {
using Vector3T = Eigen::Matrix<T, 3, 1>;
using Vector2T = Eigen::Matrix<T, 2, 1>;
using Matrix33T = Eigen::Matrix<T, 3, 3>;
using Vector6T = Eigen::Matrix<T, 6, 1>;
using QuaternionT = Eigen::Quaternion<T>;
using TransformT = Twist<T>;
// line parameter
T rho = line_ptr[0];
T theta = line_ptr[1];
Eigen::Map<const QuaternionT> q_w_i_h(T_w_i_h_ptr + 3);
Eigen::Map<const Vector3T> t_w_i_h(T_w_i_h_ptr);
Eigen::Map<const QuaternionT> q_w_i_t(T_w_i_t_ptr + 3);
Eigen::Map<const Vector3T> t_w_i_t(T_w_i_t_ptr);
Eigen::Map<const QuaternionT> q_i_c(T_i_c_ptr + 3);
Eigen::Map<const Vector3T> t_i_c(T_i_c_ptr);
TransformT T_i_c(q_i_c, t_i_c);
TransformT T_w_i_h(q_w_i_h, t_w_i_h); // what is h? host
TransformT T_w_i_t(q_w_i_t, t_w_i_t); // what is t? target
TransformT T_w_c_h = T_w_i_h * T_i_c;
TransformT T_w_c_t = T_w_i_t * T_i_c;
TransformT T_c_w_h = T_w_c_h.inverse();
TransformT T_c_w_t = T_w_c_t.inverse();
TransformT T_t_c_h_c = T_w_c_t.inverse() * T_w_c_h;
Matrix33T R_t_c_h_c = T_t_c_h_c.rotationMatrix();
输入变量：
- line_ptr ：线段参数，包括 ρ \rho ρ 和 θ \theta θ。
- T_w_i_h_ptr：host帧的body坐标系在世界坐标系下的位姿。
- T_w_i_t_ptr：target帧的body坐标系在世界坐标系下的位姿。
- T_i_c_ptr：body和camera坐标系的变换矩阵。
tips: GeneralLineErrorOneCameraTerm 只优化 host frame 的 body位姿，T_w_i_t的时候也是以T_w_i_h_ptr来初始化的。
中间变量：
- T_t_c_h_c ：历史相机帧到目标相机帧的变换矩阵，即 T C i C j = ( T C j W ) T T C i W \textbf{T}^{C_j}_{C_i}=({\textbf{T}^{W}_{C_j}})^T\textbf{T}^{W}_{C_i} TCiCj=(TCjW)TTCiW。
- R_t_c_h_c：host相机帧到目标相机帧的旋转矩阵，不包括平移，即 R C i C j \textbf{R}^{C_j}_{C_i} RCiCj
Vector3T line_c;
line_c << ceres::cos(theta), ceres::sin(theta), T(0); //这里还没乘上rho，因为是方向向量
line_c = R_c_l.cast<T>() * line_c; // 从局部参数空间转换到相机坐标系
Vector3T origin_t = R_t_c_h_c * x_dir_.cast<T>() + rho*T_t_c_h_c.pos;
Vector3T line_c_t = R_t_c_h_c * line_c;
Vector3T n_c_t = line_c_t.cross(origin_t);
- origin_t ：目标相机帧下的局部参数坐标系原点的坐标，即 s ′ \textbf{s}^{\prime} s′。
- line_c_t ：目标相机帧下的线段方向向量，即 c v ′ ^c\textbf{v}^{\prime} cv′。
- n_c_t ：目标帧的归一化相机坐标系下的直线方程，即
l
m
i
=
[
s
′
]
×
c
v
′
\textbf{l}^{m_i}=[\textbf{s}^{\prime}]\times {^c\textbf{v}^{\prime}}
lmi=[s′]×cv′。
T l_norm = n_c_t(0) * n_c_t(0) + n_c_t(1) * n_c_t(1);
T l_sqrtnorm = ceres::sqrt(l_norm);
T e1 = T(obs_t(0)) * n_c_t(0) + T(obs_t(1)) * n_c_t(1) + n_c_t(2);
T e2 = T(obs_t(2)) * n_c_t(0) + T(obs_t(3)) * n_c_t(1) + n_c_t(2);
Eigen::Map<Vector2T> residual(residual_ptr);
residual(0) = e1 / l_sqrtnorm;
residual(1) = e2 / l_sqrtnorm;
residual =
general_line::GeneralLineErrorOneCameraTerm::sqrtInfo_.cast<T>() *
residual;
return true;
}
2. general_line_parameterization.h
namespace general_line {
// 定义的误差函数这里是
// define a local parameterization for upadting the angle to be constrained in [-pi, pi)
class GeneralLineLocalParameterization {
public:
template<typename T>
bool operator()(const T *theta, const T *delta, T *thetaPlusDelta) const {
thetaPlusDelta[0] = theta[0] + delta[0]; // 逆深度可以直接加减
thetaPlusDelta[1] = normalizeAngle(theta[1] + T(1.0) * delta[1]);
return true;
}
static ceres::LocalParameterization *create() {
return (new ceres::AutoDiffLocalParameterization<GeneralLineLocalParameterization, 2, 2>);
}
};
} // namespace struct_vio
- 其中 normalizeAngle 函数是为了将 θ \theta θ 控制在 [-pi, pi]范围内。
3. plucker_line_parameterization
class LineOrthParameterization : public ceres::LocalParameterization {
virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;
virtual bool ComputeJacobian(const double *x, double *jacobian) const;
virtual int GlobalSize() const { return 4; };
virtual int LocalSize() const { return 4; };
};
定义了一个线特征的局部正交参数化类，包括参数更新和雅各比计算等，正交参数化表达参见
SLAM线特征学习（1）——基本的线特征表示与优化推导，在此不再赘述。
4. plucker_projection_factor.h
声明了三种factor ：lineProjectionFactor，lineProjectionFactor_incamera 和 lineProjectionFactor_instartframe。
同样定义了两种误差，PluckerLineErrorOneCameraTerm和PluckerLineErrorTwoCameraTerm，差别就在于是否同时优化历史帧和目标帧，这里以OneCameraTerm为例。
class PluckerLineErrorOneCameraTerm {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
PluckerLineErrorOneCameraTerm(const Eigen::Vector4d &_pts_i)
: obs_i(_pts_i) {}
template<typename T>
// theta, rho: line parameter
// phi: mahata angle
bool operator()(const T *const line_ptr, const T *const T_w_i_t_ptr,
const T *const T_i_c_ptr, T *residual_ptr) const {
using Vector3T = Eigen::Matrix<T, 3, 1>;
using Vector4T = Eigen::Matrix<T, 4, 1>;
using Vector2T = Eigen::Matrix<T, 2, 1>;
using Matrix33T = Eigen::Matrix<T, 3, 3>;
using Vector6T = Eigen::Matrix<T, 6, 1>;
using QuaternionT = Eigen::Quaternion<T>;
using TransformT = Twist<T>;
// line parameter
Eigen::Map<const QuaternionT> q_w_i_t(T_w_i_t_ptr + 3);
Eigen::Map<const Vector3T> t_w_i_t(T_w_i_t_ptr);
Eigen::Map<const QuaternionT> q_i_c(T_i_c_ptr + 3);
Eigen::Map<const Vector3T> t_i_c(T_i_c_ptr);
TransformT T_i_c(q_i_c, t_i_c);
TransformT T_w_i_t(q_w_i_t, t_w_i_t);
TransformT T_w_c_t = T_w_i_t * T_i_c;
与之前的GeneralLineErrorOneCameraTerm不同的是，输入变量为T_w_i_t_ptr，即待优化的是目标帧的body位姿。
Vector4T line_orth;
line_orth << line_ptr[0], line_ptr[1], line_ptr[2], line_ptr[3];
Vector6T line_w = orth_to_plk(line_orth);
Vector6T line_c = plk_from_pose(line_w, T_w_c_t.rotationMatrix(), T_w_c_t.pos);
Vector3T n_c = line_c.head(3);
T l_norm = n_c(0) * n_c(0) + n_c(1) * n_c(1);
T l_sqrtnorm = ceres::sqrt(l_norm);
T e1 = T(obs_i(0)) * n_c(0) + T(obs_i(1)) * n_c(1) + n_c(2);
T e2 = T(obs_i(2)) * n_c(0) + T(obs_i(3)) * n_c(1) + n_c(2);
Eigen::Map<Vector2T> residual(residual_ptr);
residual(0) = e1 / l_sqrtnorm;
residual(1) = e2 / l_sqrtnorm;
- line_orth ：世界坐标系下的空间直线的正交4参数表示。
- 将输入的正交4参数表示的直线坐标转换成plk坐标后，直接得到直线法向量（直线方程），代入求解点到直线距离的残差。
residual = PluckerLineErrorOneCameraTerm::sqrtInfo_.cast<T>() *
residual;
return true;
}
static ceres::CostFunction *create(const Eigen::Vector4d &obs_t_) {
return (new ceres::AutoDiffCostFunction<PluckerLineErrorOneCameraTerm, 2, 4,
7, 7>(
new PluckerLineErrorOneCameraTerm(obs_t_)));
}
public:
static Eigen::Matrix2d
sqrtInfo_; // the inverse square root of the measurement covariance matrix
Eigen::Vector4d obs_i;
};
- 2, 4, 7, 7 分别代表残差维度、待优化直线参数维度、待优化目标帧body位姿维度和待优化body和camera变换矩阵的维度
5. plucker_projection_factor.cpp
主要定义了ceres优化过程中的雅各比矩阵。
lineProjectionFactor::lineProjectionFactor
总的雅各比链式求解公式如下：
∂ r L ∂ X = ∂ r L ∂ L I ∂ L I ∂ L n ∂ L n ∂ L c { ∂ L c ∂ T w i X [ 0 ～ 6 ] ∂ L c ∂ T b c X [ 7 ～ 13 ] ∂ L c ∂ L o r t h X [ 14 ～ 17 ] \frac{\partial \textbf{r}_{\textbf{L}}}{\partial \textbf{X}}= \frac{\partial \textbf{r}_{\textbf{L}}}{\partial L_{I}} \frac{\partial L_{I}}{\partial L_{n}} \frac{\partial L_{n}}{\partial L_{c}} \begin{cases} \frac{\partial L_{c}}{\partial T_{wi}} & X[0～6] \\ \frac{\partial L_{c}}{\partial T_{bc}} & X[7～13] \\ \frac{\partial L_{c}}{\partial L_{orth}} & X[14～17] \end{cases} ∂X∂rL=∂LI∂rL∂Ln∂LI∂Lc∂Ln⎩⎪⎨⎪⎧∂Twi∂Lc∂Tbc∂Lc∂Lorth∂LcX[0～6]X[7～13]X[14～17]
bool lineProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
Eigen::Vector3d tic(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Quaterniond qic(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
Eigen::Vector4d line_orth(parameters[2][0], parameters[2][1], parameters[2][2], parameters[2][3]);
Vector6d line_w = orth_to_plk(line_orth);
Eigen::Matrix3d Rwb(Qi);
Eigen::Vector3d twb(Pi);
Vector6d line_b = plk_from_pose(line_w, Rwb, twb);
//std::cout << line_b.norm() <<"\n";
Eigen::Matrix3d Rbc(qic);
Eigen::Vector3d tbc(tic);
Vector6d line_c = plk_from_pose(line_b, Rbc, tbc);
// 直线的投影矩阵K为单位阵
Eigen::Vector3d nc = line_c.head(3);
double l_norm = nc(0) * nc(0) + nc(1) * nc(1);
double l_sqrtnorm = sqrt(l_norm);
double l_trinorm = l_norm * l_sqrtnorm;
double e1 = obs_i(0) * nc(0) + obs_i(1) * nc(1) + nc(2);
double e2 = obs_i(2) * nc(0) + obs_i(3) * nc(1) + nc(2);
Eigen::Map<Eigen::Vector2d> residual(residuals);
residual(0) = e1 / l_sqrtnorm;
residual(1) = e2 / l_sqrtnorm;
// std::cout <<"---- sqrt_info: ------"<< sqrt_info << std::endl;
// sqrt_info.setIdentity();
residual = sqrt_info * residual;
残差形式一如既往，接下来看雅各比的求解。
按雅各比链式法则，首先求解的是
∂
r
L
∂
L
I
\frac{\partial \textbf{r}_{\textbf{L}}}{\partial L_{I}}
∂LI∂rL，即误差对图像坐标系下直线方程的雅各比。
if (jacobians) {
Eigen::Matrix<double, 2, 3> jaco_e_l(2, 3);
jaco_e_l << (obs_i(0) / l_sqrtnorm - nc(0) * e1 / l_trinorm), (obs_i(1) / l_sqrtnorm - nc(1) * e1 / l_trinorm),
1.0 / l_sqrtnorm,
(obs_i(2) / l_sqrtnorm - nc(0) * e2 / l_trinorm), (obs_i(3) / l_sqrtnorm - nc(1) * e2 / l_trinorm),
1.0 / l_sqrtnorm;
jaco_e_l = sqrt_info * jaco_e_l;
接下来就是求解 ∂ L I ∂ L n ∂ L n ∂ L c \frac{\partial L_{I}}{\partial L_{n}} \frac{\partial L_{n}}{\partial L_{c}} ∂Ln∂LI∂Lc∂Ln，由于代码中为了仿真方便直接设置投影矩阵为单位阵，所以下面将直接求解 ∂ L I ∂ L c \frac{\partial L_{I}}{\partial L_{c}} ∂Lc∂LI。
Eigen::Matrix<double, 3, 6> jaco_l_Lc(3, 6);
jaco_l_Lc.setZero();
jaco_l_Lc.block(0, 0, 3, 3) = Eigen::Matrix3d::Identity();
Eigen::Matrix<double, 2, 6> jaco_e_Lc;
jaco_e_Lc = jaco_e_l * jaco_l_Lc;
显然， ∂ L I ∂ L c = [ I 3 × 3 0 3 × 3 ] 3 × 6 \frac{\partial L_{I}}{\partial L_{c}}=\begin{bmatrix} \textbf{I}_{3\times3} & \textbf{0}_{3\times3} \end{bmatrix}_{3\times6} ∂Lc∂LI=[I3×303×3]3×6
接下来首先求解对相机的body位姿 T w i T_{wi} Twi 的雅各比：
if (jacobians[0]) {
//std::cout <<"jacobian_pose_i"<<"\n";
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
Matrix6d invTbc;
invTbc << Rbc.transpose(), -Rbc.transpose() * skew_symmetric(tbc),
Eigen::Matrix3d::Zero(), Rbc.transpose();
Vector3d nw = line_w.head(3);
Vector3d dw = line_w.tail(3);
Eigen::Matrix<double, 6, 6> jaco_Lc_pose;
jaco_Lc_pose.setZero();
jaco_Lc_pose.block(0, 0, 3, 3) = Rwb.transpose() * skew_symmetric(dw); // Lc_t
jaco_Lc_pose.block(0, 3, 3, 3) = skew_symmetric(
Rwb.transpose() * (nw + skew_symmetric(dw) * twb)); // Lc_theta
jaco_Lc_pose.block(3, 3, 3, 3) = skew_symmetric(Rwb.transpose() * dw);
jaco_Lc_pose = invTbc * jaco_Lc_pose;
//std::cout <<invTbc<<"\n"<<jaco_Lc_pose<<"\n\n";
jacobian_pose_i.leftCols<6>() = jaco_e_Lc * jaco_Lc_pose;
//std::cout <<jacobian_pose_i<<"\n\n";
jacobian_pose_i.rightCols<1>().setZero(); //最后一列设成0
}
- 这里虽然参数变量为7个维度，但实际上还是对姿态角(3维)和平移(3维)求雅各比，最后一列设为0了，解析计算参考如下，结合李群扰动模型即可求解 (注意图中右上角少了一个负号)：
- invTbc，为plk坐标的从相机坐标系到body坐标系的转移矩阵。
对外参变换矩阵 T b c T_{bc} Tbc 的雅各比求解与对body位姿的求解形式一致，就是换了个参数，接下来看对世界坐标系下直线正交参数的雅各比求解：
if (jacobians[2]) {
Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> jacobian_lineOrth(jacobians[2]);
Eigen::Matrix3d Rwc = Rwb * Rbc;
Eigen::Vector3d twc = Rwb * tbc + twb;
Matrix6d invTwc;
invTwc << Rwc.transpose(), -Rwc.transpose() * skew_symmetric(twc),
Eigen::Matrix3d::Zero(), Rwc.transpose();
//std::cout<<invTwc<<"\n";
Vector3d nw = line_w.head(3);
Vector3d vw = line_w.tail(3);
Vector3d u1 = nw / nw.norm();
Vector3d u2 = vw / vw.norm();
Vector3d u3 = u1.cross(u2);
Vector2d w(nw.norm(), vw.norm());
w = w / w.norm();
Eigen::Matrix<double, 6, 4> jaco_Lw_orth;
jaco_Lw_orth.setZero();
jaco_Lw_orth.block(3, 0, 3, 1) = w[1] * u3;
jaco_Lw_orth.block(0, 1, 3, 1) = -w[0] * u3;
jaco_Lw_orth.block(0, 2, 3, 1) = w(0) * u2;
jaco_Lw_orth.block(3, 2, 3, 1) = -w(1) * u1;
jaco_Lw_orth.block(0, 3, 3, 1) = -w(1) * u1;
jaco_Lw_orth.block(3, 3, 3, 1) = w(0) * u2;
//std::cout<<jaco_Lw_orth<<"\n";
jacobian_lineOrth = jaco_e_Lc * invTwc * jaco_Lw_orth;
}
参考如下：
lineProjectionFactor_incamera::lineProjectionFactor_incamera
其中残差是用第 i 帧 （代码中写的是obs_i, 但理论上应该是 obs_j ？）的观测和从第 i 帧 的直线plk参数递推出的第 j 帧的直线plk参数来计算, 如下：
L
c
j
←
R
w
b
j
,
R
b
c
L
w
←
R
w
b
i
,
R
b
c
L
c
i
L_{c_j} \xleftarrow{R_{w_{bj}}, R_{bc}} L_{w} \xleftarrow{R_{w_{bi}}, R_{bc}} L_{c_{i}}
LcjRwbj,RbcLwRwbi,RbcLci
- 仍然是要先求解 ∂ r L ∂ X = ∂ r L ∂ L I ∂ L I ∂ L n ∂ L n ∂ L c \frac{\partial \textbf{r}_{\textbf{L}}}{\partial \textbf{X}}= \frac{\partial \textbf{r}_{\textbf{L}}}{\partial L_{I}} \frac{\partial L_{I}}{\partial L_{n}} \frac{\partial L_{n}}{\partial L_{c}} ∂X∂rL=∂LI∂rL∂Ln∂LI∂Lc∂Ln
- 对各参数空间的雅各比求解，注意与lineProjectionFactor::lineProjectionFactor形式上略有不同，但仍然是基于同一个线性转换矩阵求解，以
∂
n
w
∂
t
\frac{\partial n_w}{\partial t}
∂t∂nw 为例：
∂ n w ∂ t = R b i w T [ R b i w ( t w b i + ∂ t ) ] × d b i ∂ t = [ t w b i + ∂ t ] × R w b i d b i ∂ t = − [ R w b i d b i ] × \frac{\partial n_w}{\partial t}= \frac { R_{b_iw}^T[R_{b_iw}(t_{w_bi}+\partial t)]\times d_{bi}}{{\partial t}}=\frac{[t_{w_bi}+\partial t]\times R_{wb_i}d_{b_i}}{\partial t}=-[R_{wb_i}d_{b_i}]\times ∂t∂nw=∂tRbiwT[Rbiw(twbi+∂t)]×dbi=∂t[twbi+∂t]×Rwbidbi=−[Rwbidbi]×
lineProjectionFactor_instartframe::lineProjectionFactor_instartframe
该factor只优化直线参数。
- 仍然是要先求解 ∂ r L ∂ X = ∂ r L ∂ L I ∂ L I ∂ L n ∂ L n ∂ L c \frac{\partial \textbf{r}_{\textbf{L}}}{\partial \textbf{X}}= \frac{\partial \textbf{r}_{\textbf{L}}}{\partial L_{I}} \frac{\partial L_{I}}{\partial L_{n}} \frac{\partial L_{n}}{\partial L_{c}} ∂X∂rL=∂LI∂rL∂Ln∂LI∂Lc∂Ln。
- 然后只计算对直线参数的雅各比，形式一致。
6. pose_local_parameterization
定义了位姿局部参数化的一系列操作，包括参数更新和雅各比计算等，其中
- virtual int GlobalSize() const { return 7; }; 即参数自由度为7（存在冗余）。
- virtual int LocalSize() const { return 6; }; 即delta所在正切空间的参数自由度为6 （姿态3+平移3）。
PoseLocalParameterization::Plus
位置更新通过简单加减，姿态更新通过四元数相乘。
PoseLocalParameterization::ComputeJacobian
在局部参数和全局参数的雅各比之间进行转换，参考如下：
以及
7. line_manager（比较重要，论文中创新部分）
首先看定义的两个线特征struct。
- LineLandmark
struct LineLandmark {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
bool is_triangulation{false};
bool boptimize_success{false};
int id;
std::array<double, 2> para_data{};
Eigen::Vector2d para;
Eigen::Vector3d x_dir;
Eigen::Vector3d y_dir;
Eigen::Vector3d z_dir;
Eigen::Matrix3d R_c_l;
Eigen::Vec6d plk_w;
Eigen::Vec6d plk_h;
Eigen::Vec3d spt_w;
Eigen::Vec3d ept_w;
// groudtruth
Eigen::Vec6d plk_w_gt;
Eigen::Vector3d spt_w_gt;
Eigen::Vector3d ept_w_gt;
// plucker optimazation
Eigen::Vec4d line_orth_c;
};
待解释。
2. line_obs （简单的线特征观测）
struct line_obs {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::Vector4d obs;
int landmark_id;
};
包含两个端点的坐标和线特征ID。
triangulate_line
输入变量：
- Eigen::aligned_unordered_map<int, LineLandmark> &line_landmarks，所有的空间线
- Eigen::aligned_map<int, Eigen::aligned_map<int, line_obs>> &line_obs_map，每条空间线(第一个int)在某相机帧中(第二个int）的观测
- Eigen::aligned_vector &poses，所有的位姿
- bool plucker_optimize，是否进行普吕克优化 ( 0: LINE_OPTIMIZE 1: PLUCKER_OPTIMIZE)
for (auto &landmark : line_landmarks) {
int landmark_id = landmark.second.id;
if (line_obs_map.find(landmark_id) == line_obs_map.end() or
line_obs_map.at(landmark_id).size() < 2) {
continue;
}
int start_frame_id = line_obs_map.at(landmark_id).begin()->first;
Transformd T_w_s = poses.at(start_frame_id);
遍历所有空间线，找到第一次观测到该空间线的起始帧 S S S 和相应的位姿 T w s T_{ws} Tws，若只在最新一帧观测到或只被一个历史帧观测到，则跳过。
Eigen::Vector4d obs_line =
line_obs_map.at(landmark_id).at(start_frame_id).obs;
Eigen::Vector3d spt{obs_line(0), obs_line(1), 1};
Eigen::Vector3d ept{obs_line(2), obs_line(3), 1};
Eigen::Vector4d plane_para = pi_from_ppp(spt, ept, Eigen::Vector3d(0, 0, 0));
Eigen::Vector3d plane_norm = plane_para.head(3);
plane_norm.normalize();
获取该空间线在起始帧的线段观测，通过两个端点和相机光心确定一个平面方程，并提取其法向量 plane_norm。
Eigen::Vector3d mpt = (spt + ept) / 2;
// Eigen::Vector3d mpt = spt;
Eigen::Vector3d y_dir = mpt.normalized();
Eigen::Vector3d x_dir = y_dir.cross(plane_norm);
x_dir.norm();
landmark.second.x_dir = y_dir;
landmark.second.y_dir = x_dir;
landmark.second.z_dir = plane_norm;
landmark.second.R_c_l.col(0) = x_dir;
landmark.second.R_c_l.col(1) = y_dir;
landmark.second.R_c_l.col(2) = plane_norm;
- 以线段观测的中点 m_pt 为原点 (论文里以端点为原点建立局部坐标系），建立局部坐标系 P P P ，以光心到 m_pt 为坐标系 P P P 的y轴方向向量，上一步求得的法向量为z轴方向，叉乘获得x轴方向，进一步获得局部坐标系和相机坐标系的转换矩阵 R c l R_{cl} Rcl。
- 需要额外注意的是，landmark.second中存储的xy轴方向，与Rcl中的xy正好相反。
Eigen::Vector3d obs_dir = ept - spt;
obs_dir.normalize();
Eigen::Vector3d tij;
Eigen::Matrix3d Rij;
Eigen::Vector4d obsi, obsj; // obs from two frame are used to do triangulation
// plane pi from ith obs in ith camera frame
Eigen::Vector4d pii;
Eigen::Vector3d ni; // normal vector of plane
double min_cos_theta = 1;
获取线段观测的方向向量，初始化第 i 帧和 第 j 帧的相对位姿、各帧观测、第 i 帧平面和法向量，准备进行三角化。
for (const auto &obs : line_obs_map.at(landmark_id)) // 遍历该空间线所有的观测
{
int target_frame_id = obs.first;
if (target_frame_id == start_frame_id) // 第一个观测是start frame 上
{
obsi = obs.second.obs;
Eigen::Vector3d p1(obs.second.obs[0], obs.second.obs[1], 1);
Eigen::Vector3d p2(obs.second.obs[2], obs.second.obs[3], 1);
pii = pi_from_ppp(p1, p2, Vector3d(0, 0, 0));
ni = pii.head(3);
ni.normalize();
continue;
}
Transformd T_w_t = poses.at(target_frame_id);
Transformd T_s_t = T_w_s.inverse() * T_w_t;
// 非start frame(其他帧)上的观测
Eigen::Vector3d t = T_s_t.pos; // tij
Eigen::Matrix3d R = T_s_t.rotationMatrix(); // Rij
// plane pi from jth obs in ith camera frame
Vector3d p3(obs.second.obs[0], obs.second.obs[1], 1);
Vector3d p4(obs.second.obs[2], obs.second.obs[3], 1);
p3 = R * p3 + t;
p4 = R * p4 + t;
Vector4d pij = pi_from_ppp(p3, p4, t);
Eigen::Vector3d nj = pij.head(3);
nj.normalize();
double cos_theta = ni.dot(nj);
if (cos_theta < min_cos_theta) {
min_cos_theta = cos_theta;
tij = t;
Rij = R;
obsj = obs.second.obs;
}
}
if (min_cos_theta > 0.998) {
continue;
char tmp;
std::cout << "parital is not enough" << std::endl;
std::cin >> tmp;
}
- 遍历该空间线的所有观测，若为起始帧上的观测，则获取对应的平面方程和法向量ni（？这里之前不是求过了吗？）
- 若不是起始帧，则求出起始帧和目标帧之间的相对位姿变换 T_s_t ，即旋转 R 和 平移 t。
- 通过相对位姿变换矩阵，把起始帧的观测两端点坐标转换到目标帧下，确立新的平面方程和法向量 nj。
- 计算法向量ni和nj之间的的 cos \cos cos 角度偏差，若小于当前最小的 cos θ \cos\theta cosθ （即变换角度 θ \theta θ 最大），则保存对应的R、t 和观测 obs_j。
- 遍历结束，获得法向量角度偏差最大的目标帧观测和相对于起始帧的位姿。若min_cos_theta 仍大于阈值( 角度偏差不够大），则警告视差不够。
- 可以理解为找出与起始帧视差最大的一个目标帧。
// plane pi from jth obs in ith camera frame
Vector3d p3(obsj(0), obsj(1), 1);
Vector3d p4(obsj(2), obsj(3), 1);
p3 = Rij * p3 + tij;
p4 = Rij * p4 + tij;
Vector4d pij = pi_from_ppp(p3, p4, tij);
Vector6d plk = pipi_plk(pii, pij);
Vector3d n = plk.head(3);
Vector3d v = plk.tail(3);
landmark.second.plk_h = plk; // plk in camera frame
landmark.second.is_triangulation = true;
求出
π
j
\pi_j
πj 在起始帧的平面方程，结合
π
i
\pi_i
πi 求出该空间线在起始帧的plk坐标，赋值给 属性plk_h 并将三角化标志位置 true。
（仿真代码中的plk坐标真值验证代码掠过）。
if (!plucker_optimize) {
Eigen::Vector3d vc_l = landmark.second.R_c_l.transpose() * vc;//转换到局部参数空间
double cos_thet = std::atan2(vc_l.y(), vc_l.x());
Eigen::Vector3d line_origin = pluckerOrigin(line_w);
double distance = landmark.second.x_dir.dot(line_origin);
// landmark.para.x() = 1.0 / distance;
landmark.second.para.x() = 0.2;
// landmark.second.para.y() = acos(cos_theta);
landmark.second.para.y() = cos_thet;
}
若不进行普吕克坐标优化，则
- 在局部参数坐标系下获取直线方向相对于图中x轴的偏角 θ \theta θ
- line_origin 是相机光心到直线的垂直点。
- 初始化空间线的局部参数
(
ρ
,
θ
)
(\rho,\theta)
(ρ,θ)，其中
ρ
\rho
ρ 初始化为 0.2。
else {
// landmark.second.line_orth = plk_to_orth(line_w);
// todo In order to equality, we use the same initial value to triangulate the line
Eigen::Vector3d vc_l = landmark.second.R_c_l.transpose() * vc;
double cos_thet = std::atan2(vc_l.y(), vc_l.x());
double d = 1 / 0.2;
//origin_c 为 线段中点在相机坐标系的坐标，但这个d？
Eigen::Vector3d origin_c = landmark.second.x_dir * d;
Eigen::Vector3d line_c;
line_c << cos(cos_thet), sin(cos_thet), 0;
line_c = landmark.second.R_c_l * line_c;
Eigen::Matrix<double, 6, 1> plk_c_h;
plk_c_h.head(3) = origin_c.cross(line_c);
plk_c_h.tail(3) = line_c;
//landmark.second.line_orth_c = plk_to_orth(landmark.second.plk_h);
landmark.second.line_orth_c = plk_to_orth(plk_c_h);
}
若进行吕克坐标优化，则
- 根据局部参数坐标，获取对应的相机坐标系下的plk坐标和正交参数化坐标。
- 这里的d的数值大小似乎没用，反正都是用来求 n c n_c nc 的？ 但这样求出的plk坐标，就没有蕴含 ∣ ∣ n c ∣ ∣ ∣ ∣ v c ∣ ∣ \frac{||n_c||}{||v_c||} ∣∣vc∣∣∣∣nc∣∣ 的距离信息了，固定成0.2。推测会在后续过程中继续优化。
updatelineplk
输入变量和 triangulate_line一样，最终目的是更新每条空间线的plk坐标和空间端点坐标 spt_w和ept_w。
for (auto &landmark : line_landmarks) {
int landmark_id = landmark.second.id;
int host_frame_id = line_obs_map.at(landmark_id).begin()->first;
同样是遍历所有空间线，得到对应的ID号和host_frame的ID号（起始帧？）。
if (!plucker_optimize) {
double d = 1 / landmark.second.para[0];
double theta = landmark.second.para[1];
Eigen::Vector3d origin = landmark.second.x_dir * d;
Eigen::Vector3d line_c;
line_c << cos(theta), sin(theta), 0;
line_c = landmark.second.R_c_l * line_c;
Eigen::Matrix<double, 6, 1> plk_c_h;
plk_c_h.head(3) = origin.cross(line_c);
plk_c_h.tail(3) = line_c;
landmark.second.plk_h = plk_c_h;
Transformd T_w_s = poses.at(host_frame_id);
Eigen::Vec6d line_w = plk_to_pose(landmark.second.plk_h, T_w_s.rotationMatrix(),
T_w_s.pos); // transfrom to world frame
landmark.second.plk_w = line_w; // transfrom to camera frame
}
else {
Vector4d line_orth_c = landmark.second.line_orth_c;
Eigen::Vec6d line_c = orth_to_plk(line_orth_c);
Transformd T_w_s = poses.at(host_frame_id);
Vector6d line_w = plk_to_pose(line_c, T_w_s.rotationMatrix(), T_w_s.pos); // transfrom to world frame
// Vector4d line_orth_w = landmark.second.line_orth;
// Eigen::Vec6d line_w = orth_to_plk(line_orth_w);
// Transformd T_w_s = poses.at(host_frame_id);
// Vector6d line_c = plk_from_pose(line_w, T_w_s.rotationMatrix(), T_w_s.pos);
landmark.second.plk_h = line_c;
landmark.second.plk_w = line_w;
}
- 若没有进行普吕克优化，则获取该空间线的参数 ( ρ , θ ) (\rho, \theta) (ρ,θ)，并得到对应的相机坐标系下的plk坐标，进一步结合全局位姿获得世界坐标系下的plk坐标 plk_w。
- 若进行普吕克优化，则直接获取该空间线在相机坐标系下的正交参数，通过函数转化获得相应的plk_c和plk_w。
Eigen::Vector3d line_origin = pluckerOrigin(landmark.second.plk_w);
landmark.second.spt_w = line_origin;
landmark.second.ept_w = line_origin;
double d1 = 0;
double d2 = 0;
auto &line_obs = line_obs_map.at(landmark_id);
for (const auto &obs : line_obs) {
int target_id = obs.first;
Transformd T_w_t = poses.at(target_id);
Transformd T_t_w = T_w_t.inverse();
Vector6d plk_t = plk_to_pose(landmark.second.plk_w, T_t_w.rotationMatrix(), T_t_w.pos);
Eigen::Vec3d p11{obs.second.obs[0], obs.second.obs[1], 1.0};
Eigen::Vec3d p21{obs.second.obs[2], obs.second.obs[3], 1.0};
Vector6d cpt = TrimLine(p11, p21, plk_t);
Eigen::Vector3d cpt1 = cpt.head<3>();
Eigen::Vector3d cpt2 = cpt.tail<3>();
if (cpt1(2) < 0 || cpt2(2) < 0) {
break;
}
if ((cpt1 - cpt2).norm() > 8) {
break;
}
landmark.second.boptimize_success = true;
Eigen::Vector3d wpt1 = T_w_t * cpt1;
Eigen::Vector3d wpt2 = T_w_t * cpt2;
- 对该空间线在每个相机帧下的观测，结合在该帧下的plk坐标(plk_t) ，通过trimline获取该帧相机坐标系下的3D空间直线的两个端点坐标，若满足条件则判定 boptimize_success = true ，并结合该帧位姿获取在世界坐标系下的3D空间端点坐标。
// line project distance
Eigen::Vector3d line_dir;
if (landmark.second.plk_w[5] < 0) {
line_dir = -landmark.second.plk_w.tail<3>();
} else {
line_dir = landmark.second.plk_w.tail<3>();
} //获取世界坐标系下的空间线方向向量
double d1_tmp = (wpt1 - line_origin).dot(line_dir);
double d2_tmp = (wpt2 - line_origin).dot(line_dir);
if (d2_tmp < d1_tmp) {
std::swap(d1_tmp, d2_tmp);
Eigen::Vector3d tmp = wpt1;
wpt1 = wpt2;
wpt2 = tmp;
} //令wpt1为离直线垂直点最近的端点
if (target_id == host_frame_id) {
landmark.second.spt_w = wpt1;
landmark.second.ept_w = wpt2;
d1 = d1_tmp;
d2 = d2_tmp;
} //若目标帧就是起始帧，则赋值该空间线的端点3D坐标
if (d1 == d2) {
d1 = d1_tmp;
d2 = d2_tmp;
continue;
}
if (d1_tmp >= d1) {
d1 = d1_tmp;
landmark.second.spt_w = wpt1;
}
if (d2_tmp <= d2) {
d2 = d2_tmp;
landmark.second.ept_w = wpt2;
}
}
本质上就是对某条空间线遍历其在所有帧内的观测，通过trimline获取相应的3D空间端点，投影到直线方向，投影模值最大的赋值给该空间线的 spt_w 和 ept_w。
optimize_line_without_plucker
使用的factor 是 general_line::GeneralLineErrorTwoCameraTerm，只优化线参数 ( ρ , θ ) (\rho, \theta) (ρ,θ) （2维） ，相机位姿和外参不参与优化，使用自动求导。
optimize_line_without_plucker2
形式稍有不同，但和上面的基本一致。
optimize_line_with_plucker
使用的factor是 plucker::LineOrthParameterization， 只优化空间线的正交参数 ( 4维），相机位姿和外参不参与优化，使用自动求导。
optimize_line_with_plucker2
形式稍有不同，但和上面的基本一致。
8. line_optimization.cpp (仿真应用）
int main(int argc, char **argv) {
general_line::GeneralLineErrorOneCameraTerm::sqrtInfo_ =
460 / 3.2 * Matrix2d::Identity();
general_line::GeneralLineErrorTwoCameraTerm::sqrtInfo_ =
460 / 3.2 * Matrix2d::Identity();
lineProjectionFactor::sqrt_info = 460 / 3.2 * Matrix2d::Identity();
PluckerLineErrorOneCameraTerm::sqrtInfo_ = 460 / 3.2 * Matrix2d::Identity();
PluckerLineErrorTwoCameraTerm::sqrtInfo_ = 460 / 3.2 * Matrix2d::Identity();
std::string pose_path =
"../bin/cam_pose_tum.txt";
// "/home/ubuntu/Downloads/vio_data_simulation/bin/keyframe/all_lines_599.txt"
std::string line_obs_path =
"../bin/keyframe/all_lines_";
std::string line_landmarks_path =
"../bin/house_model/house.txt";
// step1: read camera poses
Eigen::aligned_vector<Transformd> camera_poses;
std::vector<std::array<double, 7>> camera_pose_parameters;
std::array<double, 7> T_i_c_paramter{0, 0, 0, 0, 0, 0, 1};
read_camera_pose(pose_path, camera_poses, camera_pose_parameters);
// step2: read line landmak groudtruth
Eigen::aligned_unordered_map<int, LineLandmark> line_landmarks;
read_linelandmark(line_landmarks_path, line_landmarks);
// step3: read line observation
Eigen::aligned_map<int, Eigen::aligned_map<int, line_obs>> line_obs_map;
read_line_obs(line_obs_path, line_obs_map);
add_noise_obs(line_obs_map, 4.5);
// step4: triangulation line
cout << "line_landmark.size(): " << line_landmarks.size() << endl;
triangulate_line(line_landmarks,line_obs_map, camera_poses, plucker_optimize);
if(plucker_optimize)
{
//optimize_line_with_plucker(line_landmarks, line_obs_map, camera_pose_parameters, T_i_c_paramter);
optimize_line_with_plucker2(line_landmarks, line_obs_map, camera_pose_parameters, T_i_c_paramter);
} else
{
//optimize_line_without_plucker(line_landmarks, line_obs_map, camera_pose_parameters, T_i_c_paramter);
optimize_line_without_plucker2(line_landmarks, line_obs_map, camera_pose_parameters, T_i_c_paramter);
}
updatelineplk(line_landmarks, line_obs_map, camera_poses, plucker_optimize);
savefeatures(line_landmarks, plucker_optimize);
return 0;
}
- 首先读取所有的相机位姿、空间直线真值、直线观测（并加入噪声）。
- 基于已知的相机位姿和直线观测，进行空间线的三角化。
- 根据相应的优化方式（直线参数优化或者普吕克参数优化）对空间直线参数进行优化。
- 更新空间直线端点坐标并保存。
- 完成仿真验证。
