gtsam/gtsam_unstable/dynamics/PoseRTV.cpp

/**
 * @file PoseRTV.cpp
 * @author Alex Cunningham
 */

#include <gtsam/3rdparty/Eigen/Eigen/LU>

#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Lie-inl.h>
#include <gtsam/geometry/Pose2.h>

#include <gtsam_unstable/dynamics/inertialUtils.h>
#include <gtsam_unstable/dynamics/PoseRTV.h>

namespace gtsam {

using namespace std;

static const Vector g = delta(3, 2, 9.81);
const double pi = M_PI;

/* ************************************************************************* */
double bound(double a, double min, double max) {
	if (a < min) return min;
	else if (a > max) return max;
	else return a;
}

/* ************************************************************************* */
PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, double z,
		double vx, double vy, double vz)
: Rt_(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), v_(vx, vy, vz) {}

/* ************************************************************************* */
PoseRTV::PoseRTV(const Vector& rtv)
: Rt_(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), v_(rtv.tail(3))
{
}

/* ************************************************************************* */
Vector PoseRTV::vector() const {
	Vector rtv(9);
	rtv.head(3) = Rt_.rotation().xyz();
	rtv.segment(3,3) = Rt_.translation().vector();
	rtv.tail(3) = v_.vector();
	return rtv;
}

/* ************************************************************************* */
bool PoseRTV::equals(const PoseRTV& other, double tol) const {
	return Rt_.equals(other.Rt_, tol) && v_.equals(other.v_, tol);
}

/* ************************************************************************* */
void PoseRTV::print(const string& s) const {
	cout << s << ":" << endl;
	gtsam::print((Vector)R().xyz(), "  R:rpy");
	t().print("  T");
	v_.print("  V");
}

/* ************************************************************************* */
PoseRTV PoseRTV::Expmap(const Vector& v) {
	assert(v.size() == 9);
	Pose3 newPose = Pose3::Expmap(sub(v, 0, 6));
	Velocity3 newVel = Velocity3::Expmap(sub(v, 6, 9));
	return PoseRTV(newPose, newVel);
}

/* ************************************************************************* */
Vector PoseRTV::Logmap(const PoseRTV& p) {
	Vector Lx = Pose3::Logmap(p.Rt_);
	Vector Lv = Velocity3::Logmap(p.v_);
	return concatVectors(2, &Lx, &Lv);
}

/* ************************************************************************* */
PoseRTV PoseRTV::retract(const Vector& v) const {
	assert(v.size() == 9);
	// First order approximation
	Pose3 newPose = Rt_.retract(sub(v, 0, 6));
	Velocity3 newVel = v_ + Rt_.rotation() * Point3(sub(v, 6, 9));
	return PoseRTV(newPose, newVel);
}

/* ************************************************************************* */
Vector PoseRTV::localCoordinates(const PoseRTV& p1) const {
	const Pose3& x0 = pose(), &x1 = p1.pose();
	// First order approximation
	Vector poseLogmap = x0.localCoordinates(x1);
	Vector lv = rotation().unrotate(p1.velocity() - v_).vector();
	return concatVectors(2, &poseLogmap, &lv);
}

/* ************************************************************************* */
PoseRTV PoseRTV::inverse() const {
	return PoseRTV(Rt_.inverse(), v_.inverse());
}

/* ************************************************************************* */
PoseRTV PoseRTV::compose(const PoseRTV& p) const {
	return PoseRTV(Rt_.compose(p.Rt_), v_.compose(p.v_));
}

/* ************************************************************************* */
PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); }
PoseRTV PoseRTV::between(const PoseRTV& p,
		boost::optional<Matrix&> H1,
		boost::optional<Matrix&> H2) const {
	if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5);
	if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5);
	return inverse().compose(p);
}

/* ************************************************************************* */
PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate,
		double max_accel, double dt) const {

	// split out initial state
	const Rot3& r1 = R();
	const Velocity3& v1 = v();

	// Update vehicle heading
	Rot3 r2 = r1.retract(Vector_(3, 0.0, 0.0, heading_rate * dt));
	const double yaw2 = r2.ypr()(0);

	// Update vehicle position
	const double mag_v1 = v1.norm();

	// FIXME: this doesn't account for direction in velocity bounds
	double dv = bound(vel_rate - mag_v1, - (max_accel * dt), max_accel * dt);
	double mag_v2 = mag_v1 + dv;
	Velocity3 v2 = mag_v2 * Velocity3(cos(yaw2), sin(yaw2), 0.0);

	Point3 t2 = translationIntegration(r2, v2, dt);

	return PoseRTV(r2, t2, v2);
}

/* ************************************************************************* */
PoseRTV PoseRTV::flyingDynamics(
		double pitch_rate, double heading_rate, double lift_control, double dt) const {
	// split out initial state
	const Rot3& r1 = R();
	const Velocity3& v1 = v();

	// Update vehicle heading (and normalise yaw)
	Vector rot_rates = Vector_(3, 0.0, pitch_rate, heading_rate);
	Rot3 r2 = r1.retract(rot_rates*dt);

	// Work out dynamics on platform
	const double thrust = 50.0;
	const double lift   = 50.0;
	const double drag   = 0.1;
	double yaw2 = r2.yaw();
	double pitch2 = r2.pitch();
	double forward_accel = -thrust * sin(pitch2); // r2, pitch (in global frame?) controls forward force
	double loss_lift = lift*fabs(sin(pitch2));
	Rot3 yaw_correction_bn = Rot3::yaw(yaw2);
	Point3 forward(forward_accel, 0.0, 0.0);
	Vector Acc_n =
			yaw_correction_bn.rotate(forward).vector()   // applies locally forward force in the global frame
			- drag * Vector_(3, v1.x(), v1.y(), 0.0)     // drag term dependent on v1
			+ delta(3, 2, loss_lift - lift_control);     // falling due to lift lost from pitch

	// Update Vehicle Position and Velocity
	Velocity3 v2 = v1 + Velocity3(Acc_n * dt);
	Point3 t2 = translationIntegration(r2, v2, dt);

	return PoseRTV(r2, t2, v2);
}

/* ************************************************************************* */
PoseRTV PoseRTV::generalDynamics(
		const Vector& accel, const Vector& gyro, double dt) const {
	//	Integrate Attitude Equations
	Rot3 r2 = rotation().retract(gyro * dt);

	//	Integrate Velocity Equations
	Velocity3 v2 = v_.compose(Velocity3(dt * (r2.matrix() * accel + g)));

	//	Integrate Position Equations
	Point3 t2 = translationIntegration(r2, v2, dt);

	return PoseRTV(t2, r2, v2);
}

/* ************************************************************************* */
Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
	// split out states
	const Rot3      &r1 = R(), &r2 = x2.R();
	const Velocity3 &v1 = v(), &v2 = x2.v();

	Vector imu(6);

	// acceleration
	Vector accel = v1.localCoordinates(v2) / dt;
	imu.head(3) = r2.transpose() * (accel - g);

	// rotation rates
	// just using euler angles based on matlab code
	// FIXME: this is silly - we shouldn't use differences in Euler angles
	Matrix Enb = dynamics::RRTMnb(r1);
	Vector euler1 = r1.xyz(), euler2 = r2.xyz();
	Vector dR = euler2 - euler1;

	// normalize yaw in difference (as per Mitch's code)
	dR(2) = Rot2::fromAngle(dR(2)).theta();
	dR /= dt;
	imu.tail(3) = Enb * dR;
//	imu.tail(3) = r1.transpose() * dR;

	return imu;
}

/* ************************************************************************* */
Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const {
	// predict point for constraint
	// NOTE: uses simple Euler approach for prediction
	Point3 pred_t2 = t() + v2 * dt;
	return pred_t2;
}

/* ************************************************************************* */
double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); }
double PoseRTV::range(const PoseRTV& other,
		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
	if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5);
	if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5);
	return t().dist(other.t());
}

/* ************************************************************************* */
PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) {
	return global.transformed_from(transform);
}

PoseRTV PoseRTV::transformed_from(const Pose3& trans,
		boost::optional<Matrix&> Dglobal,
		boost::optional<Matrix&> Dtrans) const {
	// Note that we rotate the velocity
	Matrix DVr, DTt;
	Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt);
	if (!Dglobal && !Dtrans)
		return PoseRTV(trans.compose(pose()), newvel);

	// Pose3 transform is just compose
	Matrix DTc, DGc;
	Pose3 newpose = trans.compose(pose(), DTc, DGc);

	if (Dglobal) {
		*Dglobal = zeros(9,9);
		insertSub(*Dglobal, DGc, 0, 0);

		// Rotate velocity
		insertSub(*Dglobal, eye(3,3), 6, 6); // FIXME: should this actually be an identity matrix?
	}

	if (Dtrans) {
		*Dtrans = numericalDerivative22(transformed_from_, *this, trans, 1e-6);
		//
		//		*Dtrans = zeros(9,6);
		//		// directly affecting the pose
		//		insertSub(*Dtrans, DTc, 0, 0); // correct in tests
		//
		//		// rotating the velocity
		//		Matrix vRhat = skewSymmetric(-v_.x(), -v_.y(), -v_.z());
		//		trans.rotation().print("Transform rotation");
		//		gtsam::print(vRhat, "vRhat");
		//		gtsam::print(DVr, "DVr");
		//		// FIXME: find analytic derivative
		////		insertSub(*Dtrans, vRhat, 6, 0); // works if PoseRTV.rotation() = I
		////		insertSub(*Dtrans, trans.rotation().matrix() * vRhat, 6, 0); // FAIL: both tests fail
	}
	return PoseRTV(newpose, newvel);
}

} // \namespace gtsam