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

Cleaned up and added test for VSLAMFactor

release/4.3a0
Frank Dellaert 2009-11-09 04:46:34 +00:00
parent e340178de5
commit 7bd40e836d
3 changed files with 88 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
cpp/VSLAMFactor.cpp
View File

@ -4,16 +4,17 @@
* @author Alireza Fathi
*/
#include "VectorConfig.h"
#include "VSLAMFactor.h"
#include "Pose3.h"
#include "SimpleCamera.h"
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
template <class Config>
VSLAMFactor<Config>::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K)
: NonlinearFactor<Config>(z, sigma)
VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K)
: NonlinearFactor<VectorConfig>(z, sigma)
{
cameraFrameNumber_ = cn;
landmarkNumber_ = ln;
@ -28,27 +29,36 @@ VSLAMFactor<Config>::VSLAMFactor(const Vector& z, double sigma, int cn, int ln,
}
/* ************************************************************************* */
template <class Config>
void VSLAMFactor<Config>::print(const std::string& s) const {
void VSLAMFactor::print(const std::string& s) const {
printf("%s %s %s\n", s.c_str(), cameraFrameName_.c_str(), landmarkName_.c_str());
::print(ConvenientFactor::z_, s+".z");
::print(this->z_, s+".z");
}
/* ************************************************************************* */
template <class Config>
Vector VSLAMFactor<Config>::error_vector(const Config& c) const {
bool VSLAMFactor::equals(const NonlinearFactor<VectorConfig>& 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;
return true;
}
/* ************************************************************************* */
Vector VSLAMFactor::predict(const VectorConfig& c) const {
Pose3 pose = c[cameraFrameName_];
Point3 landmark = c[landmarkName_];
// Right-hand-side b = (z - h(x))/sigma
Vector h = project(SimpleCamera(K_,pose), landmark).vector();
return (ConvenientFactor::z_ - h);
return project(SimpleCamera(K_,pose), landmark).vector();
}
/* ************************************************************************* */
template <class Config>
LinearFactor::shared_ptr VSLAMFactor<Config>::linearize(const Config& c) const
Vector VSLAMFactor::error_vector(const VectorConfig& c) const {
// Right-hand-side b = (z - h(x))/sigma
Vector h = predict(c);
return (this->z_ - h);
}
/* ************************************************************************* */
LinearFactor::shared_ptr VSLAMFactor::linearize(const VectorConfig& c) const
{
// get arguments from config
Pose3 pose = c[cameraFrameName_]; // cast from Vector to Pose3 !!!
@ -62,27 +72,12 @@ LinearFactor::shared_ptr VSLAMFactor<Config>::linearize(const Config& c) const
// Right-hand-side b = (z - h(x))
Vector h = project(camera, landmark).vector();
Vector b = ConvenientFactor::z_ - h;
Vector b = this->z_ - h;
// Make new linearfactor, divide by sigma
LinearFactor::shared_ptr
p(new LinearFactor(cameraFrameName_, Dh1, landmarkName_, Dh2, b, ConvenientFactor::sigma_));
p(new LinearFactor(cameraFrameName_, Dh1, landmarkName_, Dh2, b, this->sigma_));
return p;
}
/* ************************************************************************* */
template <class Config>
bool VSLAMFactor<Config>::equals(const NonlinearFactor<Config>& f, double tol) const {
const VSLAMFactor* p = dynamic_cast<const VSLAMFactor*>(&f);
if (p == NULL) goto fail;
if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail;
if (!equal_with_abs_tol(ConvenientFactor::z_,p->z_,tol)) goto fail;
return true;
fail:
print("actual");
p->print("expected");
return false;
}
/* ************************************************************************* */

32
cpp/VSLAMFactor.h
View File

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

48
cpp/testVSLAMFactor.cpp
View File

@ -1,17 +1,53 @@
/**********************************************************
Written by Alireza Fathi, 2nd of April 2009
**********************************************************/
Written by Frank Dellaert, Nov 2009
**********************************************************/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include "numericalDerivative.h"
#include "NonlinearFactorGraph.h"
#include "VSLAMFactor.h"
#include "Point3.h"
#include "Pose3.h"
using namespace std;
using namespace gtsam;
// TODO: test VSLAMFactor !!!
// make cube
Point3 x000(-1, -1, -1), x001(-1, -1, +1), x010(-1, +1, -1), x011(-1, +1, +1),
x100(-1, -1, -1), x101(-1, -1, +1), x110(-1, +1, -1), x111(-1, +1, +1);
// make a realistic calibration matrix
double fov = 60; // degrees
size_t w=640,h=480;
Cal3_S2 K(fov,w,h);
// make cameras
/* ************************************************************************* */
TEST( VSLAMFactor, error )
{
// Create the factor with a measurement that is 3 pixels off in x
Vector z = Vector_(2,323.,240.);
double sigma=1.0;
int cameraFrameNumber=1, landmarkNumber=1;
VSLAMFactor factor(z, sigma, cameraFrameNumber, landmarkNumber, K);
// For the following configuration, the factor predicts 320,240
VectorConfig config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert("x1",x1.vector());
Point3 l1; config.insert("l1",l1.vector());
CHECK(assert_equal(Vector_(2,320.,240.),factor.predict(config)));
// Which yields an error of 3^2/2 = 4.5
DOUBLES_EQUAL(4.5,factor.error(config),1e-9);
// Check linearize
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.);
Vector b = Vector_(2,3.,0.);
LinearFactor expected("l1", Al1, "x1", Ax1, b, 1);
LinearFactor::shared_ptr actual = factor.linearize(config);
CHECK(assert_equal(expected,*actual,1e-3));
}
/* ************************************************************************* */
int main() {