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

Bearing

release/4.3a0
Frank Dellaert 2015-07-11 16:56:31 -07:00
parent 495f70bf78
commit 58a280c672
2 changed files with 39 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
gtsam/geometry/Pose3.cpp
View File

@ -314,35 +314,47 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
/* ************************************************************************* */
double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
OptionalJacobian<1, 3> H2) const {
OptionalJacobian<1, 3> H2) const {
Matrix36 D1;
Matrix3 D2;
Point3 local = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
if (!H1 && !H2) {
return transform_to(point).norm();
return local.norm();
} else {
Matrix36 D1;
Matrix3 D2;
Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z,
n = sqrt(d2);
Matrix13 D_result_d;
D_result_d << x / n, y / n, z / n;
if (H1) *H1 = D_result_d * D1;
if (H2) *H2 = D_result_d * D2;
return n;
Matrix13 D_r_local;
const double r = local.norm(D_r_local);
if (H1) *H1 = D_r_local * D1;
if (H2) *H2 = D_r_local * D2;
return r;
}
}
/* ************************************************************************* */
double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
OptionalJacobian<1,6> H2) const {
double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1,
OptionalJacobian<1, 6> H2) const {
Matrix13 D2;
double r = range(pose.translation(), H1, H2? &D2 : 0);
if (H2) {
Matrix13 H2_ = D2 * pose.rotation().matrix();
*H2 << Matrix13::Zero(), H2_;
}
double r = range(pose.translation(), H1, H2 ? &D2 : 0);
if (H2) *H2 << Matrix13::Zero(), D2 * pose.rotation().matrix();
return r;
}
/* ************************************************************************* */
Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
OptionalJacobian<2, 3> H2) const {
Matrix36 D1;
Matrix3 D2;
Point3 local = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
if (!H1 && !H2) {
return Unit3(local);
} else {
Matrix23 D_b_local;
Unit3 b = Unit3::FromPoint3(local, D_b_local);
if (H1)* H1 = D_b_local * D1;
if (H2) *H2 = D_b_local * D2;
return b;
}
}
/* ************************************************************************* */
boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
const size_t n = pairs.size();

8
gtsam/geometry/Pose3.h
View File

@ -262,6 +262,14 @@ public:
double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) const;
/**
* Calculate bearing to a landmark
* @param point 3D location of landmark
* @return bearing (Unit3)
*/
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none,
OptionalJacobian<2, 3> H2 = boost::none) const;
/// @}
/// @name Advanced Interface
/// @{