From 31a080e4bf132b814681bd5480a6282b42e506f6 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Thu, 21 Oct 2010 23:32:51 +0000 Subject: [PATCH] vISAMexample is now working. --- examples/vSLAMexample/ISAMLoop-inl.h | 6 +- examples/vSLAMexample/vISAMexample.cpp | 29 ++++-- examples/vSLAMexample/vSFMexample.cpp | 2 +- examples/vSLAMexample/vSLAMutils.cpp | 12 +-- slam/visualSLAM.h | 137 +++++++++++++------------ 5 files changed, 104 insertions(+), 82 deletions(-) diff --git a/examples/vSLAMexample/ISAMLoop-inl.h b/examples/vSLAMexample/ISAMLoop-inl.h index d72ad0322..17e76070c 100644 --- a/examples/vSLAMexample/ISAMLoop-inl.h +++ b/examples/vSLAMexample/ISAMLoop-inl.h @@ -45,16 +45,14 @@ void ISAMLoop::update(const Factors& newFactors, const Values& initialVa } } - ordering_.print(); - newFactors.linearize(linPoint_, ordering_); - cout << "Don linearize!" << endl; - boost::shared_ptr 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); } } diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index 8e466c5b9..fb3fe519b 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -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& newFactors, boost::shared_ptr& int pose_id, Pose3& pose, std::vector& measurements, SharedGaussian measurementSigma, shared_ptrK calib) { + // Create a graph of newFactors with new measurements newFactors = shared_ptr(new Graph()); - for (size_t i= 0; iaddMeasurement(measurements[i].m_p, @@ -89,10 +89,21 @@ void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& calib); } + // ... we need priors on the new pose and all new landmarks newFactors->addPosePrior(pose_id, pose, poseSigma); + for (size_t i= 0; iaddPointPrior(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]); + } + + // Create initial values for all nodes in the newFactors initialValues = shared_ptr(new Values()); initialValues->insert(pose_id, pose); + for (size_t i= 0; iinsert( 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 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: "); } diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp index b1d9c0144..33cc38380 100644 --- a/examples/vSLAMexample/vSFMexample.cpp +++ b/examples/vSLAMexample/vSFMexample.cpp @@ -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 diff --git a/examples/vSLAMexample/vSLAMutils.cpp b/examples/vSLAMexample/vSLAMutils.cpp index a62bb613d..201541164 100644 --- a/examples/vSLAMexample/vSLAMutils.cpp +++ b/examples/vSLAMexample/vSLAMutils.cpp @@ -69,7 +69,7 @@ gtsam::Pose3 readPose(const char* Fn) /* ************************************************************************* */ std::map 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 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 readFeatures(int pose_id, const char* filename) /* ************************************************************************* */ std::vector 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 readAllMeasurements(const std::string& baseFolder, const string featureFileName; measurementsFile >> featureFileName; - vector features = readFeatures(poseId, (baseFolder+"/"+featureFileName).c_str()); + vector features = readFeatures(poseId, (baseFolder+featureFileName).c_str()); allFeatures.insert( allFeatures.end(), features.begin(), features.end() ); } @@ -191,7 +191,7 @@ std::vector > 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; diff --git a/slam/visualSLAM.h b/slam/visualSLAM.h index 9c57a7dad..aec8ca338 100644 --- a/slam/visualSLAM.h +++ b/slam/visualSLAM.h @@ -30,7 +30,7 @@ namespace gtsam { namespace visualSLAM { - /** + /** * Typedefs that make up the visualSLAM namespace. */ typedef TypedSymbol PoseKey; @@ -42,7 +42,9 @@ namespace gtsam { namespace visualSLAM { typedef NonlinearEquality PoseConstraint; typedef NonlinearEquality PointConstraint; - typedef PriorFactor PosePrior; + typedef PriorFactor PosePrior; + typedef PriorFactor 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 ProjectionFactor; /** - * Non-linear factor graph for vanilla visual SLAM - */ - class Graph: public NonlinearFactorGraph { + * Non-linear factor graph for vanilla visual SLAM + */ + class Graph: public NonlinearFactorGraph { - public: + public: - typedef boost::shared_ptr shared_graph; + typedef boost::shared_ptr 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::print(s); - } + /** print out graph */ + void print(const std::string& s = "") const { + NonlinearFactorGraph::print(s); + } - /** equals */ - bool equals(const Graph& p, double tol = 1e-9) const { - return NonlinearFactorGraph::equals(p, tol); - } + /** equals */ + bool equals(const Graph& p, double tol = 1e-9) const { + return NonlinearFactorGraph::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 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 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 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 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 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 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 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 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 factor(new PointPrior(j, p, model)); + push_back(factor); + } - }; // Graph + }; // Graph - // Optimizer - typedef NonlinearOptimizer Optimizer; + // Optimizer + typedef NonlinearOptimizer Optimizer; } } // namespaces