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

Fixed small issue, should not assign to reference in case of Quaternions.

release/4.3a0
dellaert 2014-11-02 13:03:36 +01:00
parent d4dcf90bd1
commit b21db08ec1
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/geometry/Pose3.cpp
View File

@ -243,8 +243,8 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose, Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint) const { boost::optional<Matrix&> Dpoint) const {
if (Dpose) { if (Dpose) {
const Matrix R = R_.matrix(); const Matrix3 R = R_.matrix();
Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
Dpose->resize(3, 6); Dpose->resize(3, 6);
(*Dpose) << DR, R; (*Dpose) << DR, R;
} }
@ -263,7 +263,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
boost::optional<Matrix3&> Dpoint) const { boost::optional<Matrix3&> Dpoint) const {
// Only get transpose once, to avoid multiple allocations, // Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case // as well as multiple conversions in the Quaternion case
const Matrix3& Rt = R_.transpose(); const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p - t_).vector()); const Point3 q(Rt*(p - t_).vector());
if (Dpose) { if (Dpose) {
const double wx = q.x(), wy = q.y(), wz = q.z(); const double wx = q.x(), wy = q.y(), wz = q.z();
@ -280,7 +280,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose, Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint) const { boost::optional<Matrix&> Dpoint) const {
const Matrix3& Rt = R_.transpose(); const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p - t_).vector()); const Point3 q(Rt*(p - t_).vector());
if (Dpose) { if (Dpose) {
const double wx = q.x(), wy = q.y(), wz = q.z(); const double wx = q.x(), wy = q.y(), wz = q.z();