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 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;
Matrix<double,2,3> tmp;
tmp(0,0) = vj->_focal_length(0);
tmp(0,1) = 0;
tmp(0,2) = -x/z*vj->_focal_length(0);
tmp(1,0) = 0;
tmp(1,1) = vj->_focal_length(1);
tmp(1,2) = -y/z*vj->_focal_length(1);
_jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();
_jacobianOplusXj(0,0) = x*y/z_2 *vj->_focal_length(0);
_jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *vj->_focal_length(0);
_jacobianOplusXj(0,2) = y/z *vj->_focal_length(0);
_jacobianOplusXj(0,3) = -1./z *vj->_focal_length(0);
_jacobianOplusXj(0,4) = 0;
_jacobianOplusXj(0,5) = x/z_2 *vj->_focal_length(0);
_jacobianOplusXj(1,0) = (1+y*y/z_2) *vj->_focal_length(1);
_jacobianOplusXj(1,1) = -x*y/z_2 *vj->_focal_length(1);
_jacobianOplusXj(1,2) = -x/z *vj->_focal_length(1);
_jacobianOplusXj(1,3) = 0;
_jacobianOplusXj(1,4) = -1./z *vj->_focal_length(1);
_jacobianOplusXj(1,5) = y/z_2 *vj->_focal_length(1);
}