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

change: remove redudant namespace

release/4.3a0
zhaoyang 2015-06-24 22:24:09 -04:00
parent cd0fe5d98c
commit 2c1d8e38db
2 changed files with 12 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/geometry/OrientedPlane3.cpp
View File

@ -26,27 +26,27 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
/// The print fuction /// The print fuction
void OrientedPlane3::print(const std::string& s) const { void OrientedPlane3::print(const string& s) const {
Vector coeffs = planeCoefficients(); Vector4 coeffs = planeCoefficients();
cout << s << " : " << coeffs << endl; cout << s << " : " << coeffs << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane, OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane,
const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr, const Pose3& xr, OptionalJacobian<3, 6> Hr,
OptionalJacobian<3, 3> Hp) { OptionalJacobian<3, 3> Hp) {
Matrix n_hr; Matrix n_hr;
Matrix n_hp; Matrix n_hp;
Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp); Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp);
Vector n_unit = plane.n_.unitVector(); Vector3 n_unit = plane.n_.unitVector();
Vector unit_vec = n_rotated.unitVector(); Vector3 unit_vec = n_rotated.unitVector();
double pred_d = n_unit.dot(xr.translation().vector()) + plane.d_; double pred_d = n_unit.dot(xr.translation().vector()) + plane.d_;
OrientedPlane3 transformed_plane(unit_vec(0), unit_vec(1), unit_vec(2), OrientedPlane3 transformed_plane(unit_vec(0), unit_vec(1), unit_vec(2),
pred_d); pred_d);
if (Hr) { if (Hr) {
*Hr = gtsam::zeros(3, 6); *Hr = zeros(3, 6);
(*Hr).block<2, 3>(0, 0) = n_hr; (*Hr).block<2, 3>(0, 0) = n_hr;
(*Hr).block<1, 3>(2, 3) = unit_vec; (*Hr).block<1, 3>(2, 3) = unit_vec;
} }
@ -54,7 +54,7 @@ OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane,
Vector xrp = xr.translation().vector(); Vector xrp = xr.translation().vector();
Matrix n_basis = plane.n_.basis(); Matrix n_basis = plane.n_.basis();
Vector hpp = n_basis.transpose() * xrp; Vector hpp = n_basis.transpose() * xrp;
*Hp = gtsam::zeros(3, 3); *Hp = zeros(3, 3);
(*Hp).block<2, 2>(0, 0) = n_hp; (*Hp).block<2, 2>(0, 0) = n_hp;
(*Hp).block<1, 2>(2, 0) = hpp; (*Hp).block<1, 2>(2, 0) = hpp;
(*Hp)(2, 2) = 1; (*Hp)(2, 2) = 1;
@ -64,7 +64,7 @@ OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const { Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const {
Vector2 n_error = -n_.localCoordinates(plane.n_); Vector2 n_error = -n_.localCoordinates(plane.n_);
double d_error = d_ - plane.d_; double d_error = d_ - plane.d_;
Vector3 e; Vector3 e;

7
gtsam/geometry/OrientedPlane3.h
View File

@ -22,7 +22,6 @@
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h> #include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/DerivedValue.h>
namespace gtsam { namespace gtsam {
@ -81,15 +80,15 @@ public:
* @param Hr optional jacobian wrpt incremental Pose * @param Hr optional jacobian wrpt incremental Pose
* @param Hp optional Jacobian wrpt the destination plane * @param Hp optional Jacobian wrpt the destination plane
*/ */
static OrientedPlane3 Transform(const gtsam::OrientedPlane3& plane, static OrientedPlane3 Transform(const OrientedPlane3& plane,
const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none,
OptionalJacobian<3, 3> Hp = boost::none); OptionalJacobian<3, 3> Hp = boost::none);
/** Computes the error between two poses. /** Computes the error between two poses.
* The error is a norm 1 difference in tangent space. * The error is a norm 1 difference in tangent space.
* @param the other plane * @param the other plane
*/ */
Vector3 error(const gtsam::OrientedPlane3& plane) const; Vector3 error(const OrientedPlane3& plane) const;
/// Dimensionality of tangent space = 3 DOF /// Dimensionality of tangent space = 3 DOF
inline static size_t Dim() { inline static size_t Dim() {