32 using namespace gtsam;
37 static std::shared_ptr<Cal3_S2Stereo>
K(
new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
139 Matrix H1Actual, H2Actual;
143 Matrix H1Expected = (
Matrix(3, 6) << 0.0, -625.0, 0.0, -100.0, 0.0, 0.0,
144 0.0, -625.0, 0.0, -100.0, 0.0, -8.0,
145 625.0, 0.0, 0.0, 0.0, -100.0, 0.0).finished();
148 0.0, 100.0, 0.0).finished();
167 Matrix H1Actual, H2Actual;
171 Matrix H1Expected = (
Matrix(3, 6) << -100.0, 0.0, 650.0, 0.0, 100.0, 0.0,
172 -100.0, -8.0, 649.2, -8.0, 100.0, 0.0,
173 -10.0, -650.0, 0.0, 0.0, 0.0, 100.0).finished();
176 0.0, 0.0, -100.0).finished();