Some new methods and improvements to Unit3 from Skydio
Frank
parent
0273d94cfd
commit
d087f2fdf8
|
|
@ -15,12 +15,14 @@
|
|||
* @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 <gtsam/config.h> // GTSAM_USE_TBB
|
||||
|
||||
#include <boost/random/mersenne_twister.hpp>
|
||||
#include <gtsam/config.h> // for GTSAM_USE_TBB
|
||||
|
||||
#ifdef __clang__
|
||||
|
|
@ -32,13 +34,8 @@
|
|||
# pragma clang diagnostic pop
|
||||
#endif
|
||||
|
||||
#ifdef GTSAM_USE_TBB
|
||||
#include <tbb/mutex.h>
|
||||
#endif
|
||||
|
||||
#include <boost/random/variate_generator.hpp>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -46,14 +43,12 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
|
||||
Unit3 direction(point);
|
||||
if (H) {
|
||||
// 3*3 Derivative of representation with respect to point is 3*3:
|
||||
Matrix3 D_p_point;
|
||||
point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
|
||||
// Calculate the 2*3 Jacobian
|
||||
Unit3 direction;
|
||||
direction.p_ = point.normalize(H ? &D_p_point : 0);
|
||||
if (H)
|
||||
*H << direction.basis().transpose() * D_p_point;
|
||||
}
|
||||
return direction;
|
||||
}
|
||||
|
||||
|
|
@ -63,49 +58,105 @@ 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();
|
||||
return Unit3(d[0], d[1], d[2]);
|
||||
Unit3 result;
|
||||
result.p_ = Point3(d[0], d[1], d[2]);
|
||||
return result;
|
||||
}
|
||||
|
||||
#ifdef GTSAM_USE_TBB
|
||||
tbb::mutex unit3BasisMutex;
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
const Matrix32& Unit3::basis() const {
|
||||
const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
|
||||
#ifdef GTSAM_USE_TBB
|
||||
tbb::mutex::scoped_lock lock(unit3BasisMutex);
|
||||
// NOTE(hayk): At some point it seemed like this reproducably resulted in deadlock. However, I
|
||||
// can't see the reason why and I can no longer reproduce it. It may have been a red herring, or
|
||||
// there is still a latent bug to watch out for.
|
||||
tbb::mutex::scoped_lock lock(B_mutex_);
|
||||
#endif
|
||||
|
||||
// Return cached version if exists
|
||||
if (B_) return *B_;
|
||||
// Return cached basis if available and the Jacobian isn't needed.
|
||||
if (B_ && !H) {
|
||||
return *B_;
|
||||
}
|
||||
|
||||
// Return cached basis and derivatives if available.
|
||||
if (B_ && H && H_B_) {
|
||||
*H = *H_B_;
|
||||
return *B_;
|
||||
}
|
||||
|
||||
// Get the unit vector and derivative wrt this.
|
||||
// NOTE(hayk): We can't call point3(), because it would recursively call basis().
|
||||
const Point3& n = p_;
|
||||
|
||||
// Get the axis of rotation with the minimum projected length of the point
|
||||
Vector3 axis;
|
||||
double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z());
|
||||
if ((mx <= my) && (mx <= mz))
|
||||
axis = Vector3(1.0, 0.0, 0.0);
|
||||
else if ((my <= mx) && (my <= mz))
|
||||
axis = Vector3(0.0, 1.0, 0.0);
|
||||
else if ((mz <= mx) && (mz <= my))
|
||||
axis = Vector3(0.0, 0.0, 1.0);
|
||||
else
|
||||
Point3 axis;
|
||||
double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
|
||||
if ((mx <= my) && (mx <= mz)) {
|
||||
axis = Point3(1.0, 0.0, 0.0);
|
||||
} else if ((my <= mx) && (my <= mz)) {
|
||||
axis = Point3(0.0, 1.0, 0.0);
|
||||
} else if ((mz <= mx) && (mz <= my)) {
|
||||
axis = Point3(0.0, 0.0, 1.0);
|
||||
} else {
|
||||
assert(false);
|
||||
}
|
||||
|
||||
// Create the two basis vectors
|
||||
Vector3 b1 = p_.cross(axis).normalized();
|
||||
Vector3 b2 = p_.cross(b1).normalized();
|
||||
// Choose the direction of the first basis vector b1 in the tangent plane by crossing n with
|
||||
// the chosen axis.
|
||||
Matrix33 H_B1_n;
|
||||
Point3 B1 = n.cross(axis, H ? &H_B1_n : nullptr);
|
||||
|
||||
// Create the basis matrix
|
||||
// Normalize result to get a unit vector: b1 = B1 / |B1|.
|
||||
Matrix33 H_b1_B1;
|
||||
Point3 b1 = B1.normalize(H ? &H_b1_B1 : nullptr);
|
||||
|
||||
// Get the second basis vector b2, which is orthogonal to n and b1, by crossing them.
|
||||
// No need to normalize this, p and b1 are orthogonal unit vectors.
|
||||
Matrix33 H_b2_n, H_b2_b1;
|
||||
Point3 b2 = n.cross(b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr);
|
||||
|
||||
// Create the basis by stacking b1 and b2.
|
||||
B_.reset(Matrix32());
|
||||
(*B_) << b1, b2;
|
||||
(*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
|
||||
|
||||
if (H) {
|
||||
// Chain rule tomfoolery to compute the derivative.
|
||||
const Matrix32& H_n_p = *B_;
|
||||
Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p;
|
||||
Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
|
||||
|
||||
// Cache the derivative and fill the result.
|
||||
H_B_.reset(Matrix62());
|
||||
(*H_B_) << H_b1_p, H_b2_p;
|
||||
*H = *H_B_;
|
||||
}
|
||||
|
||||
return *B_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// The print fuction
|
||||
const Point3& Unit3::point3(OptionalJacobian<3, 2> H) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return p_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Unit3::unitVector(boost::optional<Matrix&> H) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return (p_.vector());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::ostream& operator<<(std::ostream& os, const Unit3& pair) {
|
||||
os << pair.p_ << endl;
|
||||
return os;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Unit3::print(const std::string& s) const {
|
||||
cout << s << ":" << p_ << endl;
|
||||
}
|
||||
|
|
@ -116,11 +167,72 @@ Matrix3 Unit3::skew() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
|
||||
double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const {
|
||||
// Get the unit vectors of each, and the derivative.
|
||||
Matrix32 H_pn_p;
|
||||
const Point3& pn = point3(H_p ? &H_pn_p : 0);
|
||||
|
||||
Matrix32 H_qn_q;
|
||||
const Point3& qn = q.point3(H_q ? &H_qn_q : 0);
|
||||
|
||||
// Compute the dot product of the Point3s.
|
||||
Matrix13 H_dot_pn, H_dot_qn;
|
||||
double d = pn.dot(qn, H_p ? &H_dot_pn : nullptr, H_q ? &H_dot_qn : nullptr);
|
||||
|
||||
if (H_p) {
|
||||
(*H_p) << H_dot_pn * H_pn_p;
|
||||
}
|
||||
|
||||
if (H_q) {
|
||||
(*H_q) = H_dot_qn * H_qn_q;
|
||||
}
|
||||
|
||||
return d;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const {
|
||||
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
|
||||
Vector2 xi = basis().transpose() * q.p_;
|
||||
if (H)
|
||||
*H = basis().transpose() * q.basis();
|
||||
Matrix23 Bt = basis().transpose();
|
||||
Vector2 xi = Bt * q.p_.vector();
|
||||
if (H_q) {
|
||||
*H_q = Bt * q.basis();
|
||||
}
|
||||
return xi;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const {
|
||||
// Get the point3 of this, and the derivative.
|
||||
Matrix32 H_qn_q;
|
||||
const Point3& qn = q.point3(H_q ? &H_qn_q : 0);
|
||||
|
||||
// 2D error here is projecting q into the tangent plane of this (p).
|
||||
Matrix62 H_B_p;
|
||||
Matrix23 Bt = basis(H_p ? &H_B_p : nullptr).transpose();
|
||||
Vector2 xi = Bt * qn.vector();
|
||||
|
||||
if (H_p) {
|
||||
// Derivatives of each basis vector.
|
||||
const Matrix32& H_b1_p = H_B_p.block<3, 2>(0, 0);
|
||||
const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0);
|
||||
|
||||
// Derivatives of the two entries of xi wrt the basis vectors.
|
||||
Matrix13 H_xi1_b1 = qn.vector().transpose();
|
||||
Matrix13 H_xi2_b2 = qn.vector().transpose();
|
||||
|
||||
// Assemble dxi/dp = dxi/dB * dB/dp.
|
||||
Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p;
|
||||
Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p;
|
||||
*H_p << H_xi1_p, H_xi2_p;
|
||||
}
|
||||
|
||||
if (H_q) {
|
||||
// dxi/dq is given by dxi/dqu * dqu/dq, where qu is the unit vector of q.
|
||||
Matrix23 H_xi_qu = Bt;
|
||||
*H_q = H_xi_qu * H_qn_q;
|
||||
}
|
||||
|
||||
return xi;
|
||||
}
|
||||
|
||||
|
|
@ -136,39 +248,46 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Unit3 Unit3::retract(const Vector2& v) const {
|
||||
// Compute the 3D xi_hat vector
|
||||
Vector3 xi_hat = basis() * v;
|
||||
double theta = xi_hat.norm();
|
||||
|
||||
// Treat case of very small v differently
|
||||
if (theta < std::numeric_limits<double>::epsilon()) {
|
||||
return Unit3(cos(theta) * p_ + xi_hat);
|
||||
// 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);
|
||||
|
||||
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());
|
||||
}
|
||||
|
||||
Vector3 exp_p_xi_hat =
|
||||
cos(theta) * p_ + xi_hat * (sin(theta) / theta);
|
||||
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& other) const {
|
||||
const double x = p_.dot(other.p_);
|
||||
// Crucial quantity here is y = theta/sin(theta) with theta=acos(x)
|
||||
// Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2)
|
||||
// We treat the special case 1 and -1 below
|
||||
const double x2 = x * x;
|
||||
const double z = 1 - x2;
|
||||
double y;
|
||||
if (z < std::numeric_limits<double>::epsilon()) {
|
||||
if (x > 0) // first order expansion at x=1
|
||||
y = 1.0 - (x - 1.0) / 3.0;
|
||||
else // cop out
|
||||
return Vector2(M_PI, 0.0);
|
||||
} else {
|
||||
Vector2 Unit3::localCoordinates(const Unit3& y) const {
|
||||
|
||||
Vector3 p = p_.vector(), q = y.p_.vector();
|
||||
double dot = p.dot(q);
|
||||
|
||||
// Check for special cases
|
||||
if (dot > 1.0 - 1e-16)
|
||||
return Vector2(0, 0);
|
||||
else if (dot < -1.0 + 1e-16)
|
||||
return Vector2(M_PI, 0);
|
||||
else {
|
||||
// no special case
|
||||
y = acos(x) / sqrt(z);
|
||||
double theta = acos(dot);
|
||||
Vector3 result_hat = (theta / sin(theta)) * (q - p * dot);
|
||||
return basis().transpose() * result_hat;
|
||||
}
|
||||
return basis().transpose() * y * (other.p_ - x * p_);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -20,16 +20,16 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
|
||||
#include <boost/optional.hpp>
|
||||
#include <boost/random/mersenne_twister.hpp>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
#include <string>
|
||||
#ifdef GTSAM_USE_TBB
|
||||
#include <tbb/mutex.h>
|
||||
#endif
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -38,8 +38,13 @@ class GTSAM_EXPORT Unit3 {
|
|||
|
||||
private:
|
||||
|
||||
Vector3 p_; ///< The location of the point on the unit sphere
|
||||
Point3 p_; ///< The location of the point on the unit sphere
|
||||
mutable boost::optional<Matrix32> B_; ///< Cached basis
|
||||
mutable boost::optional<Matrix62> H_B_; ///< Cached basis derivative
|
||||
|
||||
#ifdef GTSAM_USE_TBB
|
||||
mutable tbb::mutex B_mutex_; ///< Mutex to protect the cached basis.
|
||||
#endif
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -57,18 +62,23 @@ public:
|
|||
|
||||
/// Construct from point
|
||||
explicit Unit3(const Point3& p) :
|
||||
p_(p.vector().normalized()) {
|
||||
p_(p.normalize()) {
|
||||
}
|
||||
|
||||
/// Construct from a vector3
|
||||
explicit Unit3(const Vector3& p) :
|
||||
p_(p.normalized()) {
|
||||
explicit Unit3(const Vector3& v) :
|
||||
p_(v / v.norm()) {
|
||||
}
|
||||
|
||||
/// Construct from x,y,z
|
||||
Unit3(double x, double y, double z) :
|
||||
p_(x, y, z) {
|
||||
p_.normalize();
|
||||
p_(Point3(x, y, z).normalize()) {
|
||||
}
|
||||
|
||||
/// Construct from 2D point in plane at focal length f
|
||||
/// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point
|
||||
explicit Unit3(const Point2& p, double f=1.0) :
|
||||
p_(Point3(p.x(), p.y(), f).normalize()) {
|
||||
}
|
||||
|
||||
/// Named constructor from Point3 with optional Jacobian
|
||||
|
|
@ -83,12 +93,14 @@ public:
|
|||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
friend std::ostream& operator<<(std::ostream& os, const Unit3& pair);
|
||||
|
||||
/// The print fuction
|
||||
void print(const std::string& s = std::string()) const;
|
||||
|
||||
/// The equals function with tolerance
|
||||
bool equals(const Unit3& s, double tol = 1e-9) const {
|
||||
return equal_with_abs_tol(p_, s.p_, tol);
|
||||
return p_.equals(s.p_, tol);
|
||||
}
|
||||
/// @}
|
||||
|
||||
|
|
@ -99,37 +111,50 @@ public:
|
|||
* Returns the local coordinate frame to tangent plane
|
||||
* It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
|
||||
* tangent to the sphere at the current direction.
|
||||
* Provides derivatives of the basis with the two basis vectors stacked up as a 6x1.
|
||||
*/
|
||||
const Matrix32& basis() const;
|
||||
const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
|
||||
|
||||
/// Return skew-symmetric associated with 3D point on unit sphere
|
||||
Matrix3 skew() const;
|
||||
|
||||
/// Return unit-norm Point3
|
||||
Point3 point3(OptionalJacobian<3, 2> H = boost::none) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return Point3(p_);
|
||||
}
|
||||
const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const;
|
||||
|
||||
/// Return unit-norm Vector
|
||||
const Vector3& unitVector(boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return p_;
|
||||
}
|
||||
Vector3 unitVector(boost::optional<Matrix&> H = boost::none) const;
|
||||
|
||||
/// Return scaled direction as Point3
|
||||
friend Point3 operator*(double s, const Unit3& d) {
|
||||
return Point3(s * d.p_);
|
||||
return s * d.p_;
|
||||
}
|
||||
|
||||
/// Return dot product with q
|
||||
double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
|
||||
OptionalJacobian<1,2> H2 = boost::none) const;
|
||||
|
||||
/// Signed, vector-valued error between two directions
|
||||
Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const;
|
||||
/// @deprecated, errorVector has the proper derivatives, this confusingly has only the second.
|
||||
Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
|
||||
|
||||
/// Signed, vector-valued error between two directions
|
||||
/// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal.
|
||||
Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
|
||||
OptionalJacobian<2, 2> H_q = boost::none) const;
|
||||
|
||||
/// Distance between two directions
|
||||
double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
|
||||
|
||||
/// Cross-product between two Unit3s
|
||||
Unit3 cross(const Unit3& q) const {
|
||||
return Unit3(p_.cross(q.p_));
|
||||
}
|
||||
|
||||
/// Cross-product w Point3
|
||||
Point3 cross(const Point3& q) const {
|
||||
return Point3(p_.vector().cross(q.vector()));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Manifold
|
||||
|
|
@ -147,7 +172,7 @@ public:
|
|||
|
||||
enum CoordinatesMode {
|
||||
EXPMAP, ///< Use the exponential map to retract
|
||||
RENORM ///< Retract with vector addition and renormalize.
|
||||
RENORM ///< Retract with vector addtion and renormalize.
|
||||
};
|
||||
|
||||
/// The retract function
|
||||
|
|
@ -167,6 +192,13 @@ private:
|
|||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(p_);
|
||||
// homebrew serialize Eigen Matrix
|
||||
ar & boost::serialization::make_nvp("B11", (*B_)(0, 0));
|
||||
ar & boost::serialization::make_nvp("B12", (*B_)(0, 1));
|
||||
ar & boost::serialization::make_nvp("B21", (*B_)(1, 0));
|
||||
ar & boost::serialization::make_nvp("B22", (*B_)(1, 1));
|
||||
ar & boost::serialization::make_nvp("B31", (*B_)(2, 0));
|
||||
ar & boost::serialization::make_nvp("B32", (*B_)(2, 1));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
|
|
@ -22,19 +22,26 @@
|
|||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/slam/expressions.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/random.hpp>
|
||||
//#include <boost/thread.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace boost::assign;
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
using gtsam::symbol_shorthand::U;
|
||||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(Unit3)
|
||||
GTSAM_CONCEPT_MANIFOLD_INST(Unit3)
|
||||
|
|
@ -43,7 +50,6 @@ 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)
|
||||
|
|
@ -69,7 +75,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);
|
||||
|
|
@ -93,8 +99,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);
|
||||
|
|
@ -107,6 +113,37 @@ TEST(Unit3, unrotate) {
|
|||
}
|
||||
}
|
||||
|
||||
TEST(Unit3, dot) {
|
||||
Unit3 p(1, 0.2, 0.3);
|
||||
Unit3 q = p.retract(Vector2(0.5, 0));
|
||||
Unit3 r = p.retract(Vector2(0.8, 0));
|
||||
Unit3 t = p.retract(Vector2(0, 0.3));
|
||||
EXPECT(assert_equal(1.0, p.dot(p), 1e-8));
|
||||
EXPECT(assert_equal(0.877583, p.dot(q), 1e-5));
|
||||
EXPECT(assert_equal(0.696707, p.dot(r), 1e-5));
|
||||
EXPECT(assert_equal(0.955336, p.dot(t), 1e-5));
|
||||
|
||||
// Use numerical derivatives to calculate the expected Jacobians
|
||||
Matrix H1, H2;
|
||||
boost::function<double(const Unit3&, const Unit3&)> f = boost::bind(&Unit3::dot, _1, _2, //
|
||||
boost::none, boost::none);
|
||||
{
|
||||
p.dot(q, H1, H2);
|
||||
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-9));
|
||||
EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, q), H2, 1e-9));
|
||||
}
|
||||
{
|
||||
p.dot(r, H1, H2);
|
||||
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, r), H1, 1e-9));
|
||||
EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, r), H2, 1e-9));
|
||||
}
|
||||
{
|
||||
p.dot(t, H1, H2);
|
||||
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, t), H1, 1e-9));
|
||||
EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, t), H2, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, error) {
|
||||
Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
|
||||
|
|
@ -116,6 +153,7 @@ 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);
|
||||
|
|
@ -130,6 +168,45 @@ TEST(Unit3, error) {
|
|||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, error2) {
|
||||
Unit3 p(0.1, -0.2, 0.8);
|
||||
Unit3 q = p.retract(Vector2(0.2, -0.1));
|
||||
Unit3 r = p.retract(Vector2(0.8, 0));
|
||||
|
||||
// Hard-coded as simple regression values
|
||||
EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-8));
|
||||
EXPECT(assert_equal((Vector)(Vector2(0.198337495, -0.0991687475)), p.errorVector(q), 1e-5));
|
||||
EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.errorVector(r), 1e-5));
|
||||
|
||||
Matrix actual, expected;
|
||||
// Use numerical derivatives to calculate the expected Jacobian
|
||||
{
|
||||
expected = numericalDerivative21<Vector2, Unit3, Unit3>(
|
||||
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
|
||||
p.errorVector(q, actual, boost::none);
|
||||
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||
}
|
||||
{
|
||||
expected = numericalDerivative21<Vector2, Unit3, Unit3>(
|
||||
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
|
||||
p.errorVector(r, actual, boost::none);
|
||||
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||
}
|
||||
{
|
||||
expected = numericalDerivative22<Vector2, Unit3, Unit3>(
|
||||
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
|
||||
p.errorVector(q, boost::none, actual);
|
||||
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||
}
|
||||
{
|
||||
expected = numericalDerivative22<Vector2, Unit3, Unit3>(
|
||||
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
|
||||
p.errorVector(r, boost::none, actual);
|
||||
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, distance) {
|
||||
Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
|
||||
|
|
@ -155,107 +232,211 @@ TEST(Unit3, distance) {
|
|||
}
|
||||
|
||||
//*******************************************************************************
|
||||
|
||||
TEST(Unit3, localCoordinates) {
|
||||
{
|
||||
Unit3 p, q;
|
||||
Vector2 expected = Vector2::Zero();
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
TEST(Unit3, localCoordinates0) {
|
||||
Unit3 p;
|
||||
Vector actual = p.localCoordinates(p);
|
||||
EXPECT(assert_equal(zero(2), actual, 1e-8));
|
||||
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p, q(1, 6.12385e-21, 0);
|
||||
Vector2 expected = Vector2::Zero();
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(assert_equal(zero(2), actual, 1e-8));
|
||||
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p, q(-1, 0, 0);
|
||||
Vector2 expected(M_PI, 0);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p, q(0, 1, 0);
|
||||
Vector2 expected(0,-M_PI_2);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p, q(0, -1, 0);
|
||||
Vector2 expected(0, M_PI_2);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p(0,1,0), q(0,-1,0);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(assert_equal(q, p.retract(actual), 1e-8));
|
||||
}
|
||||
{
|
||||
Unit3 p(0,0,1), q(0,0,-1);
|
||||
Vector2 actual = p.localCoordinates(q);
|
||||
EXPECT(assert_equal(q, p.retract(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, localCoordinates1) {
|
||||
Unit3 p, q(1, 6.12385e-21, 0);
|
||||
Vector actual = p.localCoordinates(q);
|
||||
CHECK(assert_equal(zero(2), 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));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
// Wrapper to make basis return a vector6 so we can test numerical derivatives.
|
||||
Vector6 BasisTest(const Unit3& p, OptionalJacobian<6, 2> H) {
|
||||
Matrix32 B = p.basis(H);
|
||||
Vector6 B_vec;
|
||||
B_vec << B;
|
||||
return B_vec;
|
||||
}
|
||||
|
||||
TEST(Unit3, basis) {
|
||||
Unit3 p;
|
||||
Matrix32 expected;
|
||||
expected << 0, 0, 0, -1, 1, 0;
|
||||
Matrix actual = p.basis();
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
Unit3 p(0.1, -0.2, 0.9);
|
||||
|
||||
Matrix expected(3, 2);
|
||||
expected << 0.0, -0.994169047, 0.97618706,
|
||||
-0.0233922129, 0.216930458, 0.105264958;
|
||||
|
||||
Matrix62 actualH;
|
||||
Matrix actual = p.basis(actualH);
|
||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||
|
||||
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
|
||||
boost::bind(BasisTest, _1, boost::none), p);
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
/// Check the basis derivatives of a bunch of random Unit3s.
|
||||
TEST(Unit3, basis_derivatives) {
|
||||
int num_tests = 100;
|
||||
boost::mt19937 rng(42);
|
||||
for (int i = 0; i < num_tests; i++) {
|
||||
Unit3 p = Unit3::Random(rng);
|
||||
|
||||
Matrix62 actualH;
|
||||
p.basis(actualH);
|
||||
|
||||
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
|
||||
boost::bind(BasisTest, _1, boost::none), p);
|
||||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, retract) {
|
||||
{
|
||||
Unit3 p;
|
||||
Vector2 v(0.5, 0);
|
||||
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, 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;
|
||||
Vector2 v((M_PI / 2.0), 0);
|
||||
Vector v(2);
|
||||
v << (M_PI / 2.0), 0;
|
||||
Unit3 expected(Point3(0, 0, 1));
|
||||
Unit3 actual = p.retract(v);
|
||||
EXPECT(assert_equal(expected, actual, 1e-8));
|
||||
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
/// Returns a random vector
|
||||
inline static Vector randomVector(const Vector& minLimits,
|
||||
const Vector& maxLimits) {
|
||||
|
||||
// Get the number of dimensions and create the return vector
|
||||
size_t numDims = dim(minLimits);
|
||||
Vector vector = zero(numDims);
|
||||
|
||||
// Create the random vector
|
||||
for (size_t i = 0; i < numDims; i++) {
|
||||
double range = maxLimits(i) - minLimits(i);
|
||||
vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i);
|
||||
}
|
||||
return vector;
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
// Let x and y be two Unit3's.
|
||||
// The equality x.localCoordinates(x.retract(v)) == v should hold.
|
||||
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);
|
||||
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.
|
||||
// NOTE: You can not create two totally random Unit3's because you cannot always compute
|
||||
// between two any Unit3's. (For instance, they might be at the different sides of the circle).
|
||||
Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
|
||||
Vector v12 = randomVector(minXiLimit, maxXiLimit);
|
||||
Unit3 s2 = s1.retract(v12);
|
||||
|
||||
// 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));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
// Let x and y be two Unit3's.
|
||||
// The equality x.localCoordinates(x.retract(v)) == v should hold.
|
||||
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();
|
||||
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);
|
||||
|
||||
// Magnitude of the rotation can be at most pi
|
||||
if (v12.norm() > M_PI)
|
||||
v12 = v12 / M_PI;
|
||||
Unit3 s2 = s1.retract(v12);
|
||||
|
||||
// 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));
|
||||
}
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
//TEST( Pose2, between )
|
||||
//{
|
||||
// // <
|
||||
// //
|
||||
// // ^
|
||||
// //
|
||||
// // *--0--*--*
|
||||
// Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
|
||||
// Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
|
||||
//
|
||||
// Matrix actualH1,actualH2;
|
||||
// Pose2 expected(M_PI/2.0, Point2(2,2));
|
||||
// Pose2 actual1 = gT1.between(gT2);
|
||||
// Pose2 actual2 = gT1.between(gT2,actualH1,actualH2);
|
||||
// EXPECT(assert_equal(expected,actual1));
|
||||
// EXPECT(assert_equal(expected,actual2));
|
||||
//
|
||||
// Matrix expectedH1 = (Matrix(3,3) <<
|
||||
// 0.0,-1.0,-2.0,
|
||||
// 1.0, 0.0,-2.0,
|
||||
// 0.0, 0.0,-1.0
|
||||
// );
|
||||
// Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
|
||||
// EXPECT(assert_equal(expectedH1,actualH1));
|
||||
// EXPECT(assert_equal(numericalH1,actualH1));
|
||||
// // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
|
||||
// EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
|
||||
//
|
||||
// Matrix expectedH2 = (Matrix(3,3) <<
|
||||
// 1.0, 0.0, 0.0,
|
||||
// 0.0, 1.0, 0.0,
|
||||
// 0.0, 0.0, 1.0
|
||||
// );
|
||||
// Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
|
||||
// EXPECT(assert_equal(expectedH2,actualH2));
|
||||
// EXPECT(assert_equal(numericalH2,actualH2));
|
||||
//
|
||||
//}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, Random) {
|
||||
boost::mt19937 rng(42);
|
||||
|
|
@ -267,26 +448,6 @@ TEST(Unit3, Random) {
|
|||
EXPECT(assert_equal(expectedMean,actualMean,0.1));
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
// New test that uses Unit3::Random
|
||||
TEST(Unit3, localCoordinates_retract) {
|
||||
boost::mt19937 rng(42);
|
||||
size_t numIterations = 10000;
|
||||
|
||||
for (size_t i = 0; i < numIterations; i++) {
|
||||
// Create two random Unit3s
|
||||
const Unit3 s1 = Unit3::Random(rng);
|
||||
const Unit3 s2 = Unit3::Random(rng);
|
||||
// Check that they are not at opposite ends of the sphere, which is ill defined
|
||||
if (s1.unitVector().dot(s2.unitVector())<-0.9) continue;
|
||||
|
||||
// Check if the local coordinates and retract return consistent results.
|
||||
Vector v12 = s1.localCoordinates(s2);
|
||||
Unit3 actual_s2 = s1.retract(v12);
|
||||
EXPECT(assert_equal(s2, actual_s2, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST (Unit3, FromPoint3) {
|
||||
Matrix actualH;
|
||||
|
|
@ -298,12 +459,42 @@ TEST (Unit3, FromPoint3) {
|
|||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(actualH, Serialization) {
|
||||
Unit3 p(0, 1, 0);
|
||||
EXPECT(serializationTestHelpers::equalsObj(p));
|
||||
EXPECT(serializationTestHelpers::equalsXML(p));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(p));
|
||||
//*******************************************************************************
|
||||
TEST(Unit3, ErrorBetweenFactor) {
|
||||
std::vector<Unit3> data = {Unit3(1.0, 0.0, 0.0), Unit3(0.0, 0.0, 1.0)};
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
Values initial_values;
|
||||
|
||||
// Add prior factors.
|
||||
SharedNoiseModel R_prior = noiseModel::Unit::Create(2);
|
||||
for (int i = 0; i < data.size(); i++) {
|
||||
graph.add(PriorFactor<Unit3>(U(i), data[i], R_prior));
|
||||
}
|
||||
|
||||
// Add process factors using the dot product error function.
|
||||
SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01);
|
||||
for (int i = 0; i < data.size() - 1; i++) {
|
||||
Expression<Vector2> exp(Expression<Unit3>(U(i)), &Unit3::errorVector, Expression<Unit3>(U(i + 1)));
|
||||
graph.addExpressionFactor<Vector2>(R_process, Vector2::Zero(), exp);
|
||||
}
|
||||
|
||||
// Add initial values. Since there is no identity, just pick something.
|
||||
for (int i = 0; i < data.size(); i++) {
|
||||
initial_values.insert(U(i), Unit3(0.0, 1.0, 0.0));
|
||||
}
|
||||
|
||||
Values values = GaussNewtonOptimizer(graph, initial_values).optimize();
|
||||
|
||||
// Check that the y-value is very small for each.
|
||||
for (int i = 0; i < data.size(); i++) {
|
||||
EXPECT(assert_equal(0.0, values.at<Unit3>(U(i)).unitVector().y(), 1e-3));
|
||||
}
|
||||
|
||||
// Check that the dot product between variables is close to 1.
|
||||
for (int i = 0; i < data.size() - 1; i++) {
|
||||
EXPECT(assert_equal(1.0, values.at<Unit3>(U(i)).dot(values.at<Unit3>(U(i + 1))), 1e-2));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue