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

Clean-up

release/4.3a0
Frank Dellaert 2009-11-13 06:17:59 +00:00
parent fe974a3e72
commit 309f2151cf
2 changed files with 8 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
cpp/VSLAMFactor.cpp
View File

@ -13,18 +13,13 @@ using namespace std;
namespace gtsam{ namespace gtsam{
/* ************************************************************************* */ /* ************************************************************************* */
VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K) VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln, const Cal3_S2 &K)
: NonlinearFactor<VSLAMConfig>(z, sigma) : NonlinearFactor<VSLAMConfig>(z.vector(), sigma)
{ {
cameraFrameNumber_ = cn; cameraFrameNumber_ = cn;
landmarkNumber_ = ln; landmarkNumber_ = ln;
char temp[100]; cameraFrameName_ = symbol('x',cameraFrameNumber_);
temp[0] = 0; landmarkName_ = symbol('l',landmarkNumber_);
sprintf(temp, "x%d", cameraFrameNumber_);
cameraFrameName_ = string(temp);
temp[0] = 0;
sprintf(temp, "l%d", landmarkNumber_);
landmarkName_ = string(temp);
K_ = K; K_ = K;
} }
@ -35,7 +30,7 @@ void VSLAMFactor::print(const std::string& s) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool VSLAMFactor::equals(const VSLAMFactor& p, double tol) const { bool VSLAMFactor::equals(const VSLAMFactor& p, double tol) const {
if (&p == NULL) return false; if (&p == NULL) return false;
if (cameraFrameNumber_ != p.cameraFrameNumber_ || landmarkNumber_ != p.landmarkNumber_) return false; if (cameraFrameNumber_ != p.cameraFrameNumber_ || landmarkNumber_ != p.landmarkNumber_) return false;
if (!equal_with_abs_tol(this->z_,p.z_,tol)) return false; if (!equal_with_abs_tol(this->z_,p.z_,tol)) return false;

6
cpp/VSLAMFactor.h
View File

@ -11,7 +11,6 @@
#include "Cal3_S2.h" #include "Cal3_S2.h"
#include "Testable.h" #include "Testable.h"
namespace gtsam { namespace gtsam {
class VSLAMConfig; class VSLAMConfig;
@ -20,7 +19,6 @@ class VSLAMConfig;
* 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<VSLAMConfig>, Testable<NonlinearFactor<VSLAMConfig> >
class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor> class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor>
{ {
private: private:
@ -28,7 +26,7 @@ class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor>
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<VSLAMConfig> ConvenientFactor; typedef NonlinearFactor<VSLAMConfig> ConvenientFactor;
public: public:
@ -42,7 +40,7 @@ class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor>
* @param landmarkNumber is the index of the landmark * @param landmarkNumber is the index of the landmark
* @param K the constant calibration * @param K the constant calibration
*/ */
VSLAMFactor(const Vector& z, double sigma, int cameraFrameNumber, int landmarkNumber, const Cal3_S2& K); VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber, int landmarkNumber, const Cal3_S2& K);
/** /**