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

Added a range to pose function for Pose3

release/4.3a0
Alex Cunningham 2011-10-06 20:47:23 +00:00
parent 3f9feea089
commit 541c1085db
4 changed files with 73 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -1392,6 +1392,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

16
gtsam/geometry/Pose3.cpp
View File

@ -272,4 +272,20 @@ namespace gtsam {
if (H2) *H2 = D_result_d * (*H2);
return n;
}
/* ************************************************************************* */
double Pose3::range(const Pose3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
double r = range(point.translation(), H1, H2);
if (H2) {
#ifdef CORRECT_POSE3_EXMAP
Matrix H2_ = *H2 * point.rotation().matrix();
#else
Matrix H2_ = *H2;
#endif
*H2 = zeros(1, 6);
insertSub(*H2, H2_, 0, 3);
}
return r;
}
} // namespace gtsam

9
gtsam/geometry/Pose3.h
View File

@ -172,6 +172,15 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate range to another pose
* @param point SO(3) pose of landmark
* @return range (double)
*/
double range(const Pose3& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
private:
/** Serialization function */
friend class boost::serialization::access;

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

@ -570,6 +570,11 @@ 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, 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);
Pose3
xl1(Rot3::ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)),
xl2(Rot3::ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)),
xl3(Rot3::ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)),
xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4));
/* ************************************************************************* */
LieVector range_proxy(const Pose3& pose, const Point3& point) {
@ -606,6 +611,41 @@ TEST( Pose3, range )
EXPECT(assert_equal(expectedH2,actualH2));
}
/* ************************************************************************* */
LieVector range_pose_proxy(const Pose3& pose, const Pose3& point) {
return LieVector(pose.range(point));
}
TEST( Pose3, range_pose )
{
Matrix expectedH1, actualH1, expectedH2, actualH2;
// establish range is indeed zero
EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
// establish range is indeed sqrt2
EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(xl2),1e-9);
// Another pair
double actual23 = x2.range(xl3, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9);
// Check numerical derivatives
expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
expectedH2 = numericalDerivative22(range_pose_proxy, x2, xl3);
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2));
// Another test
double actual34 = x3.range(xl4, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(5,actual34,1e-9);
// Check numerical derivatives
expectedH1 = numericalDerivative21(range_pose_proxy, x3, xl4);
expectedH2 = numericalDerivative22(range_pose_proxy, x3, xl4);
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2));
}
/* ************************************************************************* */
TEST( Pose3, unicycle )
{