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

Merge branch 'feature/heterogeneousSmartFactorNoise' into feature/improvementsIncrementalFilter

release/4.3a0
lcarlone 2016-08-06 23:28:48 -04:00
parent 359a61a547 edcf94591d
commit ad984807bf
2 changed files with 37 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -74,6 +74,7 @@ namespace gtsam {
std::cout << triangulation << std::endl;
}
// get class variables
LinearizationMode getLinearizationMode() const {
return linearizationMode;
}
@ -89,12 +90,19 @@ namespace gtsam {
bool getThrowCheirality() const {
return throwCheirality;
}
double getRetriangulationThreshold() const {
return retriangulationThreshold;
}
// set class variables
void setLinearizationMode(LinearizationMode linMode) {
linearizationMode = linMode;
}
void setDegeneracyMode(DegeneracyMode degMode) {
degeneracyMode = degMode;
}
void setRetriangulationThreshold(double retriangulationTh) {
retriangulationThreshold = retriangulationTh;
}
void setRankTolerance(double rankTol) {
triangulation.rankTolerance = rankTol;
}

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

@ -79,6 +79,35 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
LevenbergMarquardtParams lm_params;
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, params) {
SmartStereoProjectionParams p;
// check default values and "get"
EXPECT(p.getLinearizationMode() == HESSIAN);
EXPECT(p.getDegeneracyMode() == IGNORE_DEGENERACY);
EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-5, 1e-9);
EXPECT(p.getVerboseCheirality() == false);
EXPECT(p.getThrowCheirality() == false);
// check "set"
p.setLinearizationMode(JACOBIAN_SVD);
p.setDegeneracyMode(ZERO_ON_DEGENERACY);
p.setRankTolerance(100);
p.setEnableEPI(true);
p.setLandmarkDistanceThreshold(200);
p.setDynamicOutlierRejectionThreshold(3);
p.setRetriangulationThreshold(1e-2);
EXPECT(p.getLinearizationMode() == JACOBIAN_SVD);
EXPECT(p.getDegeneracyMode() == ZERO_ON_DEGENERACY);
EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().rankTolerance, 100, 1e-5);
EXPECT(p.getTriangulationParameters().enableEPI == true);
EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().landmarkDistanceThreshold, 200, 1e-5);
EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().dynamicOutlierRejectionThreshold, 3, 1e-5);
EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-2, 1e-5);
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor) {
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));