void EdgePointXYCovPointXYCov::linearizeOplus()
{
const VertexPointXYCov* vi = static_cast<const VertexPointXYCov*>(_vertices[0]);
const VertexPointXYCov* vj = static_cast<const VertexPointXYCov*>(_vertices[1]);
const double& x1 = vi->estimate()[0];
const double& y1 = vi->estimate()[1];
const double& th1 = 0.0;
//vi->estimate().rotation().angle();
const double& x2 = vj->estimate()[0];
const double& y2 = vj->estimate()[1];
double aux_1 = cos(th1) ;
double aux_2 = -aux_1 ;
double aux_3 = sin(th1) ;
_jacobianOplusXi( 0 , 0 ) = aux_2 ;
_jacobianOplusXi( 0 , 1 ) = -aux_3 ;
_jacobianOplusXi( 1 , 0 ) = aux_3 ;
_jacobianOplusXi( 1 , 1 ) = aux_2 ;
_jacobianOplusXj( 0 , 0 ) = aux_1 ;
_jacobianOplusXj( 0 , 1 ) = aux_3 ;
_jacobianOplusXj( 1 , 0 ) = -aux_3 ;
_jacobianOplusXj( 1 , 1 ) = aux_1 ;
}
void EdgeSE2::linearizeOplus()
{
const VertexSE2* vi = static_cast<const VertexSE2*>(_vertices[0]);
const VertexSE2* vj = static_cast<const VertexSE2*>(_vertices[1]);
number_t thetai = vi->estimate().rotation().angle();
Vector2 dt = vj->estimate().translation() - vi->estimate().translation();
number_t si=std::sin(thetai), ci=std::cos(thetai);
_jacobianOplusXi(0, 0) = -ci; _jacobianOplusXi(0, 1) = -si; _jacobianOplusXi(0, 2) = -si*dt.x()+ci*dt.y();
_jacobianOplusXi(1, 0) = si; _jacobianOplusXi(1, 1) = -ci; _jacobianOplusXi(1, 2) = -ci*dt.x()-si*dt.y();
_jacobianOplusXi(2, 0) = 0; _jacobianOplusXi(2, 1) = 0; _jacobianOplusXi(2, 2) = -1;
_jacobianOplusXj(0, 0) = ci; _jacobianOplusXj(0, 1)= si; _jacobianOplusXj(0, 2)= 0;
_jacobianOplusXj(1, 0) =-si; _jacobianOplusXj(1, 1)= ci; _jacobianOplusXj(1, 2)= 0;
_jacobianOplusXj(2, 0) = 0; _jacobianOplusXj(2, 1)= 0; _jacobianOplusXj(2, 2)= 1;
const SE2& rmean = _inverseMeasurement;
Matrix3 z = Matrix3::Zero();
z.block<2, 2>(0, 0) = rmean.rotation().toRotationMatrix();
z(2, 2) = 1.;
_jacobianOplusXi = z * _jacobianOplusXi;
_jacobianOplusXj = z * _jacobianOplusXj;
}
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;
}
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 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;
}
/**
* \brief Jacobian for stereo projection
*/
void Edge_XYZ_VSC::linearizeOplus()
{
VertexSCam *vc = static_cast<VertexSCam *>(_vertices[1]);
VertexSBAPointXYZ *vp = static_cast<VertexSBAPointXYZ *>(_vertices[0]);
Vector4d pt, trans;
pt.head<3>() = vp->estimate();
pt(3) = 1.0;
trans.head<3>() = vc->estimate().translation();
trans(3) = 1.0;
// first get the world point in camera coords
Eigen::Matrix<double,3,1> pc = vc->w2n * pt;
// Jacobians wrt camera parameters
// set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
double px = pc(0);
double py = pc(1);
double pz = pc(2);
double ipz2 = 1.0/(pz*pz);
if (isnan(ipz2) )
{
std::cout << "[SetJac] infinite jac" << std::endl;
*(int *)0x0 = 0;
}
double ipz2fx = ipz2*vc->Kcam(0,0); // Fx
double ipz2fy = ipz2*vc->Kcam(1,1); // Fy
double b = vc->baseline; // stereo baseline
Eigen::Matrix<double,3,1> pwt;
// check for local vars
pwt = (pt-trans).head<3>(); // transform translations, use differential rotation
// dx
Eigen::Matrix<double,3,1> dp = vc->dRdx * pwt; // dR'/dq * [pw - t]
_jacobianOplusXj(0,3) = (pz*dp(0) - px*dp(2))*ipz2fx;
_jacobianOplusXj(1,3) = (pz*dp(1) - py*dp(2))*ipz2fy;
_jacobianOplusXj(2,3) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
// dy
dp = vc->dRdy * pwt; // dR'/dq * [pw - t]
_jacobianOplusXj(0,4) = (pz*dp(0) - px*dp(2))*ipz2fx;
_jacobianOplusXj(1,4) = (pz*dp(1) - py*dp(2))*ipz2fy;
_jacobianOplusXj(2,4) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
// dz
dp = vc->dRdz * pwt; // dR'/dq * [pw - t]
_jacobianOplusXj(0,5) = (pz*dp(0) - px*dp(2))*ipz2fx;
_jacobianOplusXj(1,5) = (pz*dp(1) - py*dp(2))*ipz2fy;
_jacobianOplusXj(2,5) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
// set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
dp = -vc->w2n.col(0); // dpc / dx
_jacobianOplusXj(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
_jacobianOplusXj(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
_jacobianOplusXj(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
dp = -vc->w2n.col(1); // dpc / dy
_jacobianOplusXj(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
_jacobianOplusXj(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
_jacobianOplusXj(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
dp = -vc->w2n.col(2); // dpc / dz
_jacobianOplusXj(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
_jacobianOplusXj(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
_jacobianOplusXj(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
// Jacobians wrt point parameters
// set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
dp = vc->w2n.col(0); // dpc / dx
_jacobianOplusXi(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
_jacobianOplusXi(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
_jacobianOplusXi(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
dp = vc->w2n.col(1); // dpc / dy
_jacobianOplusXi(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
_jacobianOplusXi(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
_jacobianOplusXi(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
dp = vc->w2n.col(2); // dpc / dz
_jacobianOplusXi(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
_jacobianOplusXi(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
_jacobianOplusXi(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px
}
