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

Pose2Prior is now a typedef, improved some derivatives

release/4.3a0
Frank Dellaert 2010-01-16 16:46:57 +00:00
parent 4914091c87
commit 3c9c8bcfe5
6 changed files with 68 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
cpp/BetweenFactor.h
View File

@ -45,16 +45,17 @@ namespace gtsam {
/** print */
void print(const std::string& s) const {
Base::print(s);
measured_.print("measured ");
gtsam::print(square_root_inverse_covariance_, "MeasurementCovariance");
measured_.print("measured");
gtsam::print(square_root_inverse_covariance_,
"Square Root Inverse Covariance");
}
/** equals */
bool equals(const NonlinearFactor<Config>& expected, double tol) const {
const BetweenFactor<Config, Key, T> *e =
dynamic_cast<const BetweenFactor<Config, Key, T>*> (&expected);
return e != NULL && Base::equals(expected)
&& this->measured_.equals(e->measured_, tol);
return e != NULL && Base::equals(expected) && this->measured_.equals(
e->measured_, tol);
}
/** implement functions needed to derive from Factor */
@ -62,24 +63,24 @@ namespace gtsam {
/** vector of errors */
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
// h - z
T hx = between(p1, p2);
// TODO should be done by noise model
T hx = between(p1, p2, H1, H2); // h(x)
if (H1 || H2) {
between(p1,p2,H1,H2);
if (H1) *H1 = square_root_inverse_covariance_ * *H1;
if (H2) *H2 = square_root_inverse_covariance_ * *H2;
}
// manifold equivalent of h(x)-z -> log(z,h(x))
// TODO use noise model, error vector is not whitened yet
return square_root_inverse_covariance_ * logmap(measured_, hx);
}
/** return the measured */
inline const T measured() const {return measured_;}
inline const T measured() const {
return measured_;
}
/** number of variables attached to this factor */
inline std::size_t size() const { return 2;}
inline std::size_t size() const {
return 2;
}
};
} /// namespace gtsam

6
cpp/Pose2.cpp
View File

@ -61,10 +61,8 @@ namespace gtsam {
dT1(1,0), dT1(1,1), dR1(1,0),
0.0, 0.0, -1.0);
}
if (H2) *H2 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
static const Matrix I3 = eye(3);
if (H2) *H2 = I3;
return dp;
}

93
cpp/Pose2Prior.h
View File

@ -15,55 +15,60 @@
namespace gtsam {
class Pose2Prior : public NonlinearFactor<Pose2Config> {
private:
typedef Pose2Config::Key Key;
Key key_;
Pose2 measured_;
Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
//std::list<Key> keys_;
/**
* A class for a soft prior on any Lie type
* T is the Lie group type, Config where the T's are gotten from
*/
template<class Config, class Key, class T>
class PriorFactor: public NonlinearFactor1<Config, Key, T> {
public:
private:
typedef boost::shared_ptr<Pose2Prior> shared_ptr; // shorthand for a smart pointer to a factor
typedef NonlinearFactor1<Config, Key, T> Base;
Pose2Prior(const Key& key, const Pose2& measured, const Matrix& measurement_covariance) :
key_(key),measured_(measured) {
square_root_inverse_covariance_ = inverse_square_root(measurement_covariance);
keys_.push_back(key);
}
T prior_; /** The measurement */
Matrix square_root_inverse_covariance_; /** sqrt(inv(covariance)) */
/** implement functions needed for Testable */
void print(const std::string& name) const {
std::cout << name << std::endl;
std::cout << "Factor "<< std::endl;
std::cout << "key "<< (std::string)key_<<std::endl;
measured_.print("measured ");
gtsam::print(square_root_inverse_covariance_,"MeasurementCovariance");
}
bool equals(const NonlinearFactor<Pose2Config>& expected, double tol) const {return false;}
public:
/** implement functions needed to derive from Factor */
Vector error_vector(const Pose2Config& config) const {
Pose2 p = config[key_];
return square_root_inverse_covariance_ * logmap(measured_,p);
}
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<PriorFactor> shared_ptr;
//std::list<Key> keys() const { return keys_; }
std::size_t size() const { return keys_.size(); }
/** Constructor */
PriorFactor(const Key& key, const T& prior, const Matrix& covariance) :
Base(1.0, key), prior_(prior) {
square_root_inverse_covariance_ = inverse_square_root(covariance);
}
/** implement functions needed for Testable */
/** print */
void print(const std::string& s) const {
Base::print(s);
prior_.print("prior");
gtsam::print(square_root_inverse_covariance_,
"Square Root Inverse Covariance");
}
/** equals */
bool equals(const NonlinearFactor<Config>& expected, double tol) const {
const PriorFactor<Config, Key, T> *e = dynamic_cast<const PriorFactor<
Config, Key, T>*> (&expected);
return e != NULL && Base::equals(expected) && this->prior_.equals(
e->prior_, tol);
}
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = square_root_inverse_covariance_;
// manifold equivalent of h(x)-z -> log(z,h(x))
return square_root_inverse_covariance_ * logmap(prior_, p);
}
};
/** This is just a typedef now */
typedef PriorFactor<Pose2Config, Pose2Config::Key, Pose2> Pose2Prior;
/** linearize */
boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const {
Pose2 p = config[key_];
Vector b = - error_vector(config);
Matrix H(3,3);
H(0,0)=1; H(0,1)=0; H(0,2)=0;
H(1,0)=0; H(1,1)=1; H(1,2)=0;
H(2,0)=0; H(2,1)=0; H(2,2)=1;
boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(
key_, square_root_inverse_covariance_ * H,
b, 1.0));
return linearized;
}
};
} /// namespace gtsam

22
cpp/Pose3.cpp
View File

@ -133,31 +133,11 @@ namespace gtsam {
return pose.rotation().transpose();
}
/* ************************************************************************* */
// direct measurement of the deviation of a pose from the origin
// used as soft prior
/* ************************************************************************* */
Vector hPose (const Vector& x) {
Pose3 pose(x); // transform from vector to Pose3
Vector w = pose.rotation().ypr(); // get angle differences
Vector d = pose.translation().vector(); // get translation differences
return concatVectors(2,&w,&d);
}
/* ************************************************************************* */
// derivative of direct measurement
// 6*6, entry i,j is how measurement error will change
/* ************************************************************************* */
Matrix DhPose(const Vector& x) {
Matrix H = eye(6,6);
return H;
}
/* ************************************************************************* */
// compose = Pose3(compose(R1,R2),transform_from(p1,t2);
Matrix Dcompose1(const Pose3& p1, const Pose3& p2) {
Matrix DR_R1 = eye(3);
static const Matrix DR_R1 = eye(3);
Matrix DR_t1 = zeros(3, 3);
Matrix DR = collect(2, &DR_R1, &DR_t1);
Matrix Dt = Dtransform_from1(p1, p2.translation());

9
cpp/Pose3.h
View File

@ -142,13 +142,4 @@ namespace gtsam {
Pose3 between(const Pose3& p1, const Pose3& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/** direct measurement of a pose */
Vector hPose(const Vector& x);
/**
* derivative of direct measurement
* 12*6, entry i,j is how measurement error will change
*/
Matrix DhPose(const Vector& x);
} // namespace gtsam

9
cpp/testPose2Prior.cpp
View File

@ -54,14 +54,14 @@ TEST( Pose2Prior, error )
static Pose2 prior(2,2,M_PI_2);
static Pose2Prior factor(1,prior, covariance);
/* ************************************************************************* *
/* ************************************************************************* */
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
// Hence i.e., b = approximates z-h(x0) = error_vector(x0)
Vector h(const Pose2& p1) {
return factor.evaluateError(p1);
}
/* ************************************************************************* *
/* ************************************************************************* */
TEST( Pose2Prior, linearize )
{
// Choose a linearization point at ground truth
@ -70,11 +70,10 @@ TEST( Pose2Prior, linearize )
// Actual linearization
boost::shared_ptr<GaussianFactor> actual = factor.linearize(x0);
CHECK(assert_equal(expected,*actual));
// Numerical do not work out because BetweenFactor is approximate ?
// Test with numerical derivative
Matrix numericalH = numericalDerivative11(h, prior, 1e-5);
CHECK(assert_equal(expectedH,numericalH));
CHECK(assert_equal(numericalH,actual->get_A("x1")));
}
/* ************************************************************************* */