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

doubleCross, tested

release/4.3a0
Frank Dellaert 2024-12-14 17:22:09 -05:00
parent d1634c0335
commit 8040a0a31e
3 changed files with 29 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Point3.cpp
View File

@ -69,6 +69,16 @@ Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1,
p.x() * q.y() - p.y() * q.x());
}
Point3 doubleCross(const Point3 &p, const Point3 &q, //
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) {
if (H1) *H1 = q.dot(p) * I_3x3 + p * q.transpose() - 2 * q * p.transpose();
if (H2) {
const Matrix3 W = skewSymmetric(p);
*H2 = W * W;
}
return gtsam::cross(p, gtsam::cross(p, q));
}
double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {
if (H1) *H1 << q.x(), q.y(), q.z();

7
gtsam/geometry/Point3.h
View File

@ -55,11 +55,16 @@ GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {});
/// normalize, with optional Jacobian
GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {});
/// cross product @return this x q
/// cross product @return p x q
GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
OptionalJacobian<3, 3> H_p = {},
OptionalJacobian<3, 3> H_q = {});
/// double cross product @return p x (p x q)
Point3 doubleCross(const Point3& p, const Point3& q,
OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {});
/// dot product
GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H_p = {},

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

@ -18,6 +18,8 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Point3.h>
#include "gtsam/base/Matrix.h"
#include "gtsam/base/OptionalJacobian.h"
using namespace std::placeholders;
using namespace gtsam;
@ -154,6 +156,17 @@ TEST( Point3, cross2) {
}
}
/* ************************************************************************* */
TEST(Point3, doubleCross) {
Matrix aH1, aH2;
std::function<Point3(const Point3&, const Point3&)> f =
[](const Point3& p, const Point3& q) { return doubleCross(p, q); };
const Point3 omega(1, 2, 3), theta(4, 5, 6);
doubleCross(omega, theta, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
}
//*************************************************************************
TEST (Point3, normalize) {
Matrix actualH;