Merge remote-tracking branch 'origin/develop' into feature/sam_sfm_directories
Frank Dellaert
commit
3fae6178e8
|
|
@ -15,12 +15,12 @@
|
|||
* @author Can Erdogan
|
||||
* @author Frank Dellaert
|
||||
* @author Alex Trevor
|
||||
* @author Zhaoyang Lv
|
||||
* @brief The Unit3 class - basically a point on a unit sphere
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <boost/random/mersenne_twister.hpp>
|
||||
#include <gtsam/config.h> // for GTSAM_USE_TBB
|
||||
|
||||
#ifdef __clang__
|
||||
|
|
@ -62,12 +62,10 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
|
|||
boost::uniform_on_sphere<double> randomDirection(3);
|
||||
// This variate_generator object is required for versions of boost somewhere
|
||||
// around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
|
||||
boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> >
|
||||
generator(rng, randomDirection);
|
||||
boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> > generator(
|
||||
rng, randomDirection);
|
||||
vector<double> d = generator();
|
||||
Unit3 result;
|
||||
result.p_ = Point3(d[0], d[1], d[2]);
|
||||
return result;
|
||||
return Unit3(d[0], d[1], d[2]);
|
||||
}
|
||||
|
||||
#ifdef GTSAM_USE_TBB
|
||||
|
|
@ -85,26 +83,24 @@ const Matrix32& Unit3::basis() const {
|
|||
return *B_;
|
||||
|
||||
// Get the axis of rotation with the minimum projected length of the point
|
||||
Point3 axis;
|
||||
Vector3 axis;
|
||||
double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z());
|
||||
if ((mx <= my) && (mx <= mz))
|
||||
axis = Point3(1.0, 0.0, 0.0);
|
||||
axis = Vector3(1.0, 0.0, 0.0);
|
||||
else if ((my <= mx) && (my <= mz))
|
||||
axis = Point3(0.0, 1.0, 0.0);
|
||||
axis = Vector3(0.0, 1.0, 0.0);
|
||||
else if ((mz <= mx) && (mz <= my))
|
||||
axis = Point3(0.0, 0.0, 1.0);
|
||||
axis = Vector3(0.0, 0.0, 1.0);
|
||||
else
|
||||
assert(false);
|
||||
|
||||
// Create the two basis vectors
|
||||
Point3 b1 = p_.cross(axis);
|
||||
b1 = b1 / b1.norm();
|
||||
Point3 b2 = p_.cross(b1);
|
||||
b2 = b2 / b2.norm();
|
||||
Vector3 b1 = p_.cross(axis).normalized();
|
||||
Vector3 b2 = p_.cross(b1).normalized();
|
||||
|
||||
// Create the basis matrix
|
||||
B_.reset(Matrix32());
|
||||
(*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
|
||||
(*B_) << b1, b2;
|
||||
return *B_;
|
||||
}
|
||||
|
||||
|
|
@ -122,10 +118,9 @@ Matrix3 Unit3::skew() const {
|
|||
/* ************************************************************************* */
|
||||
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
|
||||
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
|
||||
Matrix23 Bt = basis().transpose();
|
||||
Vector2 xi = Bt * q.p_.vector();
|
||||
Vector2 xi = basis().transpose() * q.p_;
|
||||
if (H)
|
||||
*H = Bt * q.basis();
|
||||
*H = basis().transpose() * q.basis();
|
||||
return xi;
|
||||
}
|
||||
|
||||
|
|
@ -142,44 +137,34 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
|
|||
/* ************************************************************************* */
|
||||
Unit3 Unit3::retract(const Vector2& v) const {
|
||||
|
||||
// Get the vector form of the point and the basis matrix
|
||||
Vector3 p = p_.vector();
|
||||
Matrix32 B = basis();
|
||||
|
||||
// Compute the 3D xi_hat vector
|
||||
Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
|
||||
|
||||
Vector3 xi_hat = basis() * v;
|
||||
double xi_hat_norm = xi_hat.norm();
|
||||
|
||||
// Avoid nan
|
||||
if (xi_hat_norm == 0.0) {
|
||||
if (v.norm() == 0.0)
|
||||
return Unit3(point3());
|
||||
else
|
||||
return Unit3(-point3());
|
||||
// When v is the so small and approximate as a direction
|
||||
if (xi_hat_norm < 1e-8) {
|
||||
return Unit3(cos(xi_hat_norm) * p_ + xi_hat);
|
||||
}
|
||||
|
||||
Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p
|
||||
Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p_
|
||||
+ sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
|
||||
return Unit3(exp_p_xi_hat);
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector2 Unit3::localCoordinates(const Unit3& y) const {
|
||||
|
||||
Vector3 p = p_.vector(), q = y.p_.vector();
|
||||
double dot = p.dot(q);
|
||||
double dot = p_.dot(y.p_);
|
||||
|
||||
// Check for special cases
|
||||
if (std::abs(dot - 1.0) < 1e-16)
|
||||
return Vector2(0, 0);
|
||||
return Vector2(0.0, 0.0);
|
||||
else if (std::abs(dot + 1.0) < 1e-16)
|
||||
return Vector2(M_PI, 0);
|
||||
return Vector2(M_PI, 0.0);
|
||||
else {
|
||||
// no special case
|
||||
double theta = acos(dot);
|
||||
Vector3 result_hat = (theta / sin(theta)) * (q - p * dot);
|
||||
const double theta = acos(dot);
|
||||
Vector3 result_hat = (theta / sin(theta)) * (y.p_ - p_ * dot);
|
||||
return basis().transpose() * result_hat;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -20,11 +20,16 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
|
||||
#include <boost/random/mersenne_twister.hpp>
|
||||
#include <boost/optional.hpp>
|
||||
#include <boost/random/mersenne_twister.hpp>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -33,7 +38,7 @@ class GTSAM_EXPORT Unit3 {
|
|||
|
||||
private:
|
||||
|
||||
Point3 p_; ///< The location of the point on the unit sphere
|
||||
Vector3 p_; ///< The location of the point on the unit sphere
|
||||
mutable boost::optional<Matrix32> B_; ///< Cached basis
|
||||
|
||||
public:
|
||||
|
|
@ -52,18 +57,18 @@ public:
|
|||
|
||||
/// Construct from point
|
||||
explicit Unit3(const Point3& p) :
|
||||
p_(p / p.norm()) {
|
||||
p_(p.vector().normalized()) {
|
||||
}
|
||||
|
||||
/// Construct from a vector3
|
||||
explicit Unit3(const Vector3& p) :
|
||||
p_(p / p.norm()) {
|
||||
p_(p.normalized()) {
|
||||
}
|
||||
|
||||
/// Construct from x,y,z
|
||||
Unit3(double x, double y, double z) :
|
||||
p_(x, y, z) {
|
||||
p_ = p_ / p_.norm();
|
||||
p_.normalize();
|
||||
}
|
||||
|
||||
/// Named constructor from Point3 with optional Jacobian
|
||||
|
|
@ -83,7 +88,7 @@ public:
|
|||
|
||||
/// The equals function with tolerance
|
||||
bool equals(const Unit3& s, double tol = 1e-9) const {
|
||||
return p_.equals(s.p_, tol);
|
||||
return equal_with_abs_tol(p_, s.p_, tol);
|
||||
}
|
||||
/// @}
|
||||
|
||||
|
|
@ -101,22 +106,22 @@ public:
|
|||
Matrix3 skew() const;
|
||||
|
||||
/// Return unit-norm Point3
|
||||
const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const {
|
||||
Point3 point3(OptionalJacobian<3, 2> H = boost::none) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return Point3(p_);
|
||||
}
|
||||
|
||||
/// Return unit-norm Vector
|
||||
const Vector3& unitVector(boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return p_;
|
||||
}
|
||||
|
||||
/// Return unit-norm Vector
|
||||
Vector3 unitVector(boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return (p_.vector());
|
||||
}
|
||||
|
||||
/// Return scaled direction as Point3
|
||||
friend Point3 operator*(double s, const Unit3& d) {
|
||||
return s * d.p_;
|
||||
return Point3(s * d.p_);
|
||||
}
|
||||
|
||||
/// Signed, vector-valued error between two directions
|
||||
|
|
|
|||
|
|
@ -41,6 +41,7 @@ GTSAM_CONCEPT_MANIFOLD_INST(Unit3)
|
|||
Point3 point3_(const Unit3& p) {
|
||||
return p.point3();
|
||||
}
|
||||
|
||||
TEST(Unit3, point3) {
|
||||
vector<Point3> ps;
|
||||
ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0)
|
||||
|
|
@ -66,7 +67,7 @@ TEST(Unit3, rotate) {
|
|||
Unit3 actual = R * p;
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
Matrix actualH, expectedH;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
|
||||
{
|
||||
expectedH = numericalDerivative21(rotate_, R, p);
|
||||
R.rotate(p, actualH, boost::none);
|
||||
|
|
@ -90,8 +91,8 @@ TEST(Unit3, unrotate) {
|
|||
Unit3 expected = Unit3(1, 1, 0);
|
||||
Unit3 actual = R.unrotate(p);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
|
||||
Matrix actualH, expectedH;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
{
|
||||
expectedH = numericalDerivative21(unrotate_, R, p);
|
||||
R.unrotate(p, actualH, boost::none);
|
||||
|
|
@ -113,7 +114,6 @@ TEST(Unit3, error) {
|
|||
EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
|
||||
|
||||
Matrix actual, expected;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
{
|
||||
expected = numericalDerivative11<Vector2,Unit3>(
|
||||
boost::bind(&Unit3::error, &p, _1, boost::none), q);
|
||||
|
|
@ -153,31 +153,44 @@ TEST(Unit3, distance) {
|
|||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, localCoordinates0) {
|
||||
Unit3 p;
|
||||
Vector actual = p.localCoordinates(p);
|
||||
EXPECT(assert_equal(zero(2), actual, 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, localCoordinates1) {
|
||||
Unit3 p, q(1, 6.12385e-21, 0);
|
||||
Vector actual = p.localCoordinates(q);
|
||||
CHECK(assert_equal(zero(2), actual, 1e-8));
|
||||
}
|
||||
TEST(Unit3, localCoordinates) {
|
||||
{
|
||||
Unit3 p;
|
||||
Vector2 actual = p.localCoordinates(p);
|
||||
EXPECT(assert_equal(zero(2), actual, 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p, q(1, 6.12385e-21, 0);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
CHECK(assert_equal(zero(2), actual, 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p, q(-1, 0, 0);
|
||||
Vector2 expected(M_PI, 0);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
CHECK(assert_equal(expected, actual, 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, localCoordinates2) {
|
||||
Unit3 p, q(-1, 0, 0);
|
||||
Vector expected = (Vector(2) << M_PI, 0).finished();
|
||||
Vector actual = p.localCoordinates(q);
|
||||
CHECK(assert_equal(expected, actual, 1e-8));
|
||||
double twist = 1e-4;
|
||||
{
|
||||
Unit3 p(0, 1, 0), q(0 - twist, -1 + twist, 0);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(actual(0) < 1e-2);
|
||||
EXPECT(actual(1) > M_PI - 1e-2)
|
||||
}
|
||||
{
|
||||
Unit3 p(0, 1, 0), q(0 + twist, -1 - twist, 0);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(actual(0) < 1e-2);
|
||||
EXPECT(actual(1) < -M_PI + 1e-2)
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, basis) {
|
||||
Unit3 p;
|
||||
Matrix expected(3, 2);
|
||||
Matrix32 expected;
|
||||
expected << 0, 0, 0, -1, 1, 0;
|
||||
Matrix actual = p.basis();
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
|
|
@ -185,20 +198,27 @@ TEST(Unit3, basis) {
|
|||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, retract) {
|
||||
Unit3 p;
|
||||
Vector v(2);
|
||||
v << 0.5, 0;
|
||||
Unit3 expected(0.877583, 0, 0.479426);
|
||||
Unit3 actual = p.retract(v);
|
||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
|
||||
{
|
||||
Unit3 p;
|
||||
Vector2 v(0.5, 0);
|
||||
Unit3 expected(0.877583, 0, 0.479426);
|
||||
Unit3 actual = p.retract(v);
|
||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p;
|
||||
Vector2 v(0, 0);
|
||||
Unit3 actual = p.retract(v);
|
||||
EXPECT(assert_equal(p, actual, 1e-6));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, retract_expmap) {
|
||||
Unit3 p;
|
||||
Vector v(2);
|
||||
v << (M_PI / 2.0), 0;
|
||||
Vector2 v((M_PI / 2.0), 0);
|
||||
Unit3 expected(Point3(0, 0, 1));
|
||||
Unit3 actual = p.retract(v);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
|
|
@ -228,9 +248,11 @@ inline static Vector randomVector(const Vector& minLimits,
|
|||
TEST(Unit3, localCoordinates_retract) {
|
||||
|
||||
size_t numIterations = 10000;
|
||||
Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit =
|
||||
Vector3(1.0, 1.0, 1.0);
|
||||
Vector minXiLimit = Vector2(-1.0, -1.0), maxXiLimit = Vector2(1.0, 1.0);
|
||||
Vector3 minSphereLimit(-1.0, -1.0, -1.0);
|
||||
Vector3 maxSphereLimit(1.0, 1.0, 1.0);
|
||||
Vector2 minXiLimit(-1.0, -1.0);
|
||||
Vector2 maxXiLimit(1.0, 1.0);
|
||||
|
||||
for (size_t i = 0; i < numIterations; i++) {
|
||||
|
||||
// Sleep for the random number generator (TODO?: Better create all of them first).
|
||||
|
|
@ -246,9 +268,9 @@ TEST(Unit3, localCoordinates_retract) {
|
|||
|
||||
// Check if the local coordinates and retract return the same results.
|
||||
Vector actual_v12 = s1.localCoordinates(s2);
|
||||
EXPECT(assert_equal(v12, actual_v12, 1e-3));
|
||||
EXPECT(assert_equal(v12, actual_v12, 1e-8));
|
||||
Unit3 actual_s2 = s1.retract(actual_v12);
|
||||
EXPECT(assert_equal(s2, actual_s2, 1e-3));
|
||||
EXPECT(assert_equal(s2, actual_s2, 1e-8));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -258,30 +280,26 @@ TEST(Unit3, localCoordinates_retract) {
|
|||
TEST(Unit3, localCoordinates_retract_expmap) {
|
||||
|
||||
size_t numIterations = 10000;
|
||||
Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit =
|
||||
Vector3(1.0, 1.0, 1.0);
|
||||
Vector minXiLimit = (Vector(2) << -M_PI, -M_PI).finished(), maxXiLimit = (Vector(2) << M_PI, M_PI).finished();
|
||||
Vector3 minSphereLimit = Vector3(-1.0, -1.0, -1.0);
|
||||
Vector3 maxSphereLimit = Vector3(1.0, 1.0, 1.0);
|
||||
Vector2 minXiLimit = Vector2(-M_PI, -M_PI);
|
||||
Vector2 maxXiLimit = Vector2(M_PI, M_PI);
|
||||
|
||||
for (size_t i = 0; i < numIterations; i++) {
|
||||
|
||||
// Sleep for the random number generator (TODO?: Better create all of them first).
|
||||
// boost::this_thread::sleep(boost::posix_time::milliseconds(0));
|
||||
|
||||
// Create the two Unit3s.
|
||||
// Unlike the above case, we can use any two Unit3's.
|
||||
Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
Vector v12 = randomVector(minXiLimit, maxXiLimit);
|
||||
Vector v = randomVector(minXiLimit, maxXiLimit);
|
||||
|
||||
// Magnitude of the rotation can be at most pi
|
||||
if (v12.norm() > M_PI)
|
||||
v12 = v12 / M_PI;
|
||||
Unit3 s2 = s1.retract(v12);
|
||||
if (v.norm() > M_PI)
|
||||
v = v / M_PI;
|
||||
Unit3 s2 = s1.retract(v);
|
||||
|
||||
// Check if the local coordinates and retract return the same results.
|
||||
Vector actual_v12 = s1.localCoordinates(s2);
|
||||
EXPECT(assert_equal(v12, actual_v12, 1e-3));
|
||||
Unit3 actual_s2 = s1.retract(actual_v12);
|
||||
EXPECT(assert_equal(s2, actual_s2, 1e-3));
|
||||
EXPECT(assert_equal(v, s1.localCoordinates(s1.retract(v)), 1e-6));
|
||||
EXPECT(assert_equal(s2, s1.retract(s1.localCoordinates(s2)), 1e-6));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <boost/serialization/extended_type_info.hpp>
|
||||
|
|
@ -241,7 +242,7 @@ namespace gtsam {
|
|||
virtual Matrix R() const { return thisR();}
|
||||
|
||||
/// Compute information matrix
|
||||
virtual Matrix information() const { return thisR().transpose() * thisR(); }
|
||||
virtual Matrix information() const { return R().transpose() * R(); }
|
||||
|
||||
/// Compute covariance matrix
|
||||
virtual Matrix covariance() const { return information().inverse(); }
|
||||
|
|
|
|||
|
|
@ -33,19 +33,12 @@ using namespace gtsam;
|
|||
using namespace noiseModel;
|
||||
using namespace boost::assign;
|
||||
|
||||
static const double kSigma = 2, s_1=1.0/kSigma, kVariance = kSigma*kSigma, prc = 1.0/kVariance;
|
||||
static const Matrix R = (Matrix(3, 3) <<
|
||||
s_1, 0.0, 0.0,
|
||||
0.0, s_1, 0.0,
|
||||
0.0, 0.0, s_1).finished();
|
||||
static const Matrix kCovariance = (Matrix(3, 3) <<
|
||||
kVariance, 0.0, 0.0,
|
||||
0.0, kVariance, 0.0,
|
||||
0.0, 0.0, kVariance).finished();
|
||||
static const double kSigma = 2, kInverseSigma = 1.0 / kSigma,
|
||||
kVariance = kSigma * kSigma, prc = 1.0 / kVariance;
|
||||
static const Matrix R = Matrix3::Identity() * kInverseSigma;
|
||||
static const Matrix kCovariance = Matrix3::Identity() * kVariance;
|
||||
static const Vector3 kSigmas(kSigma, kSigma, kSigma);
|
||||
|
||||
//static double inf = numeric_limits<double>::infinity();
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(NoiseModel, constructors)
|
||||
{
|
||||
|
|
@ -58,8 +51,8 @@ TEST(NoiseModel, constructors)
|
|||
m.push_back(Gaussian::Covariance(kCovariance,false));
|
||||
m.push_back(Gaussian::Information(kCovariance.inverse(),false));
|
||||
m.push_back(Diagonal::Sigmas(kSigmas,false));
|
||||
m.push_back(Diagonal::Variances((Vector(3) << kVariance, kVariance, kVariance).finished(),false));
|
||||
m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished(),false));
|
||||
m.push_back(Diagonal::Variances((Vector3(kVariance, kVariance, kVariance)),false));
|
||||
m.push_back(Diagonal::Precisions(Vector3(prc, prc, prc),false));
|
||||
m.push_back(Isotropic::Sigma(3, kSigma,false));
|
||||
m.push_back(Isotropic::Variance(3, kVariance,false));
|
||||
m.push_back(Isotropic::Precision(3, prc,false));
|
||||
|
|
@ -82,25 +75,23 @@ TEST(NoiseModel, constructors)
|
|||
DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9);
|
||||
|
||||
// test R matrix
|
||||
Matrix expectedR((Matrix(3, 3) <<
|
||||
s_1, 0.0, 0.0,
|
||||
0.0, s_1, 0.0,
|
||||
0.0, 0.0, s_1).finished());
|
||||
|
||||
BOOST_FOREACH(Gaussian::shared_ptr mi, m)
|
||||
EXPECT(assert_equal(expectedR,mi->R()));
|
||||
EXPECT(assert_equal(R,mi->R()));
|
||||
|
||||
// test covariance
|
||||
BOOST_FOREACH(Gaussian::shared_ptr mi, m)
|
||||
EXPECT(assert_equal(kCovariance,mi->covariance()));
|
||||
|
||||
// test covariance
|
||||
BOOST_FOREACH(Gaussian::shared_ptr mi, m)
|
||||
EXPECT(assert_equal(kCovariance.inverse(),mi->information()));
|
||||
|
||||
// test Whiten operator
|
||||
Matrix H((Matrix(3, 4) <<
|
||||
0.0, 0.0, 1.0, 1.0,
|
||||
0.0, 1.0, 0.0, 1.0,
|
||||
1.0, 0.0, 0.0, 1.0).finished());
|
||||
|
||||
Matrix expected((Matrix(3, 4) <<
|
||||
0.0, 0.0, s_1, s_1,
|
||||
0.0, s_1, 0.0, s_1,
|
||||
s_1, 0.0, 0.0, s_1).finished());
|
||||
|
||||
Matrix expected = kInverseSigma * H;
|
||||
BOOST_FOREACH(Gaussian::shared_ptr mi, m)
|
||||
EXPECT(assert_equal(expected,mi->Whiten(H)));
|
||||
|
||||
|
|
@ -122,7 +113,7 @@ TEST(NoiseModel, equals)
|
|||
{
|
||||
Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R),
|
||||
g2 = Gaussian::SqrtInformation(eye(3,3));
|
||||
Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << kSigma, kSigma, kSigma).finished()),
|
||||
Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector3(kSigma, kSigma, kSigma)),
|
||||
d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3));
|
||||
Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma),
|
||||
i2 = Isotropic::Sigma(3, 0.7);
|
||||
|
|
@ -141,7 +132,7 @@ TEST(NoiseModel, equals)
|
|||
///* ************************************************************************* */
|
||||
//TEST(NoiseModel, ConstrainedSmart )
|
||||
//{
|
||||
// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma), true);
|
||||
// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector3(sigma, 0.0, sigma), true);
|
||||
// Diagonal::shared_ptr n1 = boost::dynamic_pointer_cast<Diagonal>(nonconstrained);
|
||||
// Constrained::shared_ptr n2 = boost::dynamic_pointer_cast<Constrained>(nonconstrained);
|
||||
// EXPECT(n1);
|
||||
|
|
@ -160,8 +151,8 @@ TEST(NoiseModel, ConstrainedConstructors )
|
|||
Constrained::shared_ptr actual;
|
||||
size_t d = 3;
|
||||
double m = 100.0;
|
||||
Vector sigmas = (Vector(3) << kSigma, 0.0, 0.0).finished();
|
||||
Vector mu = Vector3(200.0, 300.0, 400.0);
|
||||
Vector3 sigmas(kSigma, 0.0, 0.0);
|
||||
Vector3 mu(200.0, 300.0, 400.0);
|
||||
actual = Constrained::All(d);
|
||||
// TODO: why should this be a thousand ??? Dummy variable?
|
||||
EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));
|
||||
|
|
@ -187,7 +178,7 @@ TEST(NoiseModel, ConstrainedMixed )
|
|||
{
|
||||
Vector feasible = Vector3(1.0, 0.0, 1.0),
|
||||
infeasible = Vector3(1.0, 1.0, 1.0);
|
||||
Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << kSigma, 0.0, kSigma).finished());
|
||||
Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector3(kSigma, 0.0, kSigma));
|
||||
// NOTE: we catch constrained variables elsewhere, so whitening does nothing
|
||||
EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible)));
|
||||
EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible)));
|
||||
|
|
@ -346,7 +337,7 @@ TEST(NoiseModel, robustNoise)
|
|||
{
|
||||
const double k = 10.0, error1 = 1.0, error2 = 100.0;
|
||||
Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0).finished();
|
||||
Vector b = (Vector(2) << error1, error2).finished();
|
||||
Vector b = Vector2(error1, error2);
|
||||
const Robust::shared_ptr robust = Robust::Create(
|
||||
mEstimator::Huber::Create(k, mEstimator::Huber::Scalar),
|
||||
Unit::Create(2));
|
||||
|
|
|
|||
Loading…
Reference in New Issue