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

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
release/4.3a0
Frank Dellaert 2009-11-11 05:14:03 +00:00
parent 00ff066b33
commit ac840d6f0a
4 changed files with 79 additions and 276 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

230
cpp/VSLAMConfig.cpp
View File

@ -19,75 +19,10 @@ using namespace std;
#define SIGMA 1.0 #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<std::string, Vector>::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 // Exponential map
// TODO: FD: I think this is horrible
VSLAMConfig VSLAMConfig::exmap(const VectorConfig & delta) const { VSLAMConfig VSLAMConfig::exmap(const VectorConfig & delta) const {
VSLAMConfig newConfig; VSLAMConfig newConfig;
@ -110,96 +45,6 @@ VSLAMConfig VSLAMConfig::exmap(const VectorConfig & delta) const {
return newConfig; 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 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) { bool VSLAMConfig::equals(const VSLAMConfig& c, double tol) const {
for(PoseMap::const_iterator it = cameraIteratorBegin(); it != cameraIteratorEnd(); it++) for (PoseMap::const_iterator it = cameraIteratorBegin(); it
{ != cameraIteratorEnd(); it++) {
if(!c.cameraPoseExists(it->first)) if (!c.cameraPoseExists(it->first)) return false;
{ if (!it->second.equals(c.cameraPose(it->first), tol)) return false;
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++) for (PointMap::const_iterator it = landmarkIteratorBegin(); it
{ != landmarkIteratorEnd(); it++) {
if(!c.landmarkPointExists(it->first)) if (!c.landmarkPointExists(it->first)) return false;
{ if (!it->second.equals(c.landmarkPoint(it->first), tol)) return false;
printf("landmark point %d didn't exist in that\n", it->first); }
goto fail; return true;
}
if(!assert_equal(it->second, c.landmarkPoint(it->first), 1e-6))
goto fail;
}
return true;
fail:
print("this");
c.print("that");
return false;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -256,50 +85,33 @@ void VSLAMConfig::addCameraPose(const int i, Pose3 cp)
pair<int, Pose3> camera; pair<int, Pose3> camera;
camera.first = i; camera.first = i;
camera.second = cp; camera.second = cp;
cameraPoses.insert(camera); cameraPoses_.insert(camera);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VSLAMConfig::addLandmarkPoint(const int i, Point3 lp) void VSLAMConfig::addLandmarkPoint(const int i, Point3 lp)
{ {
pair<int, Point3> landmark; pair<int, Point3> landmark;
landmark.first = i; landmark.first = i;
landmark.second = lp; landmark.second = lp;
landmarkPoints.insert(landmark); landmarkPoints_.insert(landmark);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VSLAMConfig::removeCameraPose(const int i) void VSLAMConfig::removeCameraPose(const int i)
{ {
if(cameraPoseExists(i)) if(cameraPoseExists(i))
cameraPoses.erase(i); cameraPoses_.erase(i);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VSLAMConfig::removeLandmarkPose(const int i) void VSLAMConfig::removeLandmarkPose(const int i)
{ {
if(landmarkPointExists(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 } // namespace gtsam

90
cpp/VSLAMConfig.h
View File

@ -12,31 +12,22 @@
#include "VectorConfig.h" #include "VectorConfig.h"
#include "Pose3.h" #include "Pose3.h"
#include "Cal3_S2.h" #include "Cal3_S2.h"
#include "Testable.h"
#pragma once #pragma once
namespace gtsam{ 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 * Config that knows about points and poses
*/ */
class VSLAMConfig { class VSLAMConfig : Testable<VSLAMConfig> {
private: private:
typedef std::map<int, gtsam::Pose3> PoseMap; typedef std::map<int, Pose3> PoseMap;
typedef std::map<int, gtsam::Point3> PointMap; typedef std::map<int, Point3> PointMap;
PointMap landmarkPoints; PointMap landmarkPoints_;
PoseMap cameraPoses; PoseMap cameraPoses_;
public: public:
typedef std::map<std::string, Vector>::const_iterator const_iterator; typedef std::map<std::string, Vector>::const_iterator const_iterator;
@ -45,50 +36,23 @@ class VSLAMConfig {
*/ */
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 * copy constructor
*/ */
VSLAMConfig(const VSLAMConfig& original): VSLAMConfig(const VSLAMConfig& original):
cameraPoses(original.cameraPoses), landmarkPoints(original.landmarkPoints){} cameraPoses_(original.cameraPoses_), landmarkPoints_(original.landmarkPoints_){}
/** /**
* Exponential map: takes 6D vectors in VectorConfig * Exponential map: takes 6D vectors in VectorConfig
* and applies them to the poses in the VSLAMConfig. * and applies them to the poses in the VSLAMConfig.
* Needed for use in nonlinear optimization * Needed for use in nonlinear optimization
*/ */
VSLAMConfig exmap(const gtsam::VectorConfig & delta) const; VSLAMConfig exmap(const VectorConfig & delta) const;
/** PoseMap::const_iterator cameraIteratorBegin() const { return cameraPoses_.begin();}
* get the VectorConfig, poses have names x1,x2, and landmarks l1,l2,... PoseMap::const_iterator cameraIteratorEnd() const { return cameraPoses_.end();}
*/ PointMap::const_iterator landmarkIteratorBegin() const { return landmarkPoints_.begin();}
DCVectorConfig getVectorConfig() const; PointMap::const_iterator landmarkIteratorEnd() const { return landmarkPoints_.end();}
/**
* 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 * print
@ -100,15 +64,15 @@ class VSLAMConfig {
*/ */
bool cameraPoseExists(int i) const bool cameraPoseExists(int i) const
{ {
PoseMap::const_iterator it = cameraPoses.find(i); PoseMap::const_iterator it = cameraPoses_.find(i);
if (it==cameraPoses.end()) if (it==cameraPoses_.end())
return false; return false;
return true; return true;
} }
gtsam::Pose3 cameraPose(int i) const { Pose3 cameraPose(int i) const {
PoseMap::const_iterator it = cameraPoses.find(i); PoseMap::const_iterator it = cameraPoses_.find(i);
if (it==cameraPoses.end()) if (it==cameraPoses_.end())
throw(std::invalid_argument("robotPose: invalid key")); throw(std::invalid_argument("robotPose: invalid key"));
return it->second; return it->second;
} }
@ -118,8 +82,8 @@ class VSLAMConfig {
*/ */
bool landmarkPointExists(int i) const bool landmarkPointExists(int i) const
{ {
PointMap::const_iterator it = landmarkPoints.find(i); PointMap::const_iterator it = landmarkPoints_.find(i);
if (it==landmarkPoints.end()) if (it==landmarkPoints_.end())
return false; return false;
return true; return true;
} }
@ -127,9 +91,9 @@ class VSLAMConfig {
/** /**
* Retrieve landmark point * Retrieve landmark point
*/ */
gtsam::Point3 landmarkPoint(int i) const { Point3 landmarkPoint(int i) const {
PointMap::const_iterator it = landmarkPoints.find(i); PointMap::const_iterator it = landmarkPoints_.find(i);
if (it==landmarkPoints.end()) if (it==landmarkPoints_.end())
throw(std::invalid_argument("markerPose: invalid key")); throw(std::invalid_argument("markerPose: invalid key"));
return it->second; return it->second;
} }
@ -137,17 +101,17 @@ class VSLAMConfig {
/** /**
* check whether two configs are equal * check whether two configs are equal
*/ */
bool equals(const VSLAMConfig& c) ; bool equals(const VSLAMConfig& c, double tol=1e-6) const;
void addCameraPose(const int i, gtsam::Pose3 cp); void addCameraPose(const int i, Pose3 cp);
void addLandmarkPoint(const int i, gtsam::Point3 lp); void addLandmarkPoint(const int i, Point3 lp);
void removeCameraPose(const int i); void removeCameraPose(const int i);
void removeLandmarkPose(const int i); void removeLandmarkPose(const int i);
void clear() {landmarkPoints.clear(); cameraPoses.clear();} void clear() {landmarkPoints_.clear(); cameraPoses_.clear();}
inline size_t size(){ inline size_t size(){
return landmarkPoints.size() + cameraPoses.size(); return landmarkPoints_.size() + cameraPoses_.size();
} }
}; };

7
cpp/VSLAMGraph.cpp
View File

@ -10,10 +10,17 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include "VSLAMGraph.h" #include "VSLAMGraph.h"
#include "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h"
using namespace std; using namespace std;
namespace gtsam{ namespace gtsam{
// explicit instantiation so all the code is there and we can link with it
template class FactorGraph<VSLAMFactor>;
template class NonlinearFactorGraph<VSLAMConfig>;
template class NonlinearOptimizer<VSLAMGraph,VSLAMConfig>;
/* ************************************************************************* */ /* ************************************************************************* */
//TODO: CB: This constructor is specific to loading VO data. Should probably //TODO: CB: This constructor is specific to loading VO data. Should probably
// get rid of this. // get rid of this.

28
cpp/testVSLAMFactor.cpp
View File

@ -6,6 +6,7 @@
#include "VSLAMConfig.h" #include "VSLAMConfig.h"
#include "VSLAMFactor.h" #include "VSLAMFactor.h"
#include "VSLAMGraph.h"
#include "Point3.h" #include "Point3.h"
#include "Pose3.h" #include "Pose3.h"
@ -30,24 +31,43 @@ TEST( VSLAMFactor, error )
Vector z = Vector_(2,323.,240.); Vector z = Vector_(2,323.,240.);
double sigma=1.0; double sigma=1.0;
int cameraFrameNumber=1, landmarkNumber=1; int cameraFrameNumber=1, landmarkNumber=1;
VSLAMFactor factor(z, sigma, cameraFrameNumber, landmarkNumber, K); boost::shared_ptr<VSLAMFactor>
factor(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, K));
// For the following configuration, the factor predicts 320,240 // For the following configuration, the factor predicts 320,240
VSLAMConfig config; VSLAMConfig config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.addCameraPose(1, x1); Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.addCameraPose(1, x1);
Point3 l1; config.addLandmarkPoint(1, l1); 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 // 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 // Check linearize
Matrix Al1 = Matrix_(2, 3, 61.584, 0., 0., 0., 61.584, 0.); 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.); 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.); Vector b = Vector_(2,3.,0.);
LinearFactor expected("l1", Al1, "x1", Ax1, b, 1); 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)); 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */