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

updated to be templated on Config.

release/4.3a0
Chris Beall 2009-10-13 20:55:07 +00:00
parent 66dac8a52f
commit 92b920cb48
2 changed files with 27 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
cpp/VSLAMFactor.cpp
View File

@ -11,8 +11,9 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K) template <class Config>
: NonlinearFactor<FGConfig>(z, sigma) VSLAMFactor<Config>::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K)
: NonlinearFactor<Config>(z, sigma)
{ {
cameraFrameNumber_ = cn; cameraFrameNumber_ = cn;
landmarkNumber_ = ln; landmarkNumber_ = ln;
@ -27,24 +28,27 @@ VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Ca
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VSLAMFactor::print(const std::string& s) const { template <class Config>
void VSLAMFactor<Config>::print(const std::string& s) const {
printf("%s %s %s\n", s.c_str(), cameraFrameName_.c_str(), landmarkName_.c_str()); printf("%s %s %s\n", s.c_str(), cameraFrameName_.c_str(), landmarkName_.c_str());
::print(z_, s+".z"); ::print(ConvenientFactor::z_, s+".z");
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector VSLAMFactor::error_vector(const FGConfig& c) const { template <class Config>
Vector VSLAMFactor<Config>::error_vector(const Config& c) const {
Pose3 pose = c[cameraFrameName_]; Pose3 pose = c[cameraFrameName_];
Point3 landmark = c[landmarkName_]; Point3 landmark = c[landmarkName_];
// Right-hand-side b = (z - h(x))/sigma // Right-hand-side b = (z - h(x))/sigma
Vector h = project(SimpleCamera(K_,pose), landmark).vector(); Vector h = project(SimpleCamera(K_,pose), landmark).vector();
return (z_ - h); return (ConvenientFactor::z_ - h);
} }
/* ************************************************************************* */ /* ************************************************************************* */
LinearFactor::shared_ptr VSLAMFactor::linearize(const FGConfig& c) const template <class Config>
LinearFactor::shared_ptr VSLAMFactor<Config>::linearize(const Config& c) const
{ {
// get arguments from config // get arguments from config
Pose3 pose = c[cameraFrameName_]; // cast from Vector to Pose3 !!! Pose3 pose = c[cameraFrameName_]; // cast from Vector to Pose3 !!!
@ -58,20 +62,21 @@ LinearFactor::shared_ptr VSLAMFactor::linearize(const FGConfig& c) const
// Right-hand-side b = (z - h(x)) // Right-hand-side b = (z - h(x))
Vector h = project(camera, landmark).vector(); Vector h = project(camera, landmark).vector();
Vector b = z_ - h; Vector b = ConvenientFactor::z_ - h;
// Make new linearfactor, divide by sigma // Make new linearfactor, divide by sigma
LinearFactor::shared_ptr LinearFactor::shared_ptr
p(new LinearFactor(cameraFrameName_, Dh1/sigma_, landmarkName_, Dh2/sigma_, b/sigma_)); p(new LinearFactor(cameraFrameName_, Dh1/ConvenientFactor::sigma_, landmarkName_, Dh2/ConvenientFactor::sigma_, b/ConvenientFactor::sigma_));
return p; return p;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool VSLAMFactor::equals(const NonlinearFactor<FGConfig>& f, double tol) const { template <class Config>
bool VSLAMFactor<Config>::equals(const NonlinearFactor<Config>& f, double tol) const {
const VSLAMFactor* p = dynamic_cast<const VSLAMFactor*>(&f); const VSLAMFactor* p = dynamic_cast<const VSLAMFactor*>(&f);
if (p == NULL) goto fail; if (p == NULL) goto fail;
if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail; if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail;
if (!equal_with_abs_tol(z_,p->z_,tol)) goto fail; if (!equal_with_abs_tol(ConvenientFactor::z_,p->z_,tol)) goto fail;
return true; return true;
fail: fail:
@ -81,14 +86,15 @@ bool VSLAMFactor::equals(const NonlinearFactor<FGConfig>& f, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
string VSLAMFactor::dump() const template <class Config>
string VSLAMFactor<Config>::dump() const
{ {
int i = getCameraFrameNumber(); int i = getCameraFrameNumber();
int j = getLandmarkNumber(); int j = getLandmarkNumber();
Vector z = measurement(); Vector z = ConvenientFactor::measurement();
char buffer[200]; char buffer[200];
buffer[0] = 0; buffer[0] = 0;
sprintf(buffer, "1 %d %d %f %d", i, j , sigma(), (int)z.size()); sprintf(buffer, "1 %d %d %f %d", i, j , ConvenientFactor::sigma(), (int)z.size());
for(size_t i = 0; i < z.size(); i++) for(size_t i = 0; i < z.size(); i++)
sprintf(buffer, "%s %f", buffer, z(i)); sprintf(buffer, "%s %f", buffer, z(i));
sprintf(buffer, "%s %s", buffer, K_.dump().c_str()); sprintf(buffer, "%s %s", buffer, K_.dump().c_str());

12
cpp/VSLAMFactor.h
View File

@ -14,18 +14,20 @@
namespace gtsam { namespace gtsam {
/** /**
* Non-linear factor for a constraint derived from a 2D measurement, * Non-linear factor for a constraint derived from a 2D measurement,
* i.e. the main building block for visual SLAM. * i.e. the main building block for visual SLAM.
*/ */
class VSLAMFactor : public NonlinearFactor<FGConfig> template <class Config>
class VSLAMFactor : public NonlinearFactor<Config>
{ {
private: private:
int cameraFrameNumber_, landmarkNumber_; int cameraFrameNumber_, landmarkNumber_;
std::string cameraFrameName_, landmarkName_; std::string cameraFrameName_, landmarkName_;
Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this. Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this.
typedef gtsam::NonlinearFactor<Config> ConvenientFactor;
public: public:
typedef boost::shared_ptr<VSLAMFactor> shared_ptr; // shorthand for a smart pointer to a factor typedef boost::shared_ptr<VSLAMFactor> shared_ptr; // shorthand for a smart pointer to a factor
@ -50,17 +52,17 @@ class VSLAMFactor : public NonlinearFactor<FGConfig>
/** /**
* calculate the error of the factor * calculate the error of the factor
*/ */
Vector error_vector(const FGConfig&) const; Vector error_vector(const Config&) const;
/** /**
* linerarization * linerarization
*/ */
LinearFactor::shared_ptr linearize(const FGConfig&) const; LinearFactor::shared_ptr linearize(const Config&) const;
/** /**
* equals * equals
*/ */
bool equals(const NonlinearFactor<FGConfig>&, double tol=1e-9) const; bool equals(const NonlinearFactor<Config>&, double tol=1e-9) const;
int getCameraFrameNumber() const { return cameraFrameNumber_; } int getCameraFrameNumber() const { return cameraFrameNumber_; }
int getLandmarkNumber() const { return landmarkNumber_; } int getLandmarkNumber() const { return landmarkNumber_; }