From ac840d6f0a6f9e452120977e23ef0139940b3ead Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 Nov 2009 05:14:03 +0000 Subject: [PATCH] 1) DCVectorConfig I believe was obsolete as well as any conversion from and to Vectors, which I removed 2) load and flush are gone. 3) I made a VSLAMConfig "Testable". 4) I added an explicit template instantiation to avoid having to include -inl.h files 5) I added some more test code in testVSLAMFactor --- cpp/VSLAMConfig.cpp | 230 ++++------------------------------------ cpp/VSLAMConfig.h | 90 +++++----------- cpp/VSLAMGraph.cpp | 7 ++ cpp/testVSLAMFactor.cpp | 28 ++++- 4 files changed, 79 insertions(+), 276 deletions(-) diff --git a/cpp/VSLAMConfig.cpp b/cpp/VSLAMConfig.cpp index 1e233da44..ab26e43d9 100644 --- a/cpp/VSLAMConfig.cpp +++ b/cpp/VSLAMConfig.cpp @@ -19,75 +19,10 @@ using namespace std; #define SIGMA 1.0 -namespace gtsam{ +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; @@ -110,96 +45,6 @@ VSLAMConfig VSLAMConfig::exmap(const VectorConfig & delta) const { 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 { @@ -219,35 +64,19 @@ void VSLAMConfig::print(const std::string& s) const } /* ************************************************************************* */ -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; - } +bool VSLAMConfig::equals(const VSLAMConfig& c, double tol) const { + for (PoseMap::const_iterator it = cameraIteratorBegin(); it + != cameraIteratorEnd(); it++) { + if (!c.cameraPoseExists(it->first)) return false; + if (!it->second.equals(c.cameraPose(it->first), tol)) return false; + } - 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; + for (PointMap::const_iterator it = landmarkIteratorBegin(); it + != landmarkIteratorEnd(); it++) { + if (!c.landmarkPointExists(it->first)) return false; + if (!it->second.equals(c.landmarkPoint(it->first), tol)) return false; + } + return true; } /* ************************************************************************* */ @@ -256,50 +85,33 @@ void VSLAMConfig::addCameraPose(const int i, Pose3 cp) pair camera; camera.first = i; camera.second = cp; - cameraPoses.insert(camera); + cameraPoses_.insert(camera); } + /* ************************************************************************* */ void VSLAMConfig::addLandmarkPoint(const int i, Point3 lp) { pair landmark; landmark.first = i; landmark.second = lp; - landmarkPoints.insert(landmark); + landmarkPoints_.insert(landmark); } + /* ************************************************************************* */ void VSLAMConfig::removeCameraPose(const int i) { if(cameraPoseExists(i)) - cameraPoses.erase(i); + cameraPoses_.erase(i); } + /* ************************************************************************* */ void VSLAMConfig::removeLandmarkPose(const int i) { if(landmarkPointExists(i)) - landmarkPoints.erase(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 index eb1e9811c..d9601d5da 100644 --- a/cpp/VSLAMConfig.h +++ b/cpp/VSLAMConfig.h @@ -12,31 +12,22 @@ #include "VectorConfig.h" #include "Pose3.h" #include "Cal3_S2.h" +#include "Testable.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 { +class VSLAMConfig : Testable { private: - typedef std::map PoseMap; - typedef std::map PointMap; - PointMap landmarkPoints; - PoseMap cameraPoses; + typedef std::map PoseMap; + typedef std::map PointMap; + PointMap landmarkPoints_; + PoseMap cameraPoses_; public: typedef std::map::const_iterator const_iterator; @@ -45,50 +36,23 @@ class VSLAMConfig { */ 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){} + 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; + VSLAMConfig exmap(const 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();} + 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 @@ -100,15 +64,15 @@ class VSLAMConfig { */ bool cameraPoseExists(int i) const { - PoseMap::const_iterator it = cameraPoses.find(i); - if (it==cameraPoses.end()) + 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()) + 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; } @@ -118,8 +82,8 @@ class VSLAMConfig { */ bool landmarkPointExists(int i) const { - PointMap::const_iterator it = landmarkPoints.find(i); - if (it==landmarkPoints.end()) + PointMap::const_iterator it = landmarkPoints_.find(i); + if (it==landmarkPoints_.end()) return false; return true; } @@ -127,9 +91,9 @@ class VSLAMConfig { /** * Retrieve landmark point */ - gtsam::Point3 landmarkPoint(int i) const { - PointMap::const_iterator it = landmarkPoints.find(i); - if (it==landmarkPoints.end()) + 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; } @@ -137,17 +101,17 @@ class VSLAMConfig { /** * 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); + bool equals(const VSLAMConfig& c, double tol=1e-6) const; + void addCameraPose(const int i, Pose3 cp); + void addLandmarkPoint(const int i, Point3 lp); void removeCameraPose(const int i); void removeLandmarkPose(const int i); - void clear() {landmarkPoints.clear(); cameraPoses.clear();} + void clear() {landmarkPoints_.clear(); cameraPoses_.clear();} inline size_t size(){ - return landmarkPoints.size() + cameraPoses.size(); + return landmarkPoints_.size() + cameraPoses_.size(); } }; diff --git a/cpp/VSLAMGraph.cpp b/cpp/VSLAMGraph.cpp index f0cd22903..196680ca4 100644 --- a/cpp/VSLAMGraph.cpp +++ b/cpp/VSLAMGraph.cpp @@ -10,10 +10,17 @@ #include #include "VSLAMGraph.h" +#include "NonlinearFactorGraph-inl.h" +#include "NonlinearOptimizer-inl.h" using namespace std; namespace gtsam{ +// explicit instantiation so all the code is there and we can link with it +template class FactorGraph; +template class NonlinearFactorGraph; +template class NonlinearOptimizer; + /* ************************************************************************* */ //TODO: CB: This constructor is specific to loading VO data. Should probably // get rid of this. diff --git a/cpp/testVSLAMFactor.cpp b/cpp/testVSLAMFactor.cpp index d3b58b317..f8b492751 100644 --- a/cpp/testVSLAMFactor.cpp +++ b/cpp/testVSLAMFactor.cpp @@ -6,6 +6,7 @@ #include "VSLAMConfig.h" #include "VSLAMFactor.h" +#include "VSLAMGraph.h" #include "Point3.h" #include "Pose3.h" @@ -30,24 +31,43 @@ TEST( VSLAMFactor, error ) Vector z = Vector_(2,323.,240.); double sigma=1.0; int cameraFrameNumber=1, landmarkNumber=1; - VSLAMFactor factor(z, sigma, cameraFrameNumber, landmarkNumber, K); + boost::shared_ptr + factor(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, K)); // For the following configuration, the factor predicts 320,240 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))); + CHECK(assert_equal(Vector_(2,320.,240.),factor->predict(config))); // Which yields an error of 3^2/2 = 4.5 - DOUBLES_EQUAL(4.5,factor.error(config),1e-9); + DOUBLES_EQUAL(4.5,factor->error(config),1e-9); // Check linearize Matrix Al1 = Matrix_(2, 3, 61.584, 0., 0., 0., 61.584, 0.); Matrix Ax1 = Matrix_(2, 6, 0., -369.504, 0., -61.584, 0., 0., 369.504, 0., 0., 0., -61.584, 0.); Vector b = Vector_(2,3.,0.); LinearFactor expected("l1", Al1, "x1", Ax1, b, 1); - LinearFactor::shared_ptr actual = factor.linearize(config); + LinearFactor::shared_ptr actual = factor->linearize(config); CHECK(assert_equal(expected,*actual,1e-3)); + + // linearize graph + VSLAMGraph graph; + graph.push_back(factor); + LinearFactorGraph expected_lfg; + expected_lfg.push_back(actual); + LinearFactorGraph actual_lfg = graph.linearize(config); + CHECK(assert_equal(expected_lfg,actual_lfg)); + + // exmap on a config + VectorConfig delta; + delta.insert("x1",Vector_(6, 0.,0.,0., 1.,1.,1.)); + delta.insert("l1",Vector_(3, 1.,2.,3.)); + VSLAMConfig actual_config = config.exmap(delta); + VSLAMConfig expected_config; + Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.addCameraPose(1, x2); + Point3 l2(1,2,3); expected_config.addLandmarkPoint(1, l2); + CHECK(assert_equal(expected_config,actual_config,1e-9)); } /* ************************************************************************* */