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

Tightened odometry sigmas to avoid ILS

release/4.3a0
Frank Dellaert 2019-06-09 19:22:54 -04:00
parent c007c7715c
commit 6bcbfe2c67
2 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/RangeISAMExample_plaza2.cpp
View File

@ -108,7 +108,7 @@ int main (int argc, char** argv) {
// Set Noise parameters // Set Noise parameters
Vector priorSigmas = Vector3(1,1,M_PI); Vector priorSigmas = Vector3(1,1,M_PI);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2); Vector odoSigmas = Vector3(0.05, 0.01, 0.1);
double sigmaR = 100; // range standard deviation double sigmaR = 100; // range standard deviation
const NM::Base::shared_ptr // all same type const NM::Base::shared_ptr // all same type
priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
@ -157,7 +157,7 @@ int main (int argc, char** argv) {
boost::tie(t, odometry) = timedOdometry; boost::tie(t, odometry) = timedOdometry;
// add odometry factor // add odometry factor
newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas))); newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry, odoNoise));
// predict pose and add as initial estimate // predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry); Pose2 predictedPose = lastPose.compose(odometry);

12
gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
View File

@ -111,11 +111,11 @@ int main(int argc, char** argv) {
// Set Noise parameters // Set Noise parameters
Vector priorSigmas = Vector3(1, 1, M_PI); Vector priorSigmas = Vector3(1, 1, M_PI);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2); Vector odoSigmas = Vector3(0.05, 0.01, 0.1);
auto odoNoise = NM::Diagonal::Sigmas(odoSigmas);
double sigmaR = 100; // range standard deviation double sigmaR = 100; // range standard deviation
const NM::Base::shared_ptr // all same type const NM::Base::shared_ptr // all same type
priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
rangeNoise = robust ? tukey : gaussian; rangeNoise = robust ? tukey : gaussian;
@ -167,11 +167,11 @@ int main(int argc, char** argv) {
double t; double t;
Pose2 odometry; Pose2 odometry;
boost::tie(t, odometry) = timedOdometry; boost::tie(t, odometry) = timedOdometry;
printf("step %d, time = %g\n",(int)i,t);
// add odometry factor // add odometry factor
newFactors.push_back( newFactors.push_back(
BetweenFactor<Pose2>(i - 1, i, odometry, BetweenFactor<Pose2>(i - 1, i, odometry, odoNoise));
NM::Diagonal::Sigmas(odoSigmas)));
// predict pose and add as initial estimate // predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry); Pose2 predictedPose = lastPose.compose(odometry);
@ -187,9 +187,9 @@ int main(int argc, char** argv) {
if (smart && totalCount < minK) { if (smart && totalCount < minK) {
try { try {
smartFactors[j]->addRange(i, range); smartFactors[j]->addRange(i, range);
printf("adding range %g for %d on %d",range,(int)j,(int)i); printf("adding range %g for %d",range,(int)j);
} catch (const invalid_argument& e) { } catch (const invalid_argument& e) {
printf("warning: omitting duplicate range %g for %d on %d",range,(int)j,(int)i); printf("warning: omitting duplicate range %g for %d",range,(int)j);
} }
cout << endl; cout << endl;
} }