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{
/* ************************************************************************* */
VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K)
: NonlinearFactor<VSLAMConfig>(z, sigma)
VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln, const Cal3_S2 &K)
: NonlinearFactor<VSLAMConfig>(z.vector(), sigma)
{
cameraFrameNumber_ = cn;
landmarkNumber_ = ln;
char temp[100];
temp[0] = 0;
sprintf(temp, "x%d", cameraFrameNumber_);
cameraFrameName_ = string(temp);
temp[0] = 0;
sprintf(temp, "l%d", landmarkNumber_);
landmarkName_ = string(temp);
landmarkNumber_ = ln;
cameraFrameName_ = symbol('x',cameraFrameNumber_);
landmarkName_ = symbol('l',landmarkNumber_);
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 (cameraFrameNumber_ != p.cameraFrameNumber_ || landmarkNumber_ != p.landmarkNumber_) 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 "Testable.h"
namespace gtsam {
class VSLAMConfig;
@ -20,7 +19,6 @@ 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<VSLAMConfig>, Testable<NonlinearFactor<VSLAMConfig> >
class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor>
{
private:
@ -28,7 +26,7 @@ class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor>
int cameraFrameNumber_, landmarkNumber_;
std::string cameraFrameName_, landmarkName_;
Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this.
typedef gtsam::NonlinearFactor<VSLAMConfig> ConvenientFactor;
typedef NonlinearFactor<VSLAMConfig> ConvenientFactor;
public:
@ -42,7 +40,7 @@ class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor>
* @param landmarkNumber is the index of the landmark
* @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);
/**