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

equals

release/4.3a0
Frank Dellaert 2009-09-07 03:35:32 +00:00
parent ff9a89c81b
commit 5ca7ab9053
2 changed files with 17 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
cpp/Point2.cpp
View File

@ -15,9 +15,20 @@ namespace gtsam {
cout << s << "(" << x_ << ", " << y_ << ")" << endl; cout << s << "(" << x_ << ", " << y_ << ")" << endl;
} }
/* ************************************************************************* */
double Point2::dist(const Point2& p2) const {
return sqrt(pow(x() - p2.x(), 2.0) + pow(y() - p2.y(), 2.0));
}
/* ************************************************************************* */
bool Point2::equals(const Point2& q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol);
}
/* ************************************************************************* */ /* ************************************************************************* */
bool assert_equal(const Point2& p, const Point2& q, double tol) { bool assert_equal(const Point2& p, const Point2& q, double tol) {
if (fabs(p.x() - q.x()) < tol && fabs(p.y() - q.y()) < tol) return true; if (p.equals(q, tol))
return true;
printf("not equal:\n"); printf("not equal:\n");
p.print("p = "); p.print("p = ");
q.print("q = "); q.print("q = ");

9
cpp/Point2.h
View File

@ -6,7 +6,7 @@
#pragma once #pragma once
#include "Matrix.h" #include "Vector.h"
namespace gtsam { namespace gtsam {
@ -53,9 +53,10 @@ namespace gtsam {
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
/** distance between two points */ /** distance between two points */
double dist(const Point2& p2) const { double dist(const Point2& p2) const;
return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0));
} /** equals with an tolerance, prints out message if unequal*/
bool equals(const Point2& q, double tol = 1e-9) const;
}; };
/** equals with an tolerance, prints out message if unequal*/ /** equals with an tolerance, prints out message if unequal*/