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;
}
/**
* \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
}