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

normalize

release/4.3a0
Frank Dellaert 2014-01-05 16:25:14 -05:00
parent 651dd3e931
commit 2acffe885e
3 changed files with 80 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
gtsam/geometry/Point3.cpp
View File

@ -26,7 +26,8 @@ INSTANTIATE_LIE(Point3);
/* ************************************************************************* */
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);
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol
&& fabs(z_ - q.z()) < tol);
}
/* ************************************************************************* */
@ -37,18 +38,18 @@ void Point3::print(const string& s) const {
/* ************************************************************************* */
bool Point3::operator== (const Point3& q) const {
bool Point3::operator==(const Point3& q) const {
return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
}
/* ************************************************************************* */
Point3 Point3::operator+(const Point3& q) const {
return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ );
return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_);
}
/* ************************************************************************* */
Point3 Point3::operator- (const Point3& q) const {
return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ );
Point3 Point3::operator-(const Point3& q) const {
return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_);
}
/* ************************************************************************* */
@ -62,36 +63,53 @@ Point3 Point3::operator/(double s) const {
}
/* ************************************************************************* */
Point3 Point3::add(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = eye(3,3);
Point3 Point3::add(const Point3 &q, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1)
*H1 = eye(3, 3);
if (H2)
*H2 = eye(3, 3);
return *this + q;
}
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = -eye(3,3);
Point3 Point3::sub(const Point3 &q, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1)
*H1 = eye(3, 3);
if (H2)
*H2 = -eye(3, 3);
return *this - q;
}
/* ************************************************************************* */
Point3 Point3::cross(const Point3 &q) const {
return Point3( y_*q.z_ - z_*q.y_,
z_*q.x_ - x_*q.z_,
x_*q.y_ - y_*q.x_ );
return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
x_ * q.y_ - y_ * q.x_);
}
/* ************************************************************************* */
double Point3::dot(const Point3 &q) const {
return ( x_*q.x_ + y_*q.y_ + z_*q.z_ );
return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_);
}
/* ************************************************************************* */
double Point3::norm() const {
return sqrt( x_*x_ + y_*y_ + z_*z_ );
return sqrt(x_ * x_ + y_ * y_ + z_ * z_);
}
/* ************************************************************************* */
Point3 Point3::normalize(boost::optional<Matrix&> H) const {
Point3 normalized = *this / norm();
if (H) {
// 3*3 Derivative
double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
H->resize(3, 3);
*H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
*H /= pow(x2 + y2 + z2, 1.5);
}
return normalized;
}
/* ************************************************************************* */

3
gtsam/geometry/Point3.h
View File

@ -176,6 +176,9 @@ namespace gtsam {
/** Distance of the point from the origin */
double norm() const;
/** normalize, with optional Jacobian */
Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
/** cross product @return this x q */
Point3 cross(const Point3 &q) const;

69
gtsam/geometry/tests/testPoint3.cpp
View File

@ -14,73 +14,84 @@
* @brief Unit tests for Point3 class
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point3)
GTSAM_CONCEPT_LIE_INST(Point3)
static Point3 P(0.2,0.7,-2);
static Point3 P(0.2, 0.7, -2);
/* ************************************************************************* */
TEST(Point3, Lie) {
Point3 p1(1,2,3);
Point3 p2(4,5,6);
Point3 p1(1, 2, 3);
Point3 p2(4, 5, 6);
Matrix H1, H2;
EXPECT(assert_equal(Point3(5,7,9), p1.compose(p2, H1, H2)));
EXPECT(assert_equal(Point3(5, 7, 9), p1.compose(p2, H1, H2)));
EXPECT(assert_equal(eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(Point3(3,3,3), p1.between(p2, H1, H2)));
EXPECT(assert_equal(Point3(3, 3, 3), p1.between(p2, H1, H2)));
EXPECT(assert_equal(-eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vector(3) << 4., 5., 6.))));
EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2)));
EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.))));
EXPECT(assert_equal(Vector_(3, 3., 3., 3.), p1.localCoordinates(p2)));
}
/* ************************************************************************* */
TEST( Point3, arithmetic)
{
CHECK(P*3==3*P);
CHECK(assert_equal( Point3(-1,-5,-6), -Point3(1,5,6) ));
CHECK(assert_equal( Point3(2,5,6), Point3(1,4,5)+Point3(1,1,1)));
CHECK(assert_equal( Point3(0,3,4), Point3(1,4,5)-Point3(1,1,1)));
CHECK(assert_equal( Point3(2,8,6), Point3(1,4,3)*2));
CHECK(assert_equal( Point3(2,2,6), 2*Point3(1,1,3)));
CHECK(assert_equal( Point3(1,2,3), Point3(2,4,6)/2));
TEST( Point3, arithmetic) {
CHECK(P * 3 == 3 * P);
CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6)));
CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1)));
CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2));
CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
}
/* ************************************************************************* */
TEST( Point3, equals)
{
TEST( Point3, equals) {
CHECK(P.equals(P));
Point3 Q;
CHECK(!P.equals(Q));
}
/* ************************************************************************* */
TEST( Point3, dot)
{
Point3 origin, ones(1,1,1);
CHECK(origin.dot(Point3(1,1,0)) == 0);
CHECK(ones.dot(Point3(1,1,0)) == 2);
TEST( Point3, dot) {
Point3 origin, ones(1, 1, 1);
CHECK(origin.dot(Point3(1, 1, 0)) == 0);
CHECK(ones.dot(Point3(1, 1, 0)) == 2);
}
/* ************************************************************************* */
TEST( Point3, stream)
{
Point3 p(1,2, -3);
TEST( Point3, stream) {
Point3 p(1, 2, -3);
std::ostringstream os;
os << p;
EXPECT(os.str() == "[1, 2, -3]';");
}
//*************************************************************************
TEST (Point3, normalize) {
Matrix actualH;
Point3 point(1, -2, 3); // arbitrary point
Point3 expected(point / sqrt(14));
EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8));
Matrix expectedH = numericalDerivative11<Point3, Point3>(
boost::bind(&Point3::normalize, _1, boost::none), point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */