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

Fix vSLAMexample compilation error. Update to new Optimizer interface. 

Fix data reading interface. Now the program requires a Data folder to be specified.
release/4.3a0
Duy-Nguyen Ta 2010-10-20 18:50:41 +00:00
parent 27ea8b2c6a
commit fa56595650
8 changed files with 182 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/Makefile.am
View File

@ -16,7 +16,7 @@ noinst_PROGRAMS += PlanarSLAMExample # Solves SLAM example from tutorial by us
noinst_PROGRAMS += Pose2SLAMExample # Solves SLAM example from tutorial by using planarSLAM
noinst_PROGRAMS += PlanarSLAMSelfContained # Solves SLAM example from tutorial with all typedefs in the script
noinst_PROGRAMS += Pose2SLAMwSPCG # Solves a simple SLAM example with SPCG solver
#SUBDIRS = vSLAMexample # does not compile....
SUBDIRS = vSLAMexample # does not compile....
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------

12
examples/vSLAMexample/Data/measurements.txt Normal file
View File

@ -0,0 +1,12 @@
10
1 ttpy10.feat
2 ttpy20.feat
3 ttpy30.feat
4 ttpy40.feat
5 ttpy50.feat
6 ttpy60.feat
7 ttpy70.feat
8 ttpy80.feat
9 ttpy90.feat
10 ttpy100.feat

12
examples/vSLAMexample/Data/poses.txt Normal file
View File

@ -0,0 +1,12 @@
10
1 ttpy10.pose
2 ttpy20.pose
3 ttpy30.pose
4 ttpy40.pose
5 ttpy50.pose
6 ttpy60.pose
7 ttpy70.pose
8 ttpy80.pose
9 ttpy90.pose
10 ttpy100.pose

13
examples/vSLAMexample/Feature2D.h
View File

@ -8,15 +8,20 @@ class Feature2D
{
public:
gtsam::Point2 m_p;
int m_id; // id of the 3D landmark that it is associated with
int m_idCamera; // id of the camera pose that makes this measurement
int m_idLandmark; // id of the 3D landmark that it is associated with
public:
Feature2D(int id, gtsam::Point2 p):m_id(id), m_p(p) {};
Feature2D(int idCamera, int idLandmark, gtsam::Point2 p)
:m_idCamera(idCamera),
m_idLandmark(idLandmark),
m_p(p)
{};
void print(const std::string& s = "") const
{
std::cout << s << std::endl;
std::cout << "Id: " << m_id << std::endl;
m_p.print();
std::cout << "Pose id: " << m_idCamera << " -- Landmark id: " << m_idLandmark << std::endl;
m_p.print("\tMeasurement: ");
}
};

4
examples/vSLAMexample/Makefile.am
View File

@ -17,10 +17,12 @@ vSLAMexample_SOURCES = vSLAMexample.cpp Feature2D.cpp landmarkUtils.cpp
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------
AM_CPPFLAGS = -I$(SparseInc) -I$(top_srcdir)/..
AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(CCOLAMDInc) -I$(top_srcdir)/..
AM_LDFLAGS = $(BOOST_LDFLAGS)
LDADD = ../../libgtsam.la
AM_DEFAULT_SOURCE_EXT = .cpp
# rule to run an executable
%.run: % $(LDADD)
./$^

82
examples/vSLAMexample/landmarkUtils.cpp
View File

@ -6,9 +6,9 @@ using namespace gtsam;
using namespace std;
/* ************************************************************************* */
bool readLandMarks(const char* landmarkFile, std::map<int, Point3>& landmarks)
{
ifstream file(landmarkFile);
std::map<int, Point3> readLandMarks(const std::string& landmarkFile)
{
ifstream file(landmarkFile.c_str());
if (!file) {
cout << "Cannot read landmark file: " << landmarkFile << endl;
exit(0);
@ -16,6 +16,7 @@ bool readLandMarks(const char* landmarkFile, std::map<int, Point3>& landmarks)
int num;
file >> num;
std::map<int, Point3> landmarks;
landmarks.clear();
for (int i = 0; i<num; i++)
{
@ -26,7 +27,7 @@ bool readLandMarks(const char* landmarkFile, std::map<int, Point3>& landmarks)
}
file.close();
return true;
return landmarks;
}
/* ************************************************************************* */
@ -66,17 +67,36 @@ gtsam::Pose3 readPose(const char* Fn)
return pose;
}
/* ************************************************************************* */
gtsam::Pose3 readPose(const char* poseFn_pre, const char* poseFn_suf, int poseId)
std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFn)
{
char poseFn[128];
sprintf(poseFn, "%s%d%s", poseFn_pre, poseId, poseFn_suf);
return readPose(poseFn);
ifstream posesFile((baseFolder+posesFn).c_str());
if (!posesFile)
{
cout << "Cannot read all pose file: " << posesFn << endl;
exit(0);
}
int numPoses;
posesFile >> numPoses;
map<int, Pose3> poses;
for (int i = 0; i<numPoses; i++)
{
int poseId;
posesFile >> poseId;
string poseFileName;
posesFile >> poseFileName;
Pose3 pose = readPose((baseFolder+poseFileName).c_str());
poses[poseId] = pose;
}
return poses;
}
/* ************************************************************************* */
gtsam::Cal3_S2 readCalibData(const char* calibFn)
gtsam::shared_ptrK readCalibData(const std::string& calibFn)
{
ifstream calibFile(calibFn);
ifstream calibFile(calibFn.c_str());
if (!calibFile)
{
cout << "Cannot read calib file: " << calibFn << endl;
@ -87,11 +107,10 @@ gtsam::Cal3_S2 readCalibData(const char* calibFn)
calibFile >> imX >> imY >> fx >> fy >> ox >> oy;
calibFile.close();
Cal3_S2 K(fx, fy, 0, ox, oy); // skew factor = 0
return K;
return shared_ptrK(new Cal3_S2(fx, fy, 0, ox, oy)); // skew factor = 0
}
/* ************************************************************************* */
std::vector<Feature2D> readFeatures(const char* filename)
std::vector<Feature2D> readFeatures(int pose_id, const char* filename)
{
ifstream file(filename);
if (!file)
@ -106,18 +125,39 @@ std::vector<Feature2D> readFeatures(const char* filename)
std::vector<Feature2D> vFeatures_;
for (size_t i = 0; i < numFeatures; i++)
{
int id; double x, y;
file >> id >> x >> y;
vFeatures_.push_back(Feature2D(id, Point2(x, y)));
int landmark_id; double x, y;
file >> landmark_id >> x >> y;
vFeatures_.push_back(Feature2D(pose_id, landmark_id, Point2(x, y)));
}
file.close();
return vFeatures_;
}
/* ************************************************************************* */
std::vector<Feature2D> readFeatures(const char* featFn_pre, const char* featFn_suf, int imageId)
{
char featFn[128];
sprintf(featFn, "%s%d%s", featFn_pre, imageId, featFn_suf);
return readFeatures(featFn);
std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn)
{;
ifstream measurementsFile((baseFolder+measurementsFn).c_str());
if (!measurementsFile)
{
cout << "Cannot read all pose file: " << measurementsFn << endl;
exit(0);
}
int numPoses;
measurementsFile >> numPoses;
vector<Feature2D> allFeatures;
allFeatures.clear();
for (int i = 0; i<numPoses; i++)
{
int poseId;
measurementsFile >> poseId;
string featureFileName;
measurementsFile >> featureFileName;
vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
allFeatures.insert( allFeatures.end(), features.begin(), features.end() );
}
return allFeatures;
}

10
examples/vSLAMexample/landmarkUtils.h
View File

@ -9,15 +9,15 @@
#include "gtsam/geometry/Cal3_S2.h"
bool readLandMarks(const char* landmarkFile, std::map<int, gtsam::Point3>& landmarks);
std::map<int, gtsam::Point3> readLandMarks(const std::string& landmarkFile);
gtsam::Pose3 readPose(const char* poseFn);
gtsam::Pose3 readPose(const char* poseFn_pre, const char* poseFn_suf, int poseId);
std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFN);
gtsam::Cal3_S2 readCalibData(const char* calibFn);
gtsam::shared_ptrK readCalibData(const std::string& calibFn);
std::vector<Feature2D> readFeatures(const char* filename);
std::vector<Feature2D> readFeatures(const char* featFn_pre, const char* featFn_suf, int imageId);
std::vector<Feature2D> readFeatureFile(const char* filename);
std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn);
#endif // LANDMARKUTILS_H

143
examples/vSLAMexample/vSLAMexample.cpp
View File

@ -5,7 +5,6 @@
* @author Duy-Nguyen
*/
#include <gtsam/CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
using namespace boost;
@ -26,39 +25,62 @@ using namespace gtsam::visualSLAM;
using namespace boost;
/* ************************************************************************* */
#define CALIB_FILE "Data/calib.txt"
#define LANDMARKS_FILE "Data/landmarks.txt"
#define CALIB_FILE "calib.txt"
#define LANDMARKS_FILE "landmarks.txt"
#define POSES_FILE "poses.txt"
#define MEASUREMENTS_FILE "measurements.txt"
#define POSEFN_PREFIX "Data/ttpy"
#define POSEFN_SUFFIX ".pose"
#define FEATFN_PREFIX "Data/ttpy"
#define FEATFN_SUFFIX ".feat"
// Base data folder
string g_dataFolder;
#define NUM_IMAGES 10
const int ImageIds[NUM_IMAGES] = {10,20,30,40,50,60,70,80,90,100};
// Store groundtruth values, read from files
shared_ptrK g_calib;
map<int, Point3> g_landmarks; // map: <landmark_id, landmark_position>
map<int, Pose3> g_poses; // map: <camera_id, pose>
std::vector<Feature2D> g_measurements; // Feature2D: {camera_id, landmark_id, 2d feature_position}
// Store groundtruth values
map<int, Point3> g_landmarks;
vector<Pose3> g_poses;
// Noise models
SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
/* ************************************************************************* */
/**
* Read all data: calibration file, landmarks, poses, and all features measurements
* Data is stored in global variables.
*/
void readAllData()
{
g_calib = readCalibData(g_dataFolder + CALIB_FILE);
// Read groundtruth landmarks' positions. These will be used later as intial estimates for landmark nodes.
g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE);
// Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
g_poses = readPoses(g_dataFolder, POSES_FILE);
// Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
g_measurements = readAllMeasurements(g_dataFolder, MEASUREMENTS_FILE);
}
/* ************************************************************************* */
/**
* Setup vSLAM graph
* by adding and associating 2D features (measurements) detected in each image
* by adding and linking 2D features (measurements) detected in each captured image
* with their corresponding landmarks.
*/
Graph setupGraph()
Graph setupGraph(std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib)
{
Graph g;
shared_ptrK sK(new Cal3_S2(readCalibData(CALIB_FILE)));
sK->print("Calibration: ");
SharedGaussian sigma(noiseModel::Isotropic::Sigma(2,5.0f));
Graph g;
for (size_t i= 0; i<NUM_IMAGES; i++)
cout << "Built graph: " << endl;
for (size_t i= 0; i<measurements.size(); i++)
{
std::vector<Feature2D> features = readFeatures(FEATFN_PREFIX, FEATFN_SUFFIX, ImageIds[i]);
for (size_t j = 0; j<features.size(); j++)
g.addMeasurement(features[j].m_p, sigma, i, features[j].m_id, sK);
measurements[i].print();
g.addMeasurement(measurements[i].m_p,
measurementSigma,
measurements[i].m_idCamera,
measurements[i].m_idLandmark,
calib);
}
return g;
@ -66,69 +88,62 @@ Graph setupGraph()
/* ************************************************************************* */
/**
* Read initial values.
* Note: These are ground-truth values, but we just use them as initial estimates.
* Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
* The returned Values structure contains all initial values for all nodes.
*/
Values initializeValues()
Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses)
{
Values initValues;
// Initialize landmarks 3D positions.
for (map<int, Point3>::iterator lmit = g_landmarks.begin(); lmit != g_landmarks.end(); lmit++)
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
initValues.insert( lmit->first, lmit->second );
// Initialize camera poses.
for (int i = 0; i<NUM_IMAGES; i++)
initValues.insert(i, g_poses[i]);
for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
initValues.insert( poseit->first, poseit->second);
return initValues;
}
/* ************************************************************************* */
int main()
int main(int argc, char* argv[])
{
shared_ptr<Graph> graph(new Graph(setupGraph()));
if (argc <2)
{
cout << "Usage: vSLAMexample <DataFolder>" << endl << endl;
cout << "\tPlease specify <DataFolder>, which contains calibration file, initial landmarks, initial poses, and feature data." << endl;
cout << "\tSample folder is in $gtsam_source_folder$/examples/vSLAMexample/Data" << endl << endl;
cout << "Example usage: vSLAMexample '$gtsam_source_folder$/examples/vSLAMexample/Data'" << endl;
exit(0);
}
// Read groundtruth landmarks' positions and poses. These will also be used later as intial estimates.
readLandMarks(LANDMARKS_FILE, g_landmarks);
for (int i = 0; i<NUM_IMAGES; i++)
{
Pose3 pose = readPose(POSEFN_PREFIX, POSEFN_SUFFIX, ImageIds[i]) ;
g_poses.push_back( pose );
}
g_dataFolder = string(argv[1]);
readAllData();
// Create an initial Values structure using groundtruth as the initial estimates
boost::shared_ptr<Values> initialValues(new Values(initializeValues()));
// Create a graph using the 2D measurements (features) and calibration data
shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
// Create an initial Values structure using groundtruth values as the initial estimates
boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
cout << "*******************************************************" << endl;
initialEstimates->print("INITIAL ESTIMATES: ");
// Add hard constraint on the first pose, used as fixed prior.
graph->addPoseConstraint(0, g_poses[0]);
graph->addPoseConstraint(g_poses.begin()->first, g_poses.begin()->second);
// Create an ordering of the variables
shared_ptr<Ordering> ordering(new Ordering);
char name[4];
for (size_t i = 0; i<g_landmarks.size(); i++)
{
sprintf(name, "l%d", i); // "li"
*ordering += name;
}
// Add prior factor for all poses in the graph
// for (map<int, Pose3>::iterator poseit = g_poses.begin(); poseit != g_poses.end(); poseit++)
// graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(10));
for (size_t i = 0; i<NUM_IMAGES; i++)
{
sprintf(name, "x%d", i); // "xj"
*ordering += name;
}
// Optimize the graph
cout << "*******************************************************" << endl;
Optimizer::Parameters::verbosityLevel verborsity = Optimizer::Parameters::DAMPED;
Optimizer::shared_values result = Optimizer::optimizeLM( graph, initialEstimates, verborsity );
// Create an optimizer and check its error
// We expect the initial to be zero because Values is the ground truth
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
Optimizer optimizer(graph, initialValues, solver);
cout << "Initial error: " << optimizer.error() << endl;
optimizer.config()->print("Initial estimates: ");
// Optimize the graph.
Optimizer::verbosityLevel verborsity = Optimizer::ERROR;
Optimizer optimResult = optimizer.levenbergMarquardt(1e-5, 1e-5, verborsity);
optimResult.config()->print("After optimization: ");
// Print final results
cout << "*******************************************************" << endl;
result->print("FINAL RESULTS: ");
}
/* ************************************************************************* */