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

made all classes that currently have an assert_equal "Testable" 

- derive from testable as in class Point2 : Testable<Point2>
- moved print and equal declarations in .h right after the constructor
- similarly, moved implementations after constructors in .cpp file
- removed obsolete assert_equal
release/4.3a0
Brian Law 2009-10-26 19:26:51 +00:00
parent 3b30fe50b0
commit 11f0d04cb6
12 changed files with 97 additions and 174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cpp/Cal3_S2.cpp
View File

@ -43,18 +43,6 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
/* ************************************************************************* */
bool assert_equal(const Cal3_S2& actual, const Cal3_S2& expected, double tol) {
bool ret = actual.equals(expected, tol);
if (!ret) {
cout << "Not Equal:" << endl;
actual.print("Actual");
expected.print("Expected");
}
return ret;
}
/* ************************************************************************* */
Point2 uncalibrate(const Cal3_S2& K, const Point2& p) {
return K.uncalibrate(p);
}

25
cpp/Cal3_S2.h
View File

@ -12,7 +12,7 @@
namespace gtsam {
/** The most common 5DOF 3D->2D calibration */
class Cal3_S2 {
class Cal3_S2: Testable<Cal3_S2> {
private:
double fx_, fy_, s_, u0_, v0_;
@ -31,6 +31,15 @@ namespace gtsam {
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
}
void print(const std::string& s = "") const {
gtsam::print(matrix(), s);
}
/**
* Check if equal up to specified tolerance
*/
bool equals(const Cal3_S2& K, double tol = 10e-9) const;
/**
* return DOF, dimensionality of tangent space
*/
@ -82,10 +91,6 @@ namespace gtsam {
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
void print(const std::string& s = "") const {
gtsam::print(matrix(), s);
}
std::string dump() const {
char buffer[100];
buffer[0] = 0;
@ -93,11 +98,6 @@ namespace gtsam {
return std::string(buffer);
}
/**
* Check if equal up to specified tolerance
*/
bool equals(const Cal3_S2& K, double tol = 10e-9) const;
/** friends */
friend Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p);
@ -115,11 +115,6 @@ namespace gtsam {
}
};
/**
* assert for unit tests
*/
bool assert_equal(const Cal3_S2& actual, const Cal3_S2& expected, double tol = 1e-8);
/**
* convert intrinsic coordinates xy to image coordinates uv
*/

47
cpp/LinearConstraint.cpp
View File

@ -38,24 +38,6 @@ LinearConstraint::LinearConstraint(const std::map<std::string, Matrix>& matrices
{
}
ConstrainedConditionalGaussian::shared_ptr LinearConstraint::eliminate(const std::string& key) {
// check to ensure key is one of constraint nodes
const_iterator it = As.find(key);
if (it == As.end())
throw invalid_argument("Node " + key + " is not in LinearConstraint");
// extract the leading matrix
Matrix A1 = it->second;
// assemble parents
map<string, Matrix> parents = As;
parents.erase(key);
// construct resulting CCG with parts
ConstrainedConditionalGaussian::shared_ptr ccg(new ConstrainedConditionalGaussian(A1, parents, b));
return ccg;
}
void LinearConstraint::print(const string& s) const {
cout << s << ": " << endl;
string key; Matrix A;
@ -85,6 +67,24 @@ bool LinearConstraint::equals(const LinearConstraint& f, double tol) const {
return true;
}
ConstrainedConditionalGaussian::shared_ptr LinearConstraint::eliminate(const std::string& key) {
// check to ensure key is one of constraint nodes
const_iterator it = As.find(key);
if (it == As.end())
throw invalid_argument("Node " + key + " is not in LinearConstraint");
// extract the leading matrix
Matrix A1 = it->second;
// assemble parents
map<string, Matrix> parents = As;
parents.erase(key);
// construct resulting CCG with parts
ConstrainedConditionalGaussian::shared_ptr ccg(new ConstrainedConditionalGaussian(A1, parents, b));
return ccg;
}
bool LinearConstraint::involves(const std::string& key) const {
return As.find(key) != As.end();
}
@ -104,17 +104,6 @@ string LinearConstraint::dump() const {
return ret;
}
bool assert_equal(const LinearConstraint& actual,
const LinearConstraint& expected, double tol) {
bool ret = actual.equals(expected, tol);
if (!ret) {
cout << "Not Equal:" << endl;
actual.print("Actual");
expected.print("Expected");
}
return ret;
}
LinearConstraint::shared_ptr combineConstraints(
const set<LinearConstraint::shared_ptr>& constraints) {
map<string, Matrix> blocks;

27
cpp/LinearConstraint.h
View File

@ -18,7 +18,7 @@ namespace gtsam {
* Linear constraints are similar to factors in structure, but represent
* a different problem
*/
class LinearConstraint {
class LinearConstraint: Testable<LinearConstraint> {
public:
typedef boost::shared_ptr<LinearConstraint> shared_ptr;
@ -69,17 +69,6 @@ public:
~LinearConstraint() {
}
/**
* Eliminates the constraint
* Note: Assumes that the constraint will be completely eliminated
* and that the matrix associated with the key is invertible
* @param key is the variable to eliminate
* @return a constrained conditional gaussian for the variable that is a
* function of its parents
*/
ConstrainedConditionalGaussian::shared_ptr
eliminate(const std::string& key);
/**
* print
* @param s optional string naming the factor
@ -91,6 +80,16 @@ public:
*/
bool equals(const LinearConstraint& f, double tol = 1e-9) const;
/**
* Eliminates the constraint
* Note: Assumes that the constraint will be completely eliminated
* and that the matrix associated with the key is invertible
* @param key is the variable to eliminate
* @return a constrained conditional gaussian for the variable that is a
* function of its parents
*/
ConstrainedConditionalGaussian::shared_ptr
eliminate(const std::string& key);
/**
* returns a version of the factor as a string
*/
@ -138,10 +137,6 @@ public:
*/
LinearConstraint::shared_ptr combineConstraints(const std::set<LinearConstraint::shared_ptr>& constraints);
/** assert equals for testing - prints when not equal */
bool assert_equal(const LinearConstraint& actual,
const LinearConstraint& expected, double tol = 1e-9);
}
#endif /* EQUALITYFACTOR_H_ */

19
cpp/Point3.cpp
View File

@ -8,6 +8,11 @@
namespace gtsam {
/* ************************************************************************* */
bool Point3::equals(const Point3 & q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
}
/* ************************************************************************* */
bool Point3::operator== (const Point3& q) const {
@ -58,11 +63,6 @@ Matrix Dsub1(const Point3 &p, const Point3 &q) {
Matrix Dsub2(const Point3 &p, const Point3 &q) {
return -eye(3,3);
}
/* ************************************************************************* */
bool Point3::equals(const Point3 & q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
}
/* ************************************************************************* */
Point3 cross(const Point3 &p, const Point3 &q)
{
@ -81,14 +81,5 @@ double norm(const Point3 &p)
return sqrt( p.x_*p.x_ + p.y_*p.y_ + p.z_*p.z_ );
}
/* ************************************************************************* */
bool assert_equal(const Point3& p, const Point3& q, double tol) {
if(p.equals(q,tol)) return true;
printf("not equal:\n");
p.print("p = ");
q.print("q = ");
(p-q).print("p-q = ");
return false;
}
/* ************************************************************************* */
} // namespace gtsam

22
cpp/Point3.h
View File

@ -13,11 +13,12 @@
#include <boost/serialization/nvp.hpp>
#include "Matrix.h"
#include "Testable.h"
namespace gtsam {
/** A 3D point */
class Point3 {
class Point3: Testable<Point3> {
private:
double x_, y_, z_;
@ -27,6 +28,14 @@ namespace gtsam {
Point3(double x, double y, double z): x_(x), y_(y), z_(z) {}
Point3(const Vector& v) : x_(v(0)), y_(v(1)), z_(v(2)) {}
/** print with optional string */
void print(const std::string& s = "") const {
std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl;
}
/** equals with an tolerance */
bool equals(const Point3& p, double tol = 1e-9) const;
/** return DOF, dimensionality of tangent space */
size_t dim() const { return 3;}
@ -53,19 +62,11 @@ namespace gtsam {
Point3 operator * (double s) const;
Point3 operator / (double s) const;
/** print with optional string */
void print(const std::string& s = "") const {
std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl;
}
/** distance between two points */
double dist(const Point3& p2) const {
return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0));
}
/** equals with an tolerance */
bool equals(const Point3& p, double tol = 1e-9) const;
/** friends */
friend Point3 cross(const Point3 &p1, const Point3 &p2);
friend double dot(const Point3 &p1, const Point3 &p2);
@ -103,7 +104,4 @@ namespace gtsam {
/** dot product */
double norm(const Point3 &p);
/** equals with an tolerance, prints out message if unequal */
bool assert_equal(const Point3& p, const Point3& q, double tol = 1e-9);
}

13
cpp/Pose2.cpp
View File

@ -14,11 +14,6 @@ namespace gtsam {
cout << s << "(" << x_ << ", " << y_ << ", " << theta_ << ")" << endl;
}
/* ************************************************************************* */
Pose2 Pose2::exmap(const Vector& v) const {
return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2));
}
/* ************************************************************************* */
bool Pose2::equals(const Pose2& q, double tol) const {
return (fabs(x_ - q.x_) < tol && fabs(y_ - q.y_) < tol && fabs(theta_
@ -26,12 +21,8 @@ namespace gtsam {
}
/* ************************************************************************* */
bool assert_equal(const Pose2& A, const Pose2& B, double tol) {
if (A.equals(B, tol)) return true;
printf("not equal:\n");
A.print("A");
B.print("B");
return false;
Pose2 Pose2::exmap(const Vector& v) const {
return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2));
}
/* ************************************************************************* */

18
cpp/Pose2.h
View File

@ -9,13 +9,14 @@
#pragma once
#include "Point2.h"
#include "Testable.h"
namespace gtsam {
/**
* A 2D pose (x,y,theta)
*/
class Pose2 {
class Pose2: Testable<Pose2> {
private:
double x_, y_, theta_;
@ -47,21 +48,18 @@ namespace gtsam {
x_(t.x()), y_(t.y()), theta_(theta) {
}
/** print with optional string */
void print(const std::string& s = "") const;
/** assert equality up to a tolerance */
bool equals(const Pose2& pose, double tol = 1e-9) const;
/** get functions for x, y, theta */
double x() const { return x_;}
double y() const { return y_;}
double theta() const { return theta_;}
/** print with optional string */
void print(const std::string& s = "") const;
Pose2 exmap(const Vector& v) const;
/** assert equality up to a tolerance */
bool equals(const Pose2& pose, double tol = 1e-9) const;
};
/** assert equality up to a tolerance */
bool assert_equal(const Pose2& A, const Pose2& B, double tol = 1e-9);
} // namespace gtsam

34
cpp/Pose3.cpp
View File

@ -9,6 +9,18 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
void Pose3::print(const string& s) const {
R_.print(s + ".R");
t_.print(s + ".t");
}
/* ************************************************************************* */
bool Pose3::equals(const Pose3& pose, double tol) const
{
return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol);
}
/* ************************************************************************* */
// Agrawal06iros, formula (6), seems to suggest this could be wrong:
Pose3 Pose3::exmap(const Vector& v) const {
@ -28,12 +40,6 @@ Matrix Pose3::matrix() const {
return stack(2, &A34, &A14);
}
/* ************************************************************************* */
void Pose3::print(const string& s) const {
R_.print(s + ".R");
t_.print(s + ".t");
}
/* ************************************************************************* */
Point3 transform_from(const Pose3& pose, const Point3& p) {
return pose.R_ * p + pose.t_;
@ -106,22 +112,6 @@ Pose3 Pose3::inverse() const
return Pose3(Rt,-(Rt*t_));
}
/* ************************************************************************* */
bool Pose3::equals(const Pose3& pose, double tol) const
{
return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol);
}
/* ************************************************************************* */
bool assert_equal(const Pose3& A, const Pose3& B, double tol)
{
if(A.equals(B,tol)) return true;
printf("not equal:\n");
A.print("A");
B.print("B");
return false;
}
/* ************************************************************************* */
Pose3 Pose3::transformPose_to(const Pose3& pose) const
{

18
cpp/Pose3.h
View File

@ -9,11 +9,12 @@
#include "Point3.h"
#include "Rot3.h"
#include "Testable.h"
namespace gtsam {
/** A 3D pose (R,t) : (Rot3,Point3) */
class Pose3 {
class Pose3 : Testable<Pose3> {
private:
Rot3 R_;
Point3 t_;
@ -51,6 +52,12 @@ namespace gtsam {
t_(V(9), V(10),V(11)) {
}
/** print with optional string */
void print(const std::string& s = "") const;
/** assert equality up to a tolerance */
bool equals(const Pose3& pose, double tol = 1e-9) const;
const Rot3& rotation() const {
return R_;
}
@ -81,15 +88,9 @@ namespace gtsam {
/** convert to 4*4 matrix */
Matrix matrix() const;
/** print with optional string */
void print(const std::string& s = "") const;
/** transforms */
Pose3 transformPose_to(const Pose3& transform) const;
/** assert equality up to a tolerance */
bool equals(const Pose3& pose, double tol = 1e-9) const;
/** friends */
friend Point3 transform_from(const Pose3& pose, const Point3& p);
friend Point3 transform_to(const Pose3& pose, const Point3& p);
@ -131,7 +132,4 @@ namespace gtsam {
*/
Matrix DhPose(const Vector& x);
/** assert equality up to a tolerance */
bool assert_equal(const Pose3& A, const Pose3& B, double tol = 1e-9);
} // namespace gtsam

19
cpp/Rot3.cpp
View File

@ -12,6 +12,11 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
/* ************************************************************************* */
/** faster than below ? */
/* ************************************************************************* */
@ -77,11 +82,6 @@ namespace gtsam {
return R.unrotate(p);
}
/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
/* ************************************************************************* */
/** see libraries/caml/geometry/math.lyx, derivative of unrotate */
/* ************************************************************************* */
@ -95,15 +95,6 @@ namespace gtsam {
return R.transpose();
}
/* ************************************************************************* */
bool assert_equal(const Rot3 & A, const Rot3 & B, double tol) {
if(A.equals(B,tol)) return true;
printf("not equal:\n");
A.print("A");
B.print("B");
return false;
}
/* ************************************************************************* */
/** This function receives a rotation 3 by 3 matrix and returns 3 rotation angles.
* The implementation is based on the algorithm in multiple view geometry

17
cpp/Rot3.h
View File

@ -11,11 +11,12 @@
#pragma once
#include "Point3.h"
#include "Testable.h"
namespace gtsam {
/* Rotation matrix */
class Rot3{
class Rot3: Testable<Rot3> {
private:
/** we store columns ! */
Point3 r1_, r2_, r3_;
@ -54,6 +55,12 @@ namespace gtsam {
r2_(Point3(R(0,1), R(1,1), R(2,1))),
r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
/** print */
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
/** equals with an tolerance */
bool equals(const Rot3& p, double tol = 1e-9) const;
/** return DOF, dimensionality of tangent space */
size_t dim() const { return 3;}
@ -121,12 +128,6 @@ namespace gtsam {
);
}
/** print */
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
/** equals with an tolerance */
bool equals(const Rot3& p, double tol = 1e-9) const;
/** friends */
friend Matrix Dunrotate1(const Rot3& R, const Point3& p);
@ -190,8 +191,6 @@ namespace gtsam {
Matrix Dunrotate1(const Rot3& R, const Point3& p);
Matrix Dunrotate2(const Rot3& R); // does not depend on p !
bool assert_equal(const Rot3& A, const Rot3& B, double tol=1e-9);
/** receives a rotation 3 by 3 matrix and returns 3 rotation angles.*/
Vector RQ(Matrix R);