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

VSLAMFactor Testable

release/4.3a0
Chris Beall 2009-11-11 21:09:43 +00:00
parent 8c695a778c
commit 70efccefbc
3 changed files with 24 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cpp/VSLAMFactor.cpp
View File

@ -35,11 +35,10 @@ void VSLAMFactor::print(const std::string& s) const {
}
/* ************************************************************************* */
bool VSLAMFactor::equals(const NonlinearFactor<VSLAMConfig>& f, double tol) const {
const VSLAMFactor* p = dynamic_cast<const VSLAMFactor*>(&f);
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;
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;
return true;
}

7
cpp/VSLAMFactor.h
View File

@ -9,6 +9,8 @@
#include "NonlinearFactor.h"
#include "LinearFactor.h"
#include "Cal3_S2.h"
#include "Testable.h"
namespace gtsam {
@ -18,7 +20,8 @@ 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>
//class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<NonlinearFactor<VSLAMConfig> >
class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor>
{
private:
@ -51,7 +54,7 @@ class VSLAMFactor : public NonlinearFactor<VSLAMConfig>
/**
* equals
*/
bool equals(const NonlinearFactor<VSLAMConfig>&, double tol=1e-9) const;
bool equals(const VSLAMFactor&, double tol=1e-9) const;
/**
* predict the measurement

15
cpp/testVSLAMFactor.cpp
View File

@ -70,6 +70,21 @@ TEST( VSLAMFactor, error )
CHECK(assert_equal(expected_config,actual_config,1e-9));
}
TEST( VSLAMFactor, equals )
{
// Create two identical factors and make sure they're equal
Vector z = Vector_(2,323.,240.);
double sigma=1.0;
int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<VSLAMFactor>
factor1(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, K));
boost::shared_ptr<VSLAMFactor>
factor2(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, K));
CHECK(assert_equal(*factor1, *factor2));
}
/* ************************************************************************* */
int main() {
TestResult tr;