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

Fixed range function for Pose3

release/4.3a0
Alex Cunningham 2011-01-13 17:09:29 +00:00
parent 10656d7700
commit 120b6878cf
2 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/Pose3.cpp
View File

@ -253,8 +253,9 @@ namespace gtsam {
boost::optional<Matrix&> H2) const {
if (!H1 && !H2) return transform_to(point).norm();
Point3 d = transform_to(point, H1, H2);
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
double x = d.x(), y = d.y(), z = d.z(),
d2 = x * x + y * y + z * z, n = sqrt(d2);
Matrix D_result_d = Matrix_(1, 3, x / n, y / n, z / n);
if (H1) *H1 = D_result_d * (*H1);
if (H2) *H2 = D_result_d * (*H2);
return n;

4
gtsam/geometry/tests/testPose3.cpp
View File

@ -466,7 +466,7 @@ TEST( Pose3, between )
/* ************************************************************************* */
// some shared test values - pulled from equivalent test in Pose2
Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 3, 0);
Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 4,-4);
Pose3 x1, x2(Rot3::ypr(0.0, 0.0, 0.0), l2), x3(Rot3::ypr(M_PI_4, 0.0, 0.0), l2);
/* ************************************************************************* */
@ -495,7 +495,7 @@ TEST( Pose3, range )
// Another test
double actual34 = x3.range(l4, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(2,actual34,1e-9);
EXPECT_DOUBLES_EQUAL(5,actual34,1e-9);
// Check numerical derivatives
expectedH1 = numericalDerivative21(range_proxy, x3, l4, 1e-5);