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

cleaned up but for some reason it segfaults in Release, as if not linked with correct dataset code...

release/4.3a0
Frank Dellaert 2013-10-18 06:04:48 +00:00
parent ae95d2fa2a
commit f6d4da5d40
2 changed files with 26 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -687,6 +687,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testDataset.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDataset.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>

45
examples/SFMExample_bal.cpp
View File

@ -26,6 +26,8 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using symbol_shorthand::C;
using symbol_shorthand::P;
// We will be using a projection factor that ties a SFM_Camera to a 3D point. // We will be using a projection factor that ties a SFM_Camera to a 3D point.
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration // An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
@ -35,53 +37,42 @@ typedef GeneralSFMFactor<SfM_Camera,Point3> MyFactor;
/* ************************************************************************* */ /* ************************************************************************* */
int main (int argc, char* argv[]) { int main (int argc, char* argv[]) {
// default file // Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre"); string filename = findExampleDataFile("dubrovnik-3-7-pre");
// If an argument is given, try loading a file
if (argc>1) filename = string(argv[1]); if (argc>1) filename = string(argv[1]);
///< The structure where we will save the SfM data // Load the SfM data from file
SfM_data mydata; SfM_data mydata; assert(readBAL(filename, mydata));
assert(readBAL(filename, mydata)); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph // Create a factor graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
// Define the camera observation noise model // We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::shared_ptr noise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Add measurements to the factor graph // Add measurements to the factor graph
size_t j = 0; size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
BOOST_FOREACH(const SfM_Measurement& measurement, track.measurements) { BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
size_t i; Point2 uv; size_t i = m.first;
boost::tie(i, uv) = measurement; Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, Symbol('x', i), Symbol('p', j))); graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
} }
j += 1; j += 1;
} }
// Add a prior on pose x1. This indirectly specifies where the origin is. // Add a prior on pose x1. This indirectly specifies where the origin is.
graph.push_back( // and a prior on the position of the first landmark to fix the scale
PriorFactor<SfM_Camera>(Symbol('x', 0), mydata.cameras[0], graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
noiseModel::Isotropic::Sigma(9, 0.1))); graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
// Add a prior on the position of the first landmark to fix the scale // Create initial estimate
graph.push_back(
PriorFactor<Point3>(Symbol('p', 0), mydata.tracks[0].p,
noiseModel::Isotropic::Sigma(3, 0.1)));
// Create the data structure to hold the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth
Values initial; Values initial;
size_t i = 0; size_t i = 0; j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
initial.insert(Symbol('x', i++), camera); BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p);
j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks)
initial.insert(Symbol('p', j++), track.p);
/* Optimize the graph and print results */ /* Optimize the graph and print results */
Values result; Values result;