171 lines
5.4 KiB
C++
171 lines
5.4 KiB
C++
/**
|
|
* @file Pose3Upright.cpp
|
|
*
|
|
* @date Jan 24, 2012
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#include <iostream>
|
|
#include "gtsam/base/OptionalJacobian.h"
|
|
#include "gtsam/base/Vector.h"
|
|
|
|
#include <gtsam_unstable/geometry/Pose3Upright.h>
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
Pose3Upright::Pose3Upright(const Rot2& bearing, const Point3& t)
|
|
: T_(bearing, Point2(t.x(), t.y())), z_(t.z())
|
|
{
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Pose3Upright::Pose3Upright(double x, double y, double z, double theta)
|
|
: T_(x, y, theta), z_(z)
|
|
{
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Pose3Upright::Pose3Upright(const Pose2& pose, double z)
|
|
: T_(pose), z_(z)
|
|
{
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Pose3Upright::Pose3Upright(const Pose3& x)
|
|
: T_(x.x(), x.y(), x.rotation().yaw()), z_(x.z())
|
|
{
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void Pose3Upright::print(const std::string& s) const {
|
|
cout << s << "(" << T_.x() << ", " << T_.y() << ", " << z_ << ", " << T_.theta() << ")" << endl;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
bool Pose3Upright::equals(const Pose3Upright& x, double tol) const {
|
|
return T_.equals(x.T_, tol) && std::abs(z_ - x.z_) < tol;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Point3 Pose3Upright::translation() const {
|
|
return Point3(x(), y(), z());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Point2 Pose3Upright::translation2() const {
|
|
return T_.t();
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Rot2 Pose3Upright::rotation2() const {
|
|
return T_.r();
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Rot3 Pose3Upright::rotation() const {
|
|
return Rot3::Yaw(theta());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Pose2 Pose3Upright::pose2() const {
|
|
return T_;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Pose3 Pose3Upright::pose() const {
|
|
return Pose3(rotation(), translation());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Pose3Upright Pose3Upright::inverse(OptionalJacobian<4, 4> H1) const {
|
|
if (!H1) {
|
|
return Pose3Upright(T_.inverse(), -z_);
|
|
}
|
|
OptionalJacobian<3, 3>::Jacobian H3x3;
|
|
// TODO(kartikarcot): Could not use reference to a view into H1 and reuse memory
|
|
// Eigen::Ref<Eigen::Matrix<double, 3, 3>> H3x3 = H1->topLeftCorner(3,3);
|
|
Pose3Upright result(T_.inverse(H3x3), -z_);
|
|
Matrix H1_ = -I_4x4;
|
|
H1_.topLeftCorner(2, 2) = H3x3.topLeftCorner(2, 2);
|
|
H1_.topRightCorner(2, 1) = H3x3.topRightCorner(2, 1);
|
|
*H1 = H1_;
|
|
return result;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Pose3Upright Pose3Upright::compose(const Pose3Upright& p2,
|
|
OptionalJacobian<4,4> H1, OptionalJacobian<4,4> H2) const {
|
|
if (!H1 && !H2)
|
|
return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_);
|
|
|
|
// TODO(kartikarcot): Could not use reference to a view into H1 and reuse memory
|
|
OptionalJacobian<3, 3>::Jacobian H3x3;
|
|
Pose3Upright result(T_.compose(p2.T_, H3x3), z_ + p2.z_);
|
|
if (H1) {
|
|
Matrix H1_ = I_4x4;
|
|
H1_.topLeftCorner(2,2) = H3x3.topLeftCorner(2,2);
|
|
H1_.topRightCorner(2, 1) = H3x3.topRightCorner(2, 1);
|
|
*H1 = H1_;
|
|
}
|
|
if (H2) *H2 = I_4x4;
|
|
return result;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Pose3Upright Pose3Upright::between(const Pose3Upright& p2,
|
|
OptionalJacobian<4,4> H1, OptionalJacobian<4,4> H2) const {
|
|
if (!H1 && !H2)
|
|
return Pose3Upright(T_.between(p2.T_), p2.z_ - z_);
|
|
|
|
// TODO(kartikarcot): Could not use reference to a view into H1 and H2 to reuse memory
|
|
OptionalJacobian<3, 3>::Jacobian H3x3_1, H3x3_2;
|
|
Pose3Upright result(T_.between(p2.T_, H3x3_1, H3x3_2), p2.z_ - z_);
|
|
if (H1) {
|
|
Matrix H1_ = -I_4x4;
|
|
H1_.topLeftCorner(2,2) = H3x3_1.topLeftCorner(2,2);
|
|
H1_.topRightCorner(2, 1) = H3x3_1.topRightCorner(2, 1);
|
|
*H1 = H1_;
|
|
}
|
|
if (H2) *H2 = I_4x4;
|
|
return result;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Pose3Upright Pose3Upright::retract(const Vector& v) const {
|
|
assert(v.size() == 4);
|
|
Vector v1(3); v1 << v(0), v(1), v(3);
|
|
return Pose3Upright(T_.retract(v1), z_ + v(2));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector Pose3Upright::localCoordinates(const Pose3Upright& p2) const {
|
|
Vector pose2 = T_.localCoordinates(p2.pose2());
|
|
Vector result(4);
|
|
result << pose2(0), pose2(1), p2.z() - z_, pose2(2);
|
|
return result;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Pose3Upright Pose3Upright::Expmap(const Vector& xi) {
|
|
assert(xi.size() == 4);
|
|
Vector v1(3); v1 << xi(0), xi(1), xi(3);
|
|
return Pose3Upright(Pose2::Expmap(v1), xi(2));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector Pose3Upright::Logmap(const Pose3Upright& p) {
|
|
Vector pose2 = Pose2::Logmap(p.pose2());
|
|
Vector result(4);
|
|
result << pose2(0), pose2(1), p.z(), pose2(2);
|
|
return result;
|
|
}
|
|
/* ************************************************************************* */
|
|
|
|
} // \namespace gtsam
|
|
|
|
|
|
|