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

Merge remote-tracking branch 'origin/feature/heterogeneousSmartFactorNoise' into feature/heterogeneousSmartFactorNoise

release/4.3a0
lcarlone 2016-08-02 18:50:54 -04:00
parent e54159c695 938454916f
commit a93c1e86f2
3 changed files with 20 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/CameraSet.h
View File

@ -57,7 +57,7 @@ protected:
Vector b(ZDim * m); Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
Vector bi = traits<Z>::Local(measured[i], predicted[i]); Vector bi = traits<Z>::Local(measured[i], predicted[i]);
if(ZDim==3 && std::isnan(bi(1))){ // compensate for the case in which the right pixel in a stereoPoint is missing (nan) if(ZDim==3 && std::isnan(bi(1))){ // if it is a stereo point and the right pixel is missing (nan)
bi(1) = 0; bi(1) = 0;
} }
b.segment<ZDim>(row) = bi; b.segment<ZDim>(row) = bi;

13
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -233,15 +233,16 @@ public:
size_t m = cameras.size(); size_t m = cameras.size();
bool retriangulate = decideIfTriangulate(cameras); bool retriangulate = decideIfTriangulate(cameras);
// triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras
MonoCameras monoCameras; MonoCameras monoCameras;
MonoMeasurements monoMeasured; MonoMeasurements monoMeasured;
for(size_t i = 0; i < m; i++) { for(size_t i = 0; i < m; i++) {
Pose3 leftPose = cameras[i].pose(); const Pose3 leftPose = cameras[i].pose();
Cal3_S2 monoCal = cameras[i].calibration().calibration(); const Cal3_S2 monoCal = cameras[i].calibration().calibration();
MonoCamera leftCamera_i(leftPose,monoCal); const MonoCamera leftCamera_i(leftPose,monoCal);
Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0)); const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0));
Pose3 rightPose = leftPose.compose( left_Pose_right ); const Pose3 rightPose = leftPose.compose( left_Pose_right );
MonoCamera rightCamera_i(rightPose,monoCal); const MonoCamera rightCamera_i(rightPose,monoCal);
const StereoPoint2 zi = measured_[i]; const StereoPoint2 zi = measured_[i];
monoCameras.push_back(leftCamera_i); monoCameras.push_back(leftCamera_i);
monoMeasured.push_back(Point2(zi.uL(),zi.v())); monoMeasured.push_back(Point2(zi.uL(),zi.v()));

14
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -1348,7 +1348,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) {
vector<Key> views; vector<Key> views;
views.push_back(x1); views.push_back(x1);
@ -1381,6 +1381,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize( boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
values); values);
// check that it is non degenerate
EXPECT(smartFactor->isValid());
Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Values rotValues; Values rotValues;
@ -1391,6 +1394,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize( boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
rotValues); rotValues);
// check that it is non degenerate
EXPECT(smartFactor->isValid());
// Hessian is invariant to rotations in the nondegenerate case // Hessian is invariant to rotations in the nondegenerate case
EXPECT( EXPECT(
assert_equal(hessianFactor->information(), assert_equal(hessianFactor->information(),
@ -1407,10 +1413,14 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactorRotTran = boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
smartFactor->linearize(tranValues); smartFactor->linearize(tranValues);
// Hessian is invariant to rotations and translations in the nondegenerate case // Hessian is invariant to rotations and translations in the degenerate case
EXPECT( EXPECT(
assert_equal(hessianFactor->information(), assert_equal(hessianFactor->information(),
#ifdef GTSAM_USE_EIGEN_MKL
hessianFactorRotTran->information(), 1e-5));
#else
hessianFactorRotTran->information(), 1e-6)); hessianFactorRotTran->information(), 1e-6));
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */