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

vISAMexample is now working.

release/4.3a0
Duy-Nguyen Ta 2010-10-21 23:32:51 +00:00
parent 7b414ce6f7
commit 31a080e4bf
5 changed files with 104 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
examples/vSLAMexample/ISAMLoop-inl.h
View File

@ -45,16 +45,14 @@ void ISAMLoop<Values>::update(const Factors& newFactors, const Values& initialVa
}
}
ordering_.print();
newFactors.linearize(linPoint_, ordering_);
cout << "Don linearize!" << endl;
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(newFactors.linearize(linPoint_, ordering_));
cout << "After linearize: " << endl;
BOOST_FOREACH(GaussianFactorGraph::sharedFactor f, *linearizedNewFactors) {
f->print("Linearized factor: ");
}
cout << "Update ISAM: " << endl;
isam.update(*linearizedNewFactors);
}
}

29
examples/vSLAMexample/vISAMexample.cpp
View File

@ -30,10 +30,10 @@ using namespace gtsam::visualSLAM;
using namespace boost;
/* ************************************************************************* */
#define CALIB_FILE "/calib.txt"
#define LANDMARKS_FILE "/landmarks.txt"
#define POSES_FILE "/posesISAM.txt"
#define MEASUREMENTS_FILE "/measurementsISAM.txt"
#define CALIB_FILE "calib.txt"
#define LANDMARKS_FILE "landmarks.txt"
#define POSES_FILE "posesISAM.txt"
#define MEASUREMENTS_FILE "measurementsISAM.txt"
// Base data folder
string g_dataFolder;
@ -78,8 +78,8 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>&
int pose_id, Pose3& pose,
std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib)
{
// Create a graph of newFactors with new measurements
newFactors = shared_ptr<Graph>(new Graph());
for (size_t i= 0; i<measurements.size(); i++)
{
newFactors->addMeasurement(measurements[i].m_p,
@ -89,10 +89,21 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>&
calib);
}
// ... we need priors on the new pose and all new landmarks
newFactors->addPosePrior(pose_id, pose, poseSigma);
for (size_t i= 0; i<measurements.size(); i++)
{
newFactors->addPointPrior(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
}
// Create initial values for all nodes in the newFactors
initialValues = shared_ptr<Values>(new Values());
initialValues->insert(pose_id, pose);
for (size_t i= 0; i<measurements.size(); i++)
{
initialValues->insert( measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark] );
}
}
@ -109,6 +120,7 @@ int main(int argc, char* argv[])
}
g_dataFolder = string(argv[1]);
g_dataFolder += "/";
readAllData();
ISAMLoop<Values> isam(3);
@ -121,12 +133,13 @@ int main(int argc, char* argv[])
i, g_poses[i],
g_measurements[i], measurementSigma, g_calib);
cout << "Add prior pose and measurements of camera " << i << endl;
newFactors->print();
initialValues->print();
// cout << "Add prior pose and measurements of camera " << i << endl;
// newFactors->print();
// initialValues->print();
isam.update(*newFactors, *initialValues);
Values currentEstimate = isam.estimate();
cout << "****************************************************" << endl;
currentEstimate.print("Current estimate: ");
}

2
examples/vSLAMexample/vSFMexample.cpp
View File

@ -118,7 +118,7 @@ int main(int argc, char* argv[])
exit(0);
}
g_dataFolder = string(argv[1]);
g_dataFolder = string(argv[1]) + "/";
readAllData();
// Create a graph using the 2D measurements (features) and the calibration data

12
examples/vSLAMexample/vSLAMutils.cpp
View File

@ -69,7 +69,7 @@ gtsam::Pose3 readPose(const char* Fn)
/* ************************************************************************* */
std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFn)
{
ifstream posesFile((baseFolder+"/"+posesFn).c_str());
ifstream posesFile((baseFolder+posesFn).c_str());
if (!posesFile)
{
cout << "Cannot read all pose file: " << posesFn << endl;
@ -86,7 +86,7 @@ std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::
string poseFileName;
posesFile >> poseFileName;
Pose3 pose = readPose((baseFolder+"/"+poseFileName).c_str());
Pose3 pose = readPose((baseFolder+poseFileName).c_str());
poses[poseId] = pose;
}
@ -136,10 +136,10 @@ std::vector<Feature2D> readFeatures(int pose_id, const char* filename)
/* ************************************************************************* */
std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn)
{
ifstream measurementsFile((baseFolder+"/"+measurementsFn).c_str());
ifstream measurementsFile((baseFolder+measurementsFn).c_str());
if (!measurementsFile)
{
cout << "Cannot read all pose file: " << measurementsFn << endl;
cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
exit(0);
}
int numPoses;
@ -155,7 +155,7 @@ std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const
string featureFileName;
measurementsFile >> featureFileName;
vector<Feature2D> features = readFeatures(poseId, (baseFolder+"/"+featureFileName).c_str());
vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
allFeatures.insert( allFeatures.end(), features.begin(), features.end() );
}
@ -191,7 +191,7 @@ std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string&
ifstream measurementsFile((baseFolder+measurementsFn).c_str());
if (!measurementsFile)
{
cout << "Cannot read all pose file: " << measurementsFn << endl;
cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
exit(0);
}
int numPoses;

137
slam/visualSLAM.h
View File

@ -30,7 +30,7 @@
namespace gtsam { namespace visualSLAM {
/**
/**
* Typedefs that make up the visualSLAM namespace.
*/
typedef TypedSymbol<Pose3,'x'> PoseKey;
@ -42,7 +42,9 @@ namespace gtsam { namespace visualSLAM {
typedef NonlinearEquality<Values, PoseKey> PoseConstraint;
typedef NonlinearEquality<Values, PointKey> PointConstraint;
typedef PriorFactor<Values, PoseKey> PosePrior;
typedef PriorFactor<Values, PoseKey> PosePrior;
typedef PriorFactor<Values, PointKey> PointPrior;
@ -80,10 +82,10 @@ namespace gtsam { namespace visualSLAM {
* @param K the constant calibration
*/
GenericProjectionFactor(const Point2& z,
const SharedGaussian& model, POSK j_pose,
LMK j_landmark, const shared_ptrK& K) :
Base(model, j_pose, j_landmark), z_(z), K_(K) {
}
const SharedGaussian& model, POSK j_pose,
LMK j_landmark, const shared_ptrK& K) :
Base(model, j_pose, j_landmark), z_(z), K_(K) {
}
/**
* print
@ -126,74 +128,83 @@ namespace gtsam { namespace visualSLAM {
typedef GenericProjectionFactor<Values, PointKey, PoseKey> ProjectionFactor;
/**
* Non-linear factor graph for vanilla visual SLAM
*/
class Graph: public NonlinearFactorGraph<Values> {
* Non-linear factor graph for vanilla visual SLAM
*/
class Graph: public NonlinearFactorGraph<Values> {
public:
public:
typedef boost::shared_ptr<Graph> shared_graph;
typedef boost::shared_ptr<Graph> shared_graph;
/** default constructor is empty graph */
Graph() {
}
/** default constructor is empty graph */
Graph() {
}
/** print out graph */
void print(const std::string& s = "") const {
NonlinearFactorGraph<Values>::print(s);
}
/** print out graph */
void print(const std::string& s = "") const {
NonlinearFactorGraph<Values>::print(s);
}
/** equals */
bool equals(const Graph& p, double tol = 1e-9) const {
return NonlinearFactorGraph<Values>::equals(p, tol);
}
/** equals */
bool equals(const Graph& p, double tol = 1e-9) const {
return NonlinearFactorGraph<Values>::equals(p, tol);
}
/**
* Add a measurement
* @param j index of camera
* @param p to which pose to constrain it to
*/
void addMeasurement(const Point2& z, const SharedGaussian& model,
PoseKey i, PointKey j, const shared_ptrK& K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(z, model, i, j, K));
push_back(factor);
}
/**
* Add a measurement
* @param j index of camera
* @param p to which pose to constrain it to
*/
void addMeasurement(const Point2& z, const SharedGaussian& model,
PoseKey i, PointKey j, const shared_ptrK& K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(z, model, i, j, K));
push_back(factor);
}
/**
* Add a constraint on a pose (for now, *must* be satisfied in any Values)
* @param j index of camera
* @param p to which pose to constrain it to
*/
void addPoseConstraint(int j, const Pose3& p = Pose3()) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(j, p));
push_back(factor);
}
/**
* Add a constraint on a pose (for now, *must* be satisfied in any Values)
* @param j index of camera
* @param p to which pose to constrain it to
*/
void addPoseConstraint(int j, const Pose3& p = Pose3()) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(j, p));
push_back(factor);
}
/**
* Add a constraint on a point (for now, *must* be satisfied in any Values)
* @param j index of landmark
* @param p to which point to constrain it to
*/
void addPointConstraint(int j, const Point3& p = Point3()) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(j, p));
push_back(factor);
}
/**
* Add a constraint on a point (for now, *must* be satisfied in any Values)
* @param j index of landmark
* @param p to which point to constrain it to
*/
void addPointConstraint(int j, const Point3& p = Point3()) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(j, p));
push_back(factor);
}
/**
* Add a prior on a pose
* @param j index of camera
* @param p to which pose to constrain it to
* @param model uncertainty model of this prior
*/
void addPosePrior(int j, const Pose3& p = Pose3(), const SharedGaussian& model = noiseModel::Unit::Create(1)) {
boost::shared_ptr<PosePrior> factor(new PosePrior(j, p, model));
push_back(factor);
}
/**
* Add a prior on a pose
* @param j index of camera
* @param p to which pose to constrain it to
* @param model uncertainty model of this prior
*/
void addPosePrior(int j, const Pose3& p = Pose3(), const SharedGaussian& model = noiseModel::Unit::Create(1)) {
boost::shared_ptr<PosePrior> factor(new PosePrior(j, p, model));
push_back(factor);
}
/**
* Add a prior on a landmark
* @param j index of landmark
* @param model uncertainty model of this prior
*/
void addPointPrior(int j, const Point3& p = Point3(), const SharedGaussian& model = noiseModel::Unit::Create(1)) {
boost::shared_ptr<PointPrior> factor(new PointPrior(j, p, model));
push_back(factor);
}
}; // Graph
}; // Graph
// Optimizer
typedef NonlinearOptimizer<Graph, Values> Optimizer;
// Optimizer
typedef NonlinearOptimizer<Graph, Values> Optimizer;
} } // namespaces