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

Using traits in many places (forced by loss of Point2 mojo)

release/4.3a0
dellaert 2014-12-22 16:02:33 +01:00
parent 469b1d4e92
commit 5473550eea
13 changed files with 59 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/nonlinear/ExtendedKalmanFilter-inl.h
View File

@ -42,7 +42,8 @@ namespace gtsam {
// Extract the current estimate of x1,P1
VectorValues result = marginal->solve(VectorValues());
T x = linearizationPoints.at<T>(lastKey).retract(result[lastKey]);
const T& current = linearizationPoints.at<T>(lastKey);
T x = traits_x<T>::Retract(current, result[lastKey]);
// Create a Jacobian Factor from the root node of the produced Bayes Net.
// This will act as a prior for the next iteration.
@ -70,9 +71,10 @@ namespace gtsam {
// Create a Jacobian Prior Factor directly P_initial.
// Since x0 is set to the provided mean, the b vector in the prior will be zero
// TODO Frank asks: is there a reason why noiseModel is not simply P_initial ?
int n = traits_x<T>::GetDimension(x_initial);
priorFactor_ = JacobianFactor::shared_ptr(
new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(x_initial.dim()),
noiseModel::Unit::Create(P_initial->dim())));
new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n),
noiseModel::Unit::Create(n)));
}
/* ************************************************************************* */

5
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -42,6 +42,11 @@ namespace gtsam {
template<class VALUE>
class ExtendedKalmanFilter {
// Check that VALUE type is a testable Manifold
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
BOOST_CONCEPT_ASSERT((IsManifold<VALUE>));
public:
typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;

2
gtsam/nonlinear/ISAM2-inl.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
template<class VALUE>
VALUE ISAM2::calculateEstimate(Key key) const {
const Vector& delta = getDelta()[key];
return theta_.at<VALUE>(key).retract(delta);
return traits_x<VALUE>::Retract(theta_.at<VALUE>(key), delta);
}
/* ************************************************************************* */

35
gtsam/nonlinear/NonlinearEquality.h
View File

@ -23,6 +23,7 @@
#include <limits>
#include <iostream>
#include <cmath>
namespace gtsam {
@ -101,9 +102,9 @@ public:
*/
NonlinearEquality(Key j, const T& feasible, double error_gain,
bool (*_compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(
feasible), allow_error_(true), error_gain_(error_gain), compare_(
_compare) {
Base(noiseModel::Constrained::All(traits_x<T>::GetDimension(feasible)),
j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
compare_(_compare) {
}
/// @}
@ -114,14 +115,14 @@ public:
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n";
gtsam::print(feasible_, "Feasible Point:\n");
std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
std::cout << "Variable Dimension: " << traits_x<T>::GetDimension(feasible_) << std::endl;
}
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&f);
return e && Base::equals(f) && feasible_.equals(e->feasible_, tol)
&& fabs(error_gain_ - e->error_gain_) < tol;
&& std::abs(error_gain_ - e->error_gain_) < tol;
}
/// @}
@ -142,11 +143,11 @@ public:
/** error function */
Vector evaluateError(const T& xj,
boost::optional<Matrix&> H = boost::none) const {
size_t nj = feasible_.dim();
const size_t nj = traits_x<T>::GetDimension(feasible_);
if (allow_error_) {
if (H)
*H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
return xj.localCoordinates(feasible_);
return traits_x<T>::Local(xj,feasible_);
} else if (compare_(feasible_, xj)) {
if (H)
*H = eye(nj);
@ -235,8 +236,8 @@ public:
*
*/
NonlinearEquality1(const X& value, Key key, double mu = 1000.0) :
Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key), value_(
value) {
Base( noiseModel::Constrained::All(traits_x<X>::GetDimension(value),
std::abs(mu)), key), value_(value) {
}
virtual ~NonlinearEquality1() {
@ -252,9 +253,9 @@ public:
Vector evaluateError(const X& x1,
boost::optional<Matrix&> H = boost::none) const {
if (H)
(*H) = eye(x1.dim());
(*H) = eye(traits_x<X>::GetDimension(x1));
// manifold equivalent of h(x)-z -> log(z,h(x))
return value_.localCoordinates(x1);
return traits_x<X>::Local(value_,x1);
}
/** Print */
@ -310,7 +311,7 @@ public:
///TODO: comment
NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) :
Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {
Base(noiseModel::Constrained::All(traits_x<X>::dimension, std::abs(mu)), key1, key2) {
}
virtual ~NonlinearEquality2() {
}
@ -324,12 +325,10 @@ public:
/** g(x) with optional derivative2 */
Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
const size_t p = X::Dim();
if (H1)
*H1 = -eye(p);
if (H2)
*H2 = eye(p);
return x1.localCoordinates(x2);
static const size_t p = traits_x<X>::dimension;
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
return traits_x<X>::Local(x1,x2);
}
private:

3
gtsam/slam/PoseTranslationPrior.h
View File

@ -62,7 +62,8 @@ public:
Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const {
const Translation& newTrans = pose.translation();
const Rotation& R = pose.rotation();
const size_t tDim = newTrans.dim(), xDim = pose.dim();
const int tDim = traits_x<Translation>::GetDimension(newTrans);
const int xDim = traits_x<Pose>::GetDimension(pose);
if (H) {
*H = gtsam::zeros(tDim, xDim);
std::pair<size_t, size_t> transInterval = POSE::translationInterval();

10
gtsam/slam/ReferenceFrameFactor.h
View File

@ -84,7 +84,7 @@ public:
* each degree of freedom.
*/
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2)
: Base(noiseModel::Isotropic::Sigma(POINT().dim(), sigma),
: Base(noiseModel::Isotropic::Sigma(POINT::dimension, sigma),
globalKey, transKey, localKey) {}
virtual ~ReferenceFrameFactor(){}
@ -99,11 +99,9 @@ public:
boost::optional<Matrix&> Dtrans = boost::none,
boost::optional<Matrix&> Dlocal = boost::none) const {
Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
if (Dlocal) {
Point dummy;
*Dlocal = -1* gtsam::eye(dummy.dim());
}
return local.localCoordinates(newlocal);
if (Dlocal)
*Dlocal = -1* gtsam::eye(Point::dimension);
return traits_x<Point>::Local(local,newlocal);
}
virtual void print(const std::string& s="",

2
gtsam_unstable/geometry/SimWall2D.cpp
View File

@ -156,7 +156,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
// Simple check to make sure norm is on side closest robot
if (cur_pose.t().distance(intersection + norm) > cur_pose.t().distance(intersection - norm))
norm = norm.inverse();
norm = - norm;
// using the reflection
const double inside_bias = 0.05;

2
gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
View File

@ -67,7 +67,7 @@ public:
template<class VALUE>
VALUE calculateEstimate(Key key) const {
const Vector delta = delta_.at(key);
return theta_.at<VALUE>(key).retract(delta);
return traits_x<VALUE>::Retract(theta_.at<VALUE>(key), delta);
}
/** read the current set of optimizer parameters */

4
gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
View File

@ -91,8 +91,8 @@ public:
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Vector v1( VALUE::Logmap(p1) );
Vector v2( VALUE::Logmap(p2) );
Vector v1( traits_x<VALUE>::Logmap(p1) );
Vector v2( traits_x<VALUE>::Logmap(p2) );
Vector alpha(tau_.size());
Vector alpha_v1(tau_.size());

6
gtsam_unstable/slam/TSAMFactors.h
View File

@ -49,7 +49,7 @@ public:
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
Point2 d = pose.transform_to(point, H1, H2);
Point2 e = measured_.between(d);
Point2 e = d - measured_;
return e.vector();
}
};
@ -99,12 +99,12 @@ public:
*H3 = D_e_point_g * D_point_g_base2;
if (H4)
*H4 = D_e_point_g * D_point_g_point;
return measured_.localCoordinates(d);
return (d - measured_).vector();
} else {
Pose2 pose_g = base1.compose(pose);
Point2 point_g = base2.transform_from(point);
Point2 d = pose_g.transform_to(point_g);
return measured_.localCoordinates(d);
return (d - measured_).vector();
}
}
};

4
tests/simulated2DConstraints.h
View File

@ -90,11 +90,11 @@ namespace simulated2D {
virtual double value(const Point& x, boost::optional<Matrix&> H =
boost::none) const {
if (H) {
Matrix D = zeros(1, x.dim());
Matrix D = zeros(1, traits_x<Point>::GetDimension(x));
D(0, IDX) = 1.0;
*H = D;
}
return Point::Logmap(x)(IDX);
return traits_x<Point>::Logmap(x)(IDX);
}
};

39
tests/testExtendedKalmanFilter.cpp
View File

@ -92,10 +92,7 @@ TEST( ExtendedKalmanFilter, linear ) {
// Create Motion Model Factor
class NonlinearMotionModel : public NoiseModelFactor2<Point2,Point2> {
public:
typedef Point2 T;
private:
typedef NoiseModelFactor2<Point2, Point2> Base;
typedef NonlinearMotionModel This;
@ -126,19 +123,19 @@ public:
virtual ~NonlinearMotionModel() {}
// Calculate the next state prediction using the nonlinear function f()
T f(const T& x_t0) const {
Point2 f(const Point2& x_t0) const {
// Calculate f(x)
double x = x_t0.x() * x_t0.x();
double y = x_t0.x() * x_t0.y();
T x_t1(x,y);
Point2 x_t1(x,y);
// In this example, the noiseModel remains constant
return x_t1;
}
// Calculate the Jacobian of the nonlinear function f()
Matrix F(const T& x_t0) const {
Matrix F(const Point2& x_t0) const {
// Calculate Jacobian of f()
Matrix F = Matrix(2,2);
F(0,0) = 2*x_t0.x();
@ -150,7 +147,7 @@ public:
}
// Calculate the inverse square root of the motion model covariance, Q
Matrix QInvSqrt(const T& x_t0) const {
Matrix QInvSqrt(const Point2& x_t0) const {
// This motion model has a constant covariance
return Q_invsqrt_;
}
@ -184,7 +181,7 @@ public:
/** Vector of errors, whitened ! */
Vector whitenedError(const Values& c) const {
return QInvSqrt(c.at<T>(key1()))*unwhitenedError(c);
return QInvSqrt(c.at<Point2>(key1()))*unwhitenedError(c);
}
/**
@ -213,35 +210,29 @@ public:
}
/** vector of errors */
Vector evaluateError(const T& p1, const T& p2,
Vector evaluateError(const Point2& p1, const Point2& p2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
// error = p2 - f(p1)
// H1 = d error / d p1 = -d f/ d p1 = -F
// H2 = d error / d p2 = I
T prediction = f(p1);
Point2 prediction = f(p1);
if(H1){
if(H1)
*H1 = -F(p1);
}
if(H2){
if(H2)
*H2 = eye(dim());
}
// Return the error between the prediction and the supplied value of p2
return prediction.localCoordinates(p2);
return (p2 - prediction).vector();
}
};
// Create Measurement Model Factor
class NonlinearMeasurementModel : public NoiseModelFactor1<Point2> {
public:
typedef Point2 T;
private:
typedef NoiseModelFactor1<Point2> Base;
typedef NonlinearMeasurementModel This;
@ -268,7 +259,7 @@ public:
// Calculate the predicted measurement using the nonlinear function h()
// Byproduct: updates Jacobian H, and noiseModel (R)
Vector h(const T& x_t1) const {
Vector h(const Point2& x_t1) const {
// Calculate prediction
Vector z_hat(1);
@ -277,7 +268,7 @@ public:
return z_hat;
}
Matrix H(const T& x_t1) const {
Matrix H(const Point2& x_t1) const {
// Update Jacobian
Matrix H(1,2);
H(0,0) = 4*x_t1.x() - x_t1.y() - 2.5;
@ -287,7 +278,7 @@ public:
}
// Calculate the inverse square root of the motion model covariance, Q
Matrix RInvSqrt(const T& x_t0) const {
Matrix RInvSqrt(const Point2& x_t0) const {
// This motion model has a constant covariance
return R_invsqrt_;
}
@ -320,7 +311,7 @@ public:
/** Vector of errors, whitened ! */
Vector whitenedError(const Values& c) const {
return RInvSqrt(c.at<T>(key()))*unwhitenedError(c);
return RInvSqrt(c.at<Point2>(key()))*unwhitenedError(c);
}
/**
@ -345,7 +336,7 @@ public:
}
/** vector of errors */
Vector evaluateError(const T& p, boost::optional<Matrix&> H1 = boost::none) const {
Vector evaluateError(const Point2& p, boost::optional<Matrix&> H1 = boost::none) const {
// error = z - h(p)
// H = d error / d p = -d h/ d p = -H
Vector z_hat = h(p);

3
tests/testNonlinearEquality.cpp
View File

@ -408,8 +408,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
// odometry constraint
eq2D::OdoEqualityConstraint::shared_ptr constraint2(
new eq2D::OdoEqualityConstraint(truth_pt1.between(truth_pt2), key1,
key2));
new eq2D::OdoEqualityConstraint(truth_pt2-truth_pt1, key1, key2));
NonlinearFactorGraph graph;
graph.push_back(constraint1);