void EdgeProjectXYZ2UVQ::linearizeOplus()
{
VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
SE3Quat T(vj->estimate());
VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
Vector3d xyz = vi->estimate();
Vector3d xyz_trans = T.map(xyz);
const double & x = xyz_trans[0];
const double & y = xyz_trans[1];
const double & z = xyz_trans[2];
double z_sq = z*z;
const double & Fx = vj->_focal_length(0);
const double & Fy = vj->_focal_length(1);
double dq_dz = -Fx/z_sq;
double x_Fx_by_zsq = x*Fx/z_sq;
double y_Fy_by_zsq = y*Fy/z_sq;
Matrix3d R = T.rotation().toRotationMatrix();
_jacobianOplusXi.row(0) = -Fx/z*R.row(0) + x_Fx_by_zsq*R.row(2);
_jacobianOplusXi.row(1) = -Fy/z*R.row(1) + y_Fy_by_zsq*R.row(2);
_jacobianOplusXi.row(2) = -dq_dz*R.row(2);
_jacobianOplusXj(0,0) = x*y/z_sq *Fx;
_jacobianOplusXj(0,1) = -(1+(x*x/z_sq)) *Fx;
_jacobianOplusXj(0,2) = y/z *Fx;
_jacobianOplusXj(0,3) = -1./z *Fx;
_jacobianOplusXj(0,4) = 0;
_jacobianOplusXj(0,5) = x/z_sq *Fx;
_jacobianOplusXj(1,0) = (1+y*y/z_sq) *Fy;
_jacobianOplusXj(1,1) = -x*y/z_sq *Fy;
_jacobianOplusXj(1,2) = -x/z *Fy;
_jacobianOplusXj(1,3) = 0;
_jacobianOplusXj(1,4) = -1./z *Fy;
_jacobianOplusXj(1,5) = y/z_sq *Fy;
_jacobianOplusXj(2,0) = -y*dq_dz ;
_jacobianOplusXj(2,1) = x*dq_dz;
_jacobianOplusXj(2,2) = 0;
_jacobianOplusXj(2,3) = 0;
_jacobianOplusXj(2,4) = 0;
_jacobianOplusXj(2,5) = -dq_dz ;
// std::cerr << _jacobianOplusXi << std::endl;
// std::cerr << _jacobianOplusXj << std::endl;
// BaseBinaryEdge<3, Vector3d, VertexPointXYZ, VertexSE3Expmap, false>::linearizeOplus();
// std::cerr << _jacobianOplusXi << std::endl;
// std::cerr << _jacobianOplusXj << std::endl;
}
void EdgeSE3Expmap::linearizeOplus() {
VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);
SE3Quat Ti(vi->estimate());
VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
SE3Quat Tj(vj->estimate());
const SE3Quat & Tij = _measurement;
SE3Quat invTij = Tij.inverse();
SE3Quat invTj_Tij = Tj.inverse()*Tij;
SE3Quat infTi_invTij = Ti.inverse()*invTij;
_jacobianOplusXi = invTj_Tij.adj();
_jacobianOplusXj = -infTi_invTij.adj();
}
//覆写求雅克比矩阵 的虚函数
// plus in manifold
virtual void linearizeOplus( )
{
if ( level() == 1 )
{
_jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
return;
}
VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ ); // q in book
double x = xyz_trans[0];//X
double y = xyz_trans[1];//Y
double invz = 1.0/xyz_trans[2];// 1/Z
double invz_2 = invz*invz;// 1/Z^2
float u = x*fx_*invz + cx_;
float v = y*fy_*invz + cy_;
// jacobian from se3 to u,v
// NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
// 旋转在前 平移在后 g2o u对∇f的偏导数 像素坐标 对 变换矩阵李代数增量 的导数
// J1= [ fx *X*Y/Z^2 -fx *(1 + X^2/Z^2) fx*Y/Z -fx/Z 0 fx * X/Z^2
// fy *(1 + Y^2/Z^2) -fy * X*Y/Z^2 -fy*X/Z 0 -fy/Z fy* Y/Z^2 ]
// 上面 误差为e = I(p2) - I2(p1) 原来e = I(p1) - I2(p2) 所以 雅克比 相差一个 负号
Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;
jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
jacobian_uv_ksai ( 0,3 ) = invz *fx_;
jacobian_uv_ksai ( 0,4 ) = 0;
jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;
jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
jacobian_uv_ksai ( 1,3 ) = 0;
jacobian_uv_ksai ( 1,4 ) = invz *fy_;
jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;
// I2对u偏导 J2 图像灰度梯度可以得到(x方向梯度 y方向梯度 离散求解 坐标前后灰度值作差/2)
//这里由于各个像素点其实是离散值，其实求的是差分，前一个像素灰度值减后一个像素灰度值，除以2，即认为是这个方向上的梯度
Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;//灰度梯度 x方向 离散形式
jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;// 灰度梯度 y方向
_jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;//最后的 雅克比矩阵
}
void EdgeProjectPSI2UV::linearizeOplus() {
VertexSBAPointXYZ* vpoint = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
Vector3d psi_a = vpoint->estimate();
VertexSE3Expmap * vpose = static_cast<VertexSE3Expmap *>(_vertices[1]);
SE3Quat T_cw = vpose->estimate();
VertexSE3Expmap * vanchor = static_cast<VertexSE3Expmap *>(_vertices[2]);
const CameraParameters * cam
= static_cast<const CameraParameters *>(parameter(0));
SE3Quat A_aw = vanchor->estimate();
SE3Quat T_ca = T_cw*A_aw.inverse();
Vector3d x_a = invert_depth(psi_a);
Vector3d y = T_ca*x_a;
Matrix<double,2,3> Jcam
= d_proj_d_y(cam->focal_length,
y);
_jacobianOplus[0] = -Jcam*d_Tinvpsi_d_psi(T_ca, psi_a);
_jacobianOplus[1] = -Jcam*d_expy_d_y(y);
_jacobianOplus[2] = Jcam*T_ca.rotation().toRotationMatrix()*d_expy_d_y(x_a);
}
void EdgeProjectXYZ2UV::linearizeOplus() {
VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
SE3Quat T(vj->estimate());
VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
Vector3d xyz = vi->estimate();
Vector3d xyz_trans = T.map(xyz);
double x = xyz_trans[0];
double y = xyz_trans[1];
double z = xyz_trans[2];
double z_2 = z*z;
const CameraParameters * cam = static_cast<const CameraParameters *>(parameter(0));
Matrix<double,2,3> tmp;
tmp(0,0) = cam->focal_length;
tmp(0,1) = 0;
tmp(0,2) = -x/z*cam->focal_length;
tmp(1,0) = 0;
tmp(1,1) = cam->focal_length;
tmp(1,2) = -y/z*cam->focal_length;
_jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();
_jacobianOplusXj(0,0) = x*y/z_2 *cam->focal_length;
_jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *cam->focal_length;
_jacobianOplusXj(0,2) = y/z *cam->focal_length;
_jacobianOplusXj(0,3) = -1./z *cam->focal_length;
_jacobianOplusXj(0,4) = 0;
_jacobianOplusXj(0,5) = x/z_2 *cam->focal_length;
_jacobianOplusXj(1,0) = (1+y*y/z_2) *cam->focal_length;
_jacobianOplusXj(1,1) = -x*y/z_2 *cam->focal_length;
_jacobianOplusXj(1,2) = -x/z *cam->focal_length;
_jacobianOplusXj(1,3) = 0;
_jacobianOplusXj(1,4) = -1./z *cam->focal_length;
_jacobianOplusXj(1,5) = y/z_2 *cam->focal_length;
}
void EdgeSE3ProjectXYZ::linearizeOplus() {
VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
SE3Quat T(vj->estimate());
VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
Vector3d xyz = vi->estimate();
Vector3d xyz_trans = T.map(xyz);
double x = xyz_trans[0];
double y = xyz_trans[1];
double z = xyz_trans[2];
double z_2 = z*z;
Matrix<double,2,3> tmp;
tmp(0,0) = fx;
tmp(0,1) = 0;
tmp(0,2) = -x/z*fx;
tmp(1,0) = 0;
tmp(1,1) = fy;
tmp(1,2) = -y/z*fy;
_jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();
_jacobianOplusXj(0,0) = x*y/z_2 *fx;
_jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx;
_jacobianOplusXj(0,2) = y/z *fx;
_jacobianOplusXj(0,3) = -1./z *fx;
_jacobianOplusXj(0,4) = 0;
_jacobianOplusXj(0,5) = x/z_2 *fx;
_jacobianOplusXj(1,0) = (1+y*y/z_2) *fy;
_jacobianOplusXj(1,1) = -x*y/z_2 *fy;
_jacobianOplusXj(1,2) = -x/z *fy;
_jacobianOplusXj(1,3) = 0;
_jacobianOplusXj(1,4) = -1./z *fy;
_jacobianOplusXj(1,5) = y/z_2 *fy;
}
