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

C++ cleanup and dataset switch

release/4.3a0
Frank Dellaert 2013-06-22 00:49:00 +00:00
parent 680d4a6a94
commit 05f883f953
3 changed files with 112 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
.cproject
View File

@ -453,14 +453,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testVelocityConstraint3.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVelocityConstraint3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBTree.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -621,6 +613,54 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testPlanarSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPlanarSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose2SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose3SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimulated2D.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimulated2DOriented.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVisualSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVisualSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testProjectionFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testProjectionFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -629,18 +669,18 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSerializationSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget> <buildTarget>testSerializationSLAM.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testSerialization.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testSerialization.run</buildTarget> <buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -677,14 +717,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testWhiteNoiseFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testWhiteNoiseFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="schedulingExample.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="schedulingExample.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -805,6 +837,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testImuFactor.run" path="build/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j8</buildArguments>
<buildTarget>testImuFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDiscreteFactor.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testDiscreteFactor.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -885,14 +925,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testBayesTreeOperations.run" path="build/gtsam_unstable/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBayesTreeOperations.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -1125,14 +1157,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testSerializationSLAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSerializationSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -1597,14 +1621,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testRot3Q.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRot3Q.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1930,6 +1946,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testImuFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>

71
examples/RangeISAMExample_plaza2.cpp
View File

@ -48,6 +48,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
namespace NM = gtsam::noiseModel;
// data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/ // data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
// Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html) // Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
@ -91,7 +92,7 @@ vector<RangeTriple> readTriples() {
} }
// main // main
int main(int argc, char** argv) { int main (int argc, char** argv) {
// load Plaza2 data // load Plaza2 data
list<TimedOdometry> odometry = readOdometry(); list<TimedOdometry> odometry = readOdometry();
@ -102,14 +103,20 @@ int main(int argc, char** argv) {
// parameters // parameters
size_t minK = 150; // minimum number of range measurements to process initially size_t minK = 150; // minimum number of range measurements to process initially
size_t incK = 5; // minimum number of range measurements to process after size_t incK = 25; // minimum number of range measurements to process after
double sigmaR = 100; // range standard deviation bool groundTruth = false;
bool batchInitialization = true; bool robust = true;
// Set Noise parameters // Set Noise parameters
const noiseModel::Robust::shared_ptr rangeNoiseModel = Vector priorSigmas = Vector_(3, 1.0, 1.0, M_PI);
noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(15), Vector odoSigmas = Vector_(3, 0.05, 0.01, 0.2);
noiseModel::Isotropic::Sigma(1, sigmaR)); double sigmaR = 100; // range standard deviation
const NM::Base::shared_ptr // all same type
priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
rangeNoise = robust ? tukey : gaussian;
// Initialize iSAM // Initialize iSAM
ISAM2 isam; ISAM2 isam;
@ -118,34 +125,40 @@ int main(int argc, char** argv) {
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
M_PI - 2.02108900000000); M_PI - 2.02108900000000);
NonlinearFactorGraph newFactors; NonlinearFactorGraph newFactors;
newFactors.add( newFactors.add(PriorFactor<Pose2>(0, pose0, priorNoise));
PriorFactor<Pose2>(0, pose0,
noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 1.0, M_PI))));
Values initial; Values initial;
initial.insert(0, pose0); initial.insert(0, pose0);
// initialize points drawn from sigma=1 Gaussian in matlab version // initialize points drawn from sigma=1 Gaussian in matlab version
initial.insert(symbol('L', 1), Point2(3.5784, 2.76944)); if (groundTruth) { // from TL file
initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492)); initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549)); initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
initial.insert(symbol('L', 5), Point2(0.714743, -0.204966)); initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
} else {
initial.insert(symbol('L', 1), Point2(3.5784, 2.76944));
initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492));
initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549));
initial.insert(symbol('L', 5), Point2(0.714743, -0.204966));
}
// set some loop variables
size_t i = 1; // step counter
size_t k = 0; // range measurement counter
bool initialized = false;
Pose2 lastPose = pose0;
size_t countK = 0;
// Loop over odometry // Loop over odometry
gttic_(iSAM); gttic_(iSAM);
size_t i = 1; // step counter
size_t k = 0; // range measurement counter
bool update = false;
Pose2 lastPose = pose0;
size_t countK = 0;
BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) { BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
//--------------------------------- odometry loop -----------------------------------------
double t; double t;
Pose2 odometry; Pose2 odometry;
boost::tie(t, odometry) = timedOdometry; boost::tie(t, odometry) = timedOdometry;
// add odometry factor // add odometry factor
newFactors.add( newFactors.add(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
BetweenFactor<Pose2>(i - 1, i, odometry,
noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.01, 0.2))));
// predict pose and add as initial estimate // predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry); Pose2 predictedPose = lastPose.compose(odometry);
@ -156,22 +169,19 @@ int main(int argc, char** argv) {
while (k < K && t >= boost::get<0>(triples[k])) { while (k < K && t >= boost::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]); size_t j = boost::get<1>(triples[k]);
double range = boost::get<2>(triples[k]); double range = boost::get<2>(triples[k]);
newFactors.add( newFactors.add(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise));
RangeFactor<Pose2, Point2>(i, symbol('L', j), range,
rangeNoiseModel));
k = k + 1; k = k + 1;
countK = countK + 1; countK = countK + 1;
update = true;
} }
// Check whether to update iSAM 2 // Check whether to update iSAM 2
if (update && (k > minK) && (countK > incK)) { if ((k > minK) && (countK > incK)) {
if (batchInitialization) { // Do a full optimize for first minK ranges if (!initialized) { // Do a full optimize for first minK ranges
gttic_(batchInitialization); gttic_(batchInitialization);
LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial); LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
initial = batchOptimizer.optimize(); initial = batchOptimizer.optimize();
gttoc_(batchInitialization); gttoc_(batchInitialization);
batchInitialization = false; // only once initialized = true;
} }
gttic_(update); gttic_(update);
isam.update(newFactors, initial); isam.update(newFactors, initial);
@ -185,7 +195,8 @@ int main(int argc, char** argv) {
countK = 0; countK = 0;
} }
i += 1; i += 1;
} // odometry loop //--------------------------------- odometry loop -----------------------------------------
} // BOOST_FOREACH
gttoc_(iSAM); gttoc_(iSAM);
// Print timings // Print timings

5
matlab/gtsam_examples/RangeISAMExample_plaza.m
View File

@ -27,19 +27,20 @@ import gtsam.*
% Time (sec) X_pose (m) Y_pose (m) % Time (sec) X_pose (m) Y_pose (m)
% TD % TD
% Time (sec) Sender / Antenna ID Receiver Node ID Range (m) % Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
if true % switch between data files if false % switch between data files
datafile = findExampleDataFile('Plaza1_.mat'); datafile = findExampleDataFile('Plaza1_.mat');
headingOffset=0; headingOffset=0;
minK=200; % minimum number of range measurements to process initially minK=200; % minimum number of range measurements to process initially
incK=5; % minimum number of range measurements to process after
else else
datafile = findExampleDataFile('Plaza2_.mat'); datafile = findExampleDataFile('Plaza2_.mat');
headingOffset=pi; headingOffset=pi;
minK=150; % needs less for init minK=150; % needs less for init
incK=25; % minimum number of range measurements to process after
end end
load(datafile) load(datafile)
M=size(DR,1); M=size(DR,1);
K=size(TD,1); K=size(TD,1);
incK=5; % minimum number of range measurements to process after
sigmaR = 100; % range standard deviation sigmaR = 100; % range standard deviation
sigmaInitial = 1; % draw initial landmark guess from Gaussian sigmaInitial = 1; % draw initial landmark guess from Gaussian
useGroundTruth = false; useGroundTruth = false;