Add misc explicit operators
parent
3ed3c3b2e8
commit
fd2f39e271
|
@ -32,7 +32,8 @@ namespace gtsam {
|
||||||
/**
|
/**
|
||||||
* Equals testing for basic types
|
* Equals testing for basic types
|
||||||
*/
|
*/
|
||||||
inline bool assert_equal(const Key& expected, const Key& actual, double tol = 0.0) {
|
inline bool assert_equal(const Key& expected, const Key& actual) {
|
||||||
|
// TODO - why isn't tol used?
|
||||||
if(expected != actual) {
|
if(expected != actual) {
|
||||||
std::cout << "Not equal:\nexpected: " << expected << "\nactual: " << actual << std::endl;
|
std::cout << "Not equal:\nexpected: " << expected << "\nactual: " << actual << std::endl;
|
||||||
return false;
|
return false;
|
||||||
|
|
|
@ -97,7 +97,6 @@ Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
|
||||||
Rot3 cRw = wRc.inverse();
|
Rot3 cRw = wRc.inverse();
|
||||||
Rot3 cRl = cRw * wL.R_;
|
Rot3 cRl = cRw * wL.R_;
|
||||||
|
|
||||||
Vector2 w_ab;
|
|
||||||
Vector3 t = ((wL.R_).transpose() * wTc.translation());
|
Vector3 t = ((wL.R_).transpose() * wTc.translation());
|
||||||
Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]);
|
Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]);
|
||||||
|
|
||||||
|
|
|
@ -56,9 +56,8 @@ public:
|
||||||
|
|
||||||
/** Copy constructor */
|
/** Copy constructor */
|
||||||
Pose3(const Pose3& pose) = default;
|
Pose3(const Pose3& pose) = default;
|
||||||
// :
|
|
||||||
// R_(pose.R_), t_(pose.t_) {
|
Pose3& operator=(const Pose3& other) = default;
|
||||||
// }
|
|
||||||
|
|
||||||
/** Construct from R,t */
|
/** Construct from R,t */
|
||||||
Pose3(const Rot3& R, const Point3& t) :
|
Pose3(const Rot3& R, const Point3& t) :
|
||||||
|
|
|
@ -53,7 +53,8 @@ namespace gtsam {
|
||||||
|
|
||||||
/** copy constructor */
|
/** copy constructor */
|
||||||
Rot2(const Rot2& r) = default;
|
Rot2(const Rot2& r) = default;
|
||||||
// : Rot2(r.c_, r.s_) {}
|
|
||||||
|
Rot2& operator=(const Rot2& other) = default;
|
||||||
|
|
||||||
/// Constructor from angle in radians == exponential map at identity
|
/// Constructor from angle in radians == exponential map at identity
|
||||||
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
|
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
|
||||||
|
|
|
@ -107,7 +107,6 @@ SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Build expX = exp(xi^)
|
// Build expX = exp(xi^)
|
||||||
Matrix4 expX;
|
|
||||||
using std::cos;
|
using std::cos;
|
||||||
using std::sin;
|
using std::sin;
|
||||||
const auto X2 = X * X;
|
const auto X2 = X * X;
|
||||||
|
|
|
@ -45,7 +45,6 @@ static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
|
||||||
static double calculateScale(const Point3Pairs &d_abPointPairs,
|
static double calculateScale(const Point3Pairs &d_abPointPairs,
|
||||||
const Rot3 &aRb) {
|
const Rot3 &aRb) {
|
||||||
double x = 0, y = 0;
|
double x = 0, y = 0;
|
||||||
Point3 da, db;
|
|
||||||
for (const auto& [da, db] : d_abPointPairs) {
|
for (const auto& [da, db] : d_abPointPairs) {
|
||||||
const Vector3 da_prime = aRb * db;
|
const Vector3 da_prime = aRb * db;
|
||||||
y += da.transpose() * da_prime;
|
y += da.transpose() * da_prime;
|
||||||
|
|
|
@ -33,7 +33,7 @@ static Y add(const Y& y1, const Y& y2) {
|
||||||
GaussianFactorGraph result = y1.first;
|
GaussianFactorGraph result = y1.first;
|
||||||
result.push_back(y2.first);
|
result.push_back(y2.first);
|
||||||
return {result, y1.second + y2.second};
|
return {result, y1.second + y2.second};
|
||||||
};
|
}
|
||||||
|
|
||||||
/* *******************************************************************************/
|
/* *******************************************************************************/
|
||||||
HybridGaussianProductFactor operator+(const HybridGaussianProductFactor& a,
|
HybridGaussianProductFactor operator+(const HybridGaussianProductFactor& a,
|
||||||
|
|
|
@ -106,7 +106,7 @@ namespace gtsam {
|
||||||
template<class CONTAINER>
|
template<class CONTAINER>
|
||||||
explicit VectorValues(const CONTAINER& c) : values_(c.begin(), c.end()) {}
|
explicit VectorValues(const CONTAINER& c) : values_(c.begin(), c.end()) {}
|
||||||
|
|
||||||
/** Implicit copy constructor to specialize the explicit constructor from any container. */
|
/** Copy constructor to specialize the explicit constructor from any container. */
|
||||||
VectorValues(const VectorValues& c) : values_(c.values_) {}
|
VectorValues(const VectorValues& c) : values_(c.values_) {}
|
||||||
|
|
||||||
/** Create from a pair of iterators over pair<Key,Vector>. */
|
/** Create from a pair of iterators over pair<Key,Vector>. */
|
||||||
|
@ -119,6 +119,7 @@ namespace gtsam {
|
||||||
/// Constructor from Vector, with Scatter
|
/// Constructor from Vector, with Scatter
|
||||||
VectorValues(const Vector& c, const Scatter& scatter);
|
VectorValues(const Vector& c, const Scatter& scatter);
|
||||||
|
|
||||||
|
// We override the copy constructor; expicitly declare operator=
|
||||||
VectorValues& operator=(const VectorValues& other) = default;
|
VectorValues& operator=(const VectorValues& other) = default;
|
||||||
|
|
||||||
/** Create a VectorValues with the same structure as \c other, but filled with zeros. */
|
/** Create a VectorValues with the same structure as \c other, but filled with zeros. */
|
||||||
|
|
|
@ -29,6 +29,8 @@ namespace gtsam {
|
||||||
size_t id;
|
size_t id;
|
||||||
Dummy();
|
Dummy();
|
||||||
~Dummy();
|
~Dummy();
|
||||||
|
Dummy(const Dummy& other) = default;
|
||||||
|
Dummy& operator=(const Dummy& other) = default;
|
||||||
void print(const std::string& s="") const ;
|
void print(const std::string& s="") const ;
|
||||||
unsigned char dummyTwoVar(unsigned char a) const ;
|
unsigned char dummyTwoVar(unsigned char a) const ;
|
||||||
};
|
};
|
||||||
|
|
|
@ -37,6 +37,8 @@ public:
|
||||||
/** copy constructors */
|
/** copy constructors */
|
||||||
FixedVector(const FixedVector& v) : Base(v) {}
|
FixedVector(const FixedVector& v) : Base(v) {}
|
||||||
|
|
||||||
|
FixedVector& operator=(const FixedVector& other) = default;
|
||||||
|
|
||||||
/** Convert from a variable-size vector to a fixed size vector */
|
/** Convert from a variable-size vector to a fixed size vector */
|
||||||
FixedVector(const Vector& v) : Base(v) {}
|
FixedVector(const Vector& v) : Base(v) {}
|
||||||
|
|
||||||
|
|
|
@ -37,6 +37,8 @@ public:
|
||||||
/// Copy constructor
|
/// Copy constructor
|
||||||
Mechanization_bRn2(const Mechanization_bRn2& other) = default;
|
Mechanization_bRn2(const Mechanization_bRn2& other) = default;
|
||||||
|
|
||||||
|
Mechanization_bRn2& operator=(const Mechanization_bRn2& other) = default;
|
||||||
|
|
||||||
/// gravity in the body frame
|
/// gravity in the body frame
|
||||||
Vector3 b_g(double g_e) const {
|
Vector3 b_g(double g_e) const {
|
||||||
Vector3 n_g(0, 0, g_e);
|
Vector3 n_g(0, 0, g_e);
|
||||||
|
|
Loading…
Reference in New Issue