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

Merge branch 'master' into retraction_name

release/4.3a0
Alex Cunningham 2011-11-05 20:30:01 +00:00
parent b3501efdec
commit e0f2087e03
6 changed files with 51 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam.h
View File

@ -45,10 +45,13 @@ class Pose2 {
class SharedGaussian {
SharedGaussian(Matrix covariance);
void print(string s) const;
};
class SharedDiagonal {
SharedDiagonal(Vector sigmas);
void print(string s) const;
Vector sample() const;
};
class VectorValues {

6
gtsam/linear/NoiseModel.cpp
View File

@ -535,7 +535,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const {
// Null model
/* ************************************************************************* */
void Null::print(const std::string &s) const
void Null::print(const std::string &s="") const
{ cout << s << ": null ()" << endl; }
Null::shared_ptr Null::Create()
@ -556,7 +556,7 @@ Fair::Fair(const double c, const ReweightScheme reweight)
double Fair::weight(const double &error) const
{ return 1.0 / (1.0 + fabs(error)/c_); }
void Fair::print(const std::string &s) const
void Fair::print(const std::string &s="") const
{ cout << s << ": fair (" << c_ << ")" << endl; }
bool Fair::equals(const Base &expected, const double tol) const {
@ -584,7 +584,7 @@ double Huber::weight(const double &error) const {
return (error < k_) ? (1.0) : (k_ / fabs(error));
}
void Huber::print(const std::string &s) const {
void Huber::print(const std::string &s="") const {
cout << s << ": huber (" << k_ << ")" << endl;
}

6
gtsam/linear/SharedDiagonal.h
View File

@ -44,6 +44,12 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
noiseModel::Diagonal::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) {
}
/// Print
inline void print(const std::string &s) const { (*this)->print(s); }
/// Generate a sample
inline Vector sample() const { return (*this)->sample(); }
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>

3
gtsam/linear/SharedGaussian.h
View File

@ -55,6 +55,9 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
Base(noiseModel::Isotropic::Sigma(GTSAM_DANGEROUS_GAUSSIAN, s)) {}
#endif
/// Print
inline void print(const std::string &s) const { (*this)->print(s); }
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>

8
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -134,6 +134,14 @@ TEST(NoiseModel, equals)
EXPECT(assert_inequal(*i1,*i2));
}
/* ************************************************************************* */
TEST(NoiseModel, sample) {
Vector s = Vector_(3,1.0,2.0,3.0);
SharedDiagonal model = sharedSigmas(s);
Vector v = model->sample();
// no check as no way yet to set random seed
}
/* ************************************************************************* */
TEST(NoiseModel, ConstrainedMixed )
{

32
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -44,13 +44,37 @@ static noiseModel::Gaussian::shared_ptr covariance(
//static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3));
/* ************************************************************************* */
Vector f(const Pose2& pose1, const Pose2& pose2) {
// Test constraint, small displacement
Vector f1(const Pose2& pose1, const Pose2& pose2) {
Pose2 z(2.1130913087, 0.468461064817, 0.436332312999);
Pose2Factor constraint(1, 2, z, covariance);
return constraint.evaluateError(pose1, pose2);
}
TEST( Pose2SLAM, constraint1 )
{
// create a factor between unknown poses p1 and p2
Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999);
Pose2Factor constraint(1, 2, pose2, covariance);
Matrix H1, H2;
Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
Matrix numericalH1 = numericalDerivative21(&f1 , pose1, pose2);
EXPECT(assert_equal(numericalH1,H1,5e-3));
Matrix numericalH2 = numericalDerivative22(&f1 , pose1, pose2);
EXPECT(assert_equal(numericalH2,H2));
}
/* ************************************************************************* */
// Test constraint, large displacement
Vector f2(const Pose2& pose1, const Pose2& pose2) {
Pose2 z(2,2,M_PI_2);
Pose2Factor constraint(1, 2, z, covariance);
return constraint.evaluateError(pose1, pose2);
}
TEST( Pose2SLAM, constraint )
TEST( Pose2SLAM, constraint2 )
{
// create a factor between unknown poses p1 and p2
Pose2 pose1, pose2(2,2,M_PI_2);
@ -58,10 +82,10 @@ TEST( Pose2SLAM, constraint )
Matrix H1, H2;
Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
Matrix numericalH1 = numericalDerivative21(&f , pose1, pose2);
Matrix numericalH1 = numericalDerivative21(&f2 , pose1, pose2);
EXPECT(assert_equal(numericalH1,H1,5e-3));
Matrix numericalH2 = numericalDerivative22(&f , pose1, pose2);
Matrix numericalH2 = numericalDerivative22(&f2 , pose1, pose2);
EXPECT(assert_equal(numericalH2,H2));
}