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);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
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;
}
b.segment<ZDim>(row) = bi;

13
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -233,15 +233,16 @@ public:
size_t m = cameras.size();
bool retriangulate = decideIfTriangulate(cameras);
// triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras
MonoCameras monoCameras;
MonoMeasurements monoMeasured;
for(size_t i = 0; i < m; i++) {
Pose3 leftPose = cameras[i].pose();
Cal3_S2 monoCal = cameras[i].calibration().calibration();
MonoCamera leftCamera_i(leftPose,monoCal);
Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0));
Pose3 rightPose = leftPose.compose( left_Pose_right );
MonoCamera rightCamera_i(rightPose,monoCal);
const Pose3 leftPose = cameras[i].pose();
const Cal3_S2 monoCal = cameras[i].calibration().calibration();
const MonoCamera leftCamera_i(leftPose,monoCal);
const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0));
const Pose3 rightPose = leftPose.compose( left_Pose_right );
const MonoCamera rightCamera_i(rightPose,monoCal);
const StereoPoint2 zi = measured_[i];
monoCameras.push_back(leftCamera_i);
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;
views.push_back(x1);
@ -1381,6 +1381,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
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));
Values rotValues;
@ -1391,6 +1394,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
rotValues);
// check that it is non degenerate
EXPECT(smartFactor->isValid());
// Hessian is invariant to rotations in the nondegenerate case
EXPECT(
assert_equal(hessianFactor->information(),
@ -1407,10 +1413,14 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
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(
assert_equal(hessianFactor->information(),
#ifdef GTSAM_USE_EIGEN_MKL
hessianFactorRotTran->information(), 1e-5));
#else
hessianFactorRotTran->information(), 1e-6));
#endif
}
/* ************************************************************************* */