diff --git a/cpp/Makefile.am b/cpp/Makefile.am index b43295f22..8542ae17e 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -139,7 +139,7 @@ testSimulated3D_LDADD = libgtsam.la check_PROGRAMS += testSimulated3D # Visual SLAM -sources += CalibratedCamera.cpp SimpleCamera.cpp VSLAMFactor.cpp +sources += CalibratedCamera.cpp SimpleCamera.cpp VSLAMConfig.cpp VSLAMFactor.cpp check_PROGRAMS += testCalibratedCamera testSimpleCamera testVSLAMFactor testCalibratedCamera_SOURCES = testCalibratedCamera.cpp testCalibratedCamera_LDADD = libgtsam.la diff --git a/cpp/VSLAMConfig.cpp b/cpp/VSLAMConfig.cpp new file mode 100644 index 000000000..1e233da44 --- /dev/null +++ b/cpp/VSLAMConfig.cpp @@ -0,0 +1,305 @@ +/** + * @file VSLAMConfig.cpp + * @brief The Config + * @author Alireza Fathi + * @author Carlos Nieto + */ + +#include + +#include +#include + +#include "VSLAMConfig.h" + +using namespace std; + +// trick from some reading group +#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) + +#define SIGMA 1.0 + +namespace gtsam{ + +/* ************************************************************************* */ +// add a delta to a DCVectorConfig +// Very ugly: there are 3D vectors for points, and 12D vectors for poses +// We check the dimension and do the right thing for each. +VectorConfig DCVectorConfig::operator+(const VectorConfig & delta) const { + DCVectorConfig result; + string j; Vector v; + FOREACH_PAIR(j, v, values) { + if (v.size()==3) { + Point3 basePoint(v); + Point3 newPoint(basePoint.exmap( delta[j] )); + result.insert( j, newPoint.vector() ); + } else { + Pose3 basePose(v); + Pose3 newPose(basePose.exmap( delta[j] )); + result.insert( j, newPose.vector() ); + } + } + return result; +} + +/* ************************************************************************* */ +void DCVectorConfig::operator+=(const VectorConfig & delta) { + for (iterator it = values.begin(); it!=values.end(); it++) { + string j = it->first; + Vector &v = it->second; // reference ! + // code below changes the reference in the config in-place !!! + if (v.size()==3) { + Point3 basePoint(v); + Point3 newPoint(basePoint.exmap( delta[j] )); + v = newPoint.vector(); + } else { + Pose3 basePose(v); + Pose3 newPose(basePose.exmap( delta[j] )); + v = newPose.vector(); + } + } +} + +/* ************************************************************************* */ +VSLAMConfig::VSLAMConfig(VectorConfig & fgconfig) { + landmarkPoints.clear(); + cameraPoses.clear(); + + for(map::const_iterator it = fgconfig.begin(); it != fgconfig.end(); it++) + { + string temp = (*it).first; + if(temp[0] == 'x') + { + int cameraNumber = atoi(temp.substr(1,temp.size()-1).c_str()); + Pose3 cameraPose(Pose3((*it).second)); + addCameraPose(cameraNumber, cameraPose); + } + if(temp[0] == 'l') + { + int landmarkNumber = atoi(temp.substr(1,temp.size()-1).c_str()); + Pose3 markerPose(Pose3((*it).second)); + Point3 landmarkPoint = markerPose.translation(); + addLandmarkPoint(landmarkNumber, landmarkPoint); + } + } + +} + + +// Exponential map +// TODO: FD: I think this is horrible +VSLAMConfig VSLAMConfig::exmap(const VectorConfig & delta) const { + + VSLAMConfig newConfig; + + for (map::const_iterator it = delta.begin(); it + != delta.end(); it++) { + string key = it->first; + if (key[0] == 'x') { + int cameraNumber = atoi(key.substr(1, key.size() - 1).c_str()); + Pose3 basePose = cameraPose(cameraNumber); + newConfig.addCameraPose(cameraNumber, basePose.exmap(it->second)); + } + if (key[0] == 'l') { + int landmarkNumber = atoi(key.substr(1, key.size() - 1).c_str()); + Point3 basePoint = landmarkPoint(landmarkNumber); + newConfig.addLandmarkPoint(landmarkNumber, basePoint.exmap(it->second)); + } + } + + return newConfig; +} + +// Not used! +/* ************************************************************************* * +void VSLAMConfig::load(std::string& path, int num_of_frames) +{ + char putatives[200]; + putatives[0] = 0; + sprintf(putatives, "%s/putatives.txt", path.c_str()); + + char poseR[200]; + poseR[0] = 0; + sprintf(poseR, "%s/pose_R_info.txt", path.c_str()); + + char poseXYZ[200]; + poseXYZ[0] = 0; + sprintf(poseXYZ, "%s/pose_xyz_info.txt", path.c_str()); + + std::ifstream putatives_In(path.c_str(), ios::in); + std::ifstream poseR_In(path.c_str(), ios::in); + std::ifstream poseXYZ_In(path.c_str(), ios::in); + + int frameNumber; + int landmarkIndex; + + double landmarkX; + double landmarkY; + double landmarkZ; + + double uLs; + double vLs; + + int cameraIndex; + + double cameraX; + double cameraY; + double cameraZ; + + double pitch; + double yaw; + double roll; + + + if(putatives_In) + putatives_In >> frameNumber >> landmarkIndex >> uLs >> vLs >> landmarkX >> landmarkY >> landmarkZ; + else + { + printf("Unable to load values from putatives\n"); + exit(0); + } + + if(poseR_In) + poseR_In >> yaw >> pitch >> roll; + else + { + printf("Unable to load values from poseR\n"); + exit(0); + } + + if(poseXYZ_In) + poseXYZ_In >> cameraX >> cameraY >> cameraZ; + else + { + printf("Unable to load values from poseXYZ\n"); + exit(0); + } + + char lname [50]; + sprintf (lname, "l%d", frameNumber); + + + landmarkPoints[landmarkIndex] = Point3(landmarkX, landmarkY, landmarkZ); + + Rot3 R = rodriguez(yaw,pitch,roll); + Point3 c (cameraX, cameraY, cameraZ); + + cameraPoses[cameraIndex]= Pose3(R,c); + + putatives_In.close(); + poseR_In.close(); + poseXYZ_In.close(); +} + +/* ************************************************************************* */ +void VSLAMConfig::flush(int referenceMarker, const std::string& path) +{ + + + +} + + +/* ************************************************************************* */ +void VSLAMConfig::print(const std::string& s) const +{ + printf("%s:\n", s.c_str()); + printf("Camera Poses:\n"); + for(PoseMap::const_iterator it = cameraIteratorBegin(); it != cameraIteratorEnd(); it++) + { + printf("x%d:\n", it->first); + it->second.print(); + } + printf("Landmark Points:\n"); + for(PointMap::const_iterator it = landmarkIteratorBegin(); it != landmarkIteratorEnd(); it++) + { + printf("l%d:\n", (*it).first); + (*it).second.print(); + } +} + +/* ************************************************************************* */ +bool VSLAMConfig::equals(const VSLAMConfig& c) { + for(PoseMap::const_iterator it = cameraIteratorBegin(); it != cameraIteratorEnd(); it++) + { + if(!c.cameraPoseExists(it->first)) + { + printf("camera pose %d didn't exist in that\n", it->first); + goto fail; + } + if(!assert_equal(it->second, c.cameraPose(it->first), 1e-6)) + goto fail; + } + + for(PointMap::const_iterator it = landmarkIteratorBegin(); it != landmarkIteratorEnd(); it++) + { + if(!c.landmarkPointExists(it->first)) + { + printf("landmark point %d didn't exist in that\n", it->first); + goto fail; + } + if(!assert_equal(it->second, c.landmarkPoint(it->first), 1e-6)) + goto fail; + } + + return true; + + fail: + print("this"); + c.print("that"); + return false; +} + +/* ************************************************************************* */ +void VSLAMConfig::addCameraPose(const int i, Pose3 cp) +{ + pair camera; + camera.first = i; + camera.second = cp; + cameraPoses.insert(camera); +} +/* ************************************************************************* */ +void VSLAMConfig::addLandmarkPoint(const int i, Point3 lp) +{ + pair landmark; + landmark.first = i; + landmark.second = lp; + landmarkPoints.insert(landmark); +} +/* ************************************************************************* */ +void VSLAMConfig::removeCameraPose(const int i) +{ + if(cameraPoseExists(i)) + cameraPoses.erase(i); +} +/* ************************************************************************* */ +void VSLAMConfig::removeLandmarkPose(const int i) +{ + if(landmarkPointExists(i)) + landmarkPoints.erase(i); +} +/* ************************************************************************* */ +DCVectorConfig VSLAMConfig::getVectorConfig() const +{ + DCVectorConfig cfg; + char buffer[100]; + string stbuf; + int ln; Point3 landmarkPoint; + FOREACH_PAIR(ln, landmarkPoint, landmarkPoints) { + buffer[0] = 0; + sprintf(buffer, "l%d", ln); + stbuf = string(buffer); + cfg.insert(stbuf, landmarkPoint.vector()); // 3D for points + } + int cn; Pose3 cameraPose; + FOREACH_PAIR(cn, cameraPose, cameraPoses) { + buffer[0] = 0; + sprintf(buffer, "x%d", cn); + stbuf = string(buffer); + cfg.insert(stbuf, cameraPose.vector()); // 12D for poses + } + return cfg; +} + +} // namespace gtsam + diff --git a/cpp/VSLAMConfig.h b/cpp/VSLAMConfig.h new file mode 100644 index 000000000..eb1e9811c --- /dev/null +++ b/cpp/VSLAMConfig.h @@ -0,0 +1,155 @@ +/** + * @file VSLAMConfig.h + * @brief Config for VSLAM + * @author Alireza Fathi + * @author Carlos Nieto + */ + +#include +#include +#include +#include +#include "VectorConfig.h" +#include "Pose3.h" +#include "Cal3_S2.h" + +#pragma once + +namespace gtsam{ + +/** + * special VectorConfig derived class that knows we are dealing with Pose3 objects + * should be more elegant in later version of gtsam + */ +class DCVectorConfig : public gtsam::VectorConfig { +public: + gtsam::VectorConfig operator+(const gtsam::VectorConfig & delta) const; + void operator+=(const gtsam::VectorConfig & delta); +}; + +/** + * Config that knows about points and poses + */ +class VSLAMConfig { + + private: + typedef std::map PoseMap; + typedef std::map PointMap; + PointMap landmarkPoints; + PoseMap cameraPoses; + + public: + typedef std::map::const_iterator const_iterator; + /** + * default constructor + */ + VSLAMConfig() {} + + /** + * constructor that loads from file + */ + VSLAMConfig(std::string& path, int num_of_frames) { load(path, num_of_frames);} + + /** + * constructor from an VectorConfig + */ + VSLAMConfig(gtsam::VectorConfig & Vectorconfig); + + /* + * copy constructor + */ + VSLAMConfig(const VSLAMConfig& original): + cameraPoses(original.cameraPoses), landmarkPoints(original.landmarkPoints){} + + /** + * Exponential map: takes 6D vectors in VectorConfig + * and applies them to the poses in the VSLAMConfig. + * Needed for use in nonlinear optimization + */ + VSLAMConfig exmap(const gtsam::VectorConfig & delta) const; + + /** + * get the VectorConfig, poses have names x1,x2, and landmarks l1,l2,... + */ + DCVectorConfig getVectorConfig() const; + + + + /** + * load values from files. + */ + void load(std::string& path, int num_of_frames); + + /** + * flush the poses into files (results), + */ + void flush(int referenceMarker, const std::string& path); + + PoseMap::const_iterator cameraIteratorBegin() const { return cameraPoses.begin();} + PoseMap::const_iterator cameraIteratorEnd() const { return cameraPoses.end();} + PointMap::const_iterator landmarkIteratorBegin() const { return landmarkPoints.begin();} + PointMap::const_iterator landmarkIteratorEnd() const { return landmarkPoints.end();} + + /** + * print + */ + void print(const std::string& s = "") const; + + /** + * Retrieve robot pose + */ + bool cameraPoseExists(int i) const + { + PoseMap::const_iterator it = cameraPoses.find(i); + if (it==cameraPoses.end()) + return false; + return true; + } + + gtsam::Pose3 cameraPose(int i) const { + PoseMap::const_iterator it = cameraPoses.find(i); + if (it==cameraPoses.end()) + throw(std::invalid_argument("robotPose: invalid key")); + return it->second; + } + + /** + * Check whether a landmark point exists + */ + bool landmarkPointExists(int i) const + { + PointMap::const_iterator it = landmarkPoints.find(i); + if (it==landmarkPoints.end()) + return false; + return true; + } + + /** + * Retrieve landmark point + */ + gtsam::Point3 landmarkPoint(int i) const { + PointMap::const_iterator it = landmarkPoints.find(i); + if (it==landmarkPoints.end()) + throw(std::invalid_argument("markerPose: invalid key")); + return it->second; + } + + /** + * check whether two configs are equal + */ + bool equals(const VSLAMConfig& c) ; + void addCameraPose(const int i, gtsam::Pose3 cp); + void addLandmarkPoint(const int i, gtsam::Point3 lp); + + void removeCameraPose(const int i); + void removeLandmarkPose(const int i); + + void clear() {landmarkPoints.clear(); cameraPoses.clear();} + + inline size_t size(){ + return landmarkPoints.size() + cameraPoses.size(); + } +}; + +} // namespace gtsam + diff --git a/cpp/VSLAMFactor.cpp b/cpp/VSLAMFactor.cpp index ddfa9290b..67ed040fa 100644 --- a/cpp/VSLAMFactor.cpp +++ b/cpp/VSLAMFactor.cpp @@ -4,17 +4,17 @@ * @author Alireza Fathi */ -#include "VectorConfig.h" +#include "VSLAMConfig.h" #include "VSLAMFactor.h" #include "Pose3.h" #include "SimpleCamera.h" using namespace std; -using namespace gtsam; +namespace gtsam{ /* ************************************************************************* */ VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K) - : NonlinearFactor(z, sigma) + : NonlinearFactor(z, sigma) { cameraFrameNumber_ = cn; landmarkNumber_ = ln; @@ -31,11 +31,11 @@ VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Ca /* ************************************************************************* */ void VSLAMFactor::print(const std::string& s) const { printf("%s %s %s\n", s.c_str(), cameraFrameName_.c_str(), landmarkName_.c_str()); - ::print(this->z_, s+".z"); + gtsam::print(this->z_, s+".z"); } /* ************************************************************************* */ -bool VSLAMFactor::equals(const NonlinearFactor& f, double tol) const { +bool VSLAMFactor::equals(const NonlinearFactor& f, double tol) const { const VSLAMFactor* p = dynamic_cast(&f); if (p == NULL) return false; if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) return false; @@ -44,25 +44,25 @@ bool VSLAMFactor::equals(const NonlinearFactor& f, double tol) con } /* ************************************************************************* */ -Vector VSLAMFactor::predict(const VectorConfig& c) const { - Pose3 pose = c[cameraFrameName_]; - Point3 landmark = c[landmarkName_]; +Vector VSLAMFactor::predict(const VSLAMConfig& c) const { + Pose3 pose = c.cameraPose(cameraFrameNumber_); + Point3 landmark = c.landmarkPoint(landmarkNumber_); return project(SimpleCamera(K_,pose), landmark).vector(); } /* ************************************************************************* */ -Vector VSLAMFactor::error_vector(const VectorConfig& c) const { +Vector VSLAMFactor::error_vector(const VSLAMConfig& c) const { // Right-hand-side b = (z - h(x))/sigma Vector h = predict(c); return (this->z_ - h); } /* ************************************************************************* */ -LinearFactor::shared_ptr VSLAMFactor::linearize(const VectorConfig& c) const +LinearFactor::shared_ptr VSLAMFactor::linearize(const VSLAMConfig& c) const { // get arguments from config - Pose3 pose = c[cameraFrameName_]; // cast from Vector to Pose3 !!! - Point3 landmark = c[landmarkName_]; // cast from Vector to Point3 !! + Pose3 pose = c.cameraPose(cameraFrameNumber_); + Point3 landmark = c.landmarkPoint(landmarkNumber_); SimpleCamera camera(K_,pose); @@ -81,3 +81,4 @@ LinearFactor::shared_ptr VSLAMFactor::linearize(const VectorConfig& c) const } /* ************************************************************************* */ +} // namespace gtsam diff --git a/cpp/VSLAMFactor.h b/cpp/VSLAMFactor.h index 6c22a8549..b53c48a8e 100644 --- a/cpp/VSLAMFactor.h +++ b/cpp/VSLAMFactor.h @@ -12,20 +12,20 @@ namespace gtsam { -class VectorConfig; +class VSLAMConfig; /** * Non-linear factor for a constraint derived from a 2D measurement, * i.e. the main building block for visual SLAM. */ -class VSLAMFactor : public NonlinearFactor +class VSLAMFactor : public NonlinearFactor { private: int cameraFrameNumber_, landmarkNumber_; std::string cameraFrameName_, landmarkName_; Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this. - typedef gtsam::NonlinearFactor ConvenientFactor; + typedef gtsam::NonlinearFactor ConvenientFactor; public: @@ -51,22 +51,22 @@ class VSLAMFactor : public NonlinearFactor /** * equals */ - bool equals(const NonlinearFactor&, double tol=1e-9) const; + bool equals(const NonlinearFactor&, double tol=1e-9) const; /** * predict the measurement */ - Vector predict(const VectorConfig&) const; + Vector predict(const VSLAMConfig&) const; /** * calculate the error of the factor */ - Vector error_vector(const VectorConfig&) const; + Vector error_vector(const VSLAMConfig&) const; /** * linerarization */ - LinearFactor::shared_ptr linearize(const VectorConfig&) const; + LinearFactor::shared_ptr linearize(const VSLAMConfig&) const; int getCameraFrameNumber() const { return cameraFrameNumber_; } int getLandmarkNumber() const { return landmarkNumber_; } diff --git a/cpp/VSLAMGraph.cpp b/cpp/VSLAMGraph.cpp new file mode 100644 index 000000000..ba08f1de0 --- /dev/null +++ b/cpp/VSLAMGraph.cpp @@ -0,0 +1,100 @@ +/** + * @file VSLAMGraph.h + * @brief A factor graph for the VSLAM problem + * @author Alireza Fathi + * @author Carlos Nieto + */ + +#include +#include +#include + +//#include "VSLAMFactor.h" +#include "VSLAMGraph.h" + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +VSLAMGraph::VSLAMGraph(const std::string& path) +{ + ifstream ifs(path.c_str(), ios::in); + + if(ifs) { + // read calibration K + double fx, fy, s, u0, v0; + ifs >> fx >> fy >> s >> u0 >> v0; + Cal3_S2 K(fx, fy, s, u0, v0); + + // read sigma + double sigma; + ifs >> sigma; + + // read number of frames + int nrFrames; + ifs >> nrFrames; + nFrames = nrFrames; + + // read all frames + for (int i=0;i> nrMeasurements; + // read all measurements in ith frame + for (int k=0;k> j >> u >> v; + Point2 z(u,v); + + // this works + //VSLAMFactor::shared_ptr testing(new VSLAMFactor()); + //factors_.push_back(testing); + + VSLAMFactor::shared_ptr f1(new VSLAMFactor::VSLAMFactor(z.vector(), sigma, i+1, j, K)); + factors_.push_back(f1); + //cout << "Added factor " << i+1 << endl; + // just to keep a record of all the feature id's that have been encountered + // value is unused/useless right now, but could be used to keep count + feat_ids.insert(pair(j,0)); + } + } + } + else { + printf("Unable to load values in %s\n", path.c_str()); + exit(0); + } + + ifs.close(); +} + +/* ************************************************************************* */ +VSLAMGraph::VSLAMGraph(const std::string& path, + int nrFrames, double sigma, + const gtsam::Cal3_S2 &K) +{ + ifstream ifs(path.c_str(), ios::in); + + if(ifs) { + int cameraFrameNumber, landmarkNumber; + double landmarkX, landmarkY, landmarkZ; + double u, v; + ifs >> cameraFrameNumber >> landmarkNumber >> u >> v >> landmarkX >> landmarkY >> landmarkZ; + + //Store the measurements + Vector z(2); + z(0)=u; + z(1)=v; + + //VSLAMFactor::shared_ptr f1(new VSLAMFactor::VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, K)); + //factors_.push_back(f1); + } + else { + printf("Unable to load values in %s\n", path.c_str()); + exit(0); + } + + ifs.close(); +} + +/* ************************************************************************* */ + diff --git a/cpp/VSLAMGraph.h b/cpp/VSLAMGraph.h new file mode 100644 index 000000000..6779aef58 --- /dev/null +++ b/cpp/VSLAMGraph.h @@ -0,0 +1,66 @@ +/** + * @file VSLAMGraph.h + * @brief A factor graph for the VSLAM problem + * @author Alireza Fathi + * @author Carlos Nieto + */ + + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include "VSLAMFactor.h" +#include "VSLAMFactor0.h" +#include "StereoFactor.h" +#include "VSLAMConfig.h" + + +using namespace std; + +/** + * Non-linear factor graph for visual SLAM + */ +class VSLAMGraph : public gtsam::NonlinearFactorGraph{ +private: + int nFrames; + typedef map feat_ids_type; + feat_ids_type feat_ids; + +public: + + /** default constructor is empty graph */ + VSLAMGraph() {} + + /** + * Constructor that loads measurements from file + * @param path to the file + */ + VSLAMGraph(const std::string& path); + + /** + * Constructor that loads from VO file (not tested) + * @param path to the file + * @param nrFrames the number of frames to load + * @return new factor graph + */ + VSLAMGraph(const std::string& path, int nrFrames, double sigma, const gtsam::Cal3_S2& K); + + /** + * print out graph + */ + void print(const std::string& s = "") const { + gtsam::NonlinearFactorGraph::print(s); + } + + void load_dumped(const std::string& path); + + int Get_nFrames(){return nFrames;}; + int Get_nFeat_ids(){return feat_ids.size();}; + feat_ids_type* Get_feat_ids_map(){return &feat_ids;}; +}; diff --git a/cpp/testVSLAMFactor.cpp b/cpp/testVSLAMFactor.cpp index 019067938..d3b58b317 100644 --- a/cpp/testVSLAMFactor.cpp +++ b/cpp/testVSLAMFactor.cpp @@ -4,6 +4,7 @@ #include +#include "VSLAMConfig.h" #include "VSLAMFactor.h" #include "Point3.h" #include "Pose3.h" @@ -32,9 +33,9 @@ TEST( VSLAMFactor, error ) VSLAMFactor factor(z, sigma, cameraFrameNumber, landmarkNumber, K); // For the following configuration, the factor predicts 320,240 - VectorConfig config; - Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert("x1",x1.vector()); - Point3 l1; config.insert("l1",l1.vector()); + VSLAMConfig config; + Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.addCameraPose(1, x1); + Point3 l1; config.addLandmarkPoint(1, l1); CHECK(assert_equal(Vector_(2,320.,240.),factor.predict(config))); // Which yields an error of 3^2/2 = 4.5