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

add expressions for cross() and dot()

release/4.3a0
Jose Luis Blanco Claraco 2021-06-11 09:41:26 +02:00
parent 19d3fe04de
commit 2827584f69
No known key found for this signature in database
GPG Key ID: D443304FBD70A641
2 changed files with 45 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/slam/expressions.h
View File

@ -48,6 +48,18 @@ inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) {
return Line3_(f, wTc, wL);
}
inline Point3_ cross(const Point3_& a, const Point3_& b) {
Point3 (*f)(const Point3 &, const Point3 &,
OptionalJacobian<3, 3>, OptionalJacobian<3, 3>) = &cross;
return Point3_(f, a, b);
}
inline Double_ dot(const Point3_& a, const Point3_& b) {
double (*f)(const Point3 &, const Point3 &,
OptionalJacobian<1, 3>, OptionalJacobian<1, 3>) = &dot;
return Double_(f, a, b);
}
namespace internal {
// define getter that returns value rather than reference
inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) {

33
tests/testExpressionFactor.cpp
View File

@ -727,6 +727,39 @@ TEST(ExpressionFactor, variadicTemplate) {
}
TEST(ExpressionFactor, crossProduct) {
auto model = noiseModel::Isotropic::Sigma(3, 1);
// Create expression
const auto a = Vector3_(1);
const auto b = Vector3_(2);
Vector3_ f_expr = cross(a, b);
// Check derivatives
Values values;
values.insert(1, Vector3(0.1, 0.2, 0.3));
values.insert(2, Vector3(0.4, 0.5, 0.6));
ExpressionFactor<Vector3> factor(model, Vector3::Zero(), f_expr);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
}
TEST(ExpressionFactor, dotProduct) {
auto model = noiseModel::Isotropic::Sigma(1, 1);
// Create expression
const auto a = Vector3_(1);
const auto b = Vector3_(2);
Double_ f_expr = dot(a, b);
// Check derivatives
Values values;
values.insert(1, Vector3(0.1, 0.2, 0.3));
values.insert(2, Vector3(0.4, 0.5, 0.6));
ExpressionFactor<double> factor(model, .0, f_expr);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
}
/* ************************************************************************* */
int main() {
TestResult tr;