12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

change interface to match PinholePose.

release/4.3a0
cbeall3 2015-07-27 23:25:05 -04:00
parent 02c7b2b81d
commit b5aa7fb7f0
2 changed files with 27 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/StereoCamera.h
View File

@ -66,8 +66,8 @@ public:
StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
/// Return shared pointer to calibration
const Cal3_S2Stereo::shared_ptr calibration() const {
return K_;
const Cal3_S2Stereo& calibration() const {
return *K_;
}
/// @}

25
gtsam/slam/tests/testTriangulationFactor.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
@ -63,6 +64,30 @@ TEST( triangulation, TriangulationFactor ) {
CHECK(assert_equal(HExpected, HActual, 1e-3));
}
//******************************************************************************
TEST( triangulation, TriangulationFactorStereo ) {
// Create the factor with a measurement that is 3 pixels off in x
Key pointKey(1);
SharedNoiseModel model;
Cal3_S2Stereo::shared_ptr stereoCal(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, 0.5));
StereoCamera camera2(pose1, stereoCal);
StereoPoint2 z2 = camera2.project(landmark);
typedef TriangulationFactor<StereoCamera> Factor;
Factor factor(camera2, z2, model, pointKey);
// Use the factor to calculate the Jacobians
Matrix HActual;
factor.evaluateError(landmark, HActual);
Matrix HExpected = numericalDerivative11<Vector,Point3>(
boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
// Verify the Jacobians are correct
CHECK(assert_equal(HExpected, HActual, 1e-3));
}
//******************************************************************************
int main() {
TestResult tr;