15 using namespace Eigen;
22 _jacobianOplus.clear();
23 _jacobianOplus.push_back(JacobianType(0, 3, 3));
24 _jacobianOplus.push_back(JacobianType(0, 3, 3));
25 _jacobianOplus.push_back(JacobianType(0, 3, 1));
32 is >>
p[0] >>
p[1] >>
p[2];
36 for (
int i = 0;
i < 3; ++
i)
37 for (
int j =
i;
j < 3; ++
j) {
38 is >> information()(
i,
j);
40 information()(
j,
i) = information()(
i,
j);
47 Vector3d
p = measurement().toVector();
48 os <<
p.x() <<
" " <<
p.y() <<
" " <<
p.z();
49 for (
int i = 0;
i < 3; ++
i)
50 for (
int j =
i;
j < 3; ++
j)
51 os <<
" " << information()(
i,
j);
59 const g2o::VertexSE2* vi =
static_cast<const g2o::VertexSE2*
>(_vertices[0]);
60 const g2o::VertexSE2* vj =
static_cast<const g2o::VertexSE2*
>(_vertices[1]);
63 double thetai = vi->estimate().rotation().angle();
65 Vector2d
dt = vj->estimate().translation() - vi->estimate().translation();
66 double si=
sin(thetai), ci=
cos(thetai);
68 _jacobianOplus[0](0, 0) = -ci; _jacobianOplus[0](0, 1) = -si; _jacobianOplus[0](0, 2) = -si*
dt.x()+ci*
dt.y();
69 _jacobianOplus[0](1, 0) = si; _jacobianOplus[0](1, 1) = -ci; _jacobianOplus[0](1, 2) = -ci*
dt.x()-si*
dt.y();
70 _jacobianOplus[0](2, 0) = 0; _jacobianOplus[0](2, 1) = 0; _jacobianOplus[0](2, 2) = -1;
72 _jacobianOplus[1](0, 0) = ci; _jacobianOplus[1](0, 1)= si; _jacobianOplus[1](0, 2)= 0;
73 _jacobianOplus[1](1, 0) =-si; _jacobianOplus[1](1, 1)= ci; _jacobianOplus[1](1, 2)= 0;
74 _jacobianOplus[1](2, 0) = 0; _jacobianOplus[1](2, 1)= 0; _jacobianOplus[1](2, 2)= 1;
77 Matrix3d
z = Matrix3d::Zero();
78 z.block<2, 2>(0, 0) = rmean.rotation().toRotationMatrix();
80 _jacobianOplus[0] =
z * _jacobianOplus[0];
81 _jacobianOplus[1] =
z * _jacobianOplus[1];
84 _jacobianOplus[0]*=vSwitch->estimate();
85 _jacobianOplus[1]*=vSwitch->estimate();
89 _jacobianOplus[2].setZero();
97 const g2o::VertexSE2*
v1 =
static_cast<const g2o::VertexSE2*
>(_vertices[0]);
98 const g2o::VertexSE2*
v2 =
static_cast<const g2o::VertexSE2*
>(_vertices[1]);
102 _error =
delta.toVector() *
v3->estimate();