gtsam/cpp/Pose3.cpp

/**
 * @file  Pose3.cpp
 * @brief 3D Pose
 */

#include "Pose3.h"

using namespace std;

namespace gtsam {

/* ************************************************************************* */
Pose3 Pose3::exmap(const Vector& v) const { 
  return Pose3(R_.exmap(sub(v,0,3)), t_.exmap(sub(v,3,6)));
}

/* ************************************************************************* */
Vector Pose3::vector() const {
	Vector r = R_.vector(), t = t_.vector();
	return concatVectors(2, &r, &t);
}

/* ************************************************************************* */
Matrix Pose3::matrix() const {
	const double row4[] = { 0, 0, 0, 1 };
	Matrix A34 = Matrix_(3, 4, vector()), A14 = Matrix_(1, 4, row4);
	return stack(2, &A34, &A14);
}

/* ************************************************************************* */
void Pose3::print(const string& s) const {
	R_.print(s + ".R");
	t_.print(s + ".t");
}

/* ************************************************************************* */
Point3 transform_from(const Pose3& pose, const Point3& p) {
  return pose.R_ * p + pose.t_;
}
/* ************************************************************************* */
/** 3by6                                                                     */
/* ************************************************************************* */
Matrix Dtransform_from1(const Pose3& pose, const Point3& p) {
  Matrix DR = Drotate1(pose.rotation(), p);
  Matrix Dt = Dadd1(pose.translation(), p);
  return collect(2,&DR,&Dt);
}
/* ************************************************************************* */
/** 3by3                                                                     */
/* ************************************************************************* */
Matrix Dtransform_from2(const Pose3& pose) {
  return Drotate2(pose.rotation());
}

/* ************************************************************************* */
Point3 transform_to(const Pose3& pose, const Point3& p) {
		Point3 sub = p - pose.t_;
		Point3 r = unrotate(pose.R_, sub);
		return r; 
}
/* ************************************************************************* */
/** 3by6                                                                     */
/* ************************************************************************* */
Matrix Dtransform_to1(const Pose3& pose, const Point3& p) {
  Point3 q = p - pose.translation();
  Matrix D_r_R =   Dunrotate1(pose.rotation(),q);
  Matrix D_r_t = - Dunrotate2(pose.rotation()); // negative because of sub

  Matrix D_r_pose = collect(2,&D_r_R,&D_r_t);
  return D_r_pose;
}
/* ************************************************************************* */
/** 3by3                                                                     */
/* ************************************************************************* */
Matrix Dtransform_to2(const Pose3& pose) {
  return Dunrotate2(pose.rotation());
}

/* ************************************************************************* */
/** direct measurement of the deviation of a pose from the origin
 * used as soft prior
 */
/* ************************************************************************* */
Vector hPose (const Vector& x) {
  Pose3 pose(x);                            // transform from vector to Pose3
  Vector w = RQ(pose.rotation().matrix()); // get angle differences
  Vector d = pose.translation().vector();  // get translation differences
  return concatVectors(2,&w,&d);
}

/* ************************************************************************* */
/** derivative of direct measurement
 * 6*6, entry i,j is how measurement error will change 
 */
/* ************************************************************************* */
Matrix DhPose(const Vector& x) {
  Matrix H = eye(6,6);
  return H;
}

/* ************************************************************************* */
Pose3 Pose3::inverse() const
{
  Rot3 Rt = R_.inverse();
  return Pose3(Rt,-(Rt*t_));
}

/* ************************************************************************* */
bool Pose3::equals(const Pose3& pose, double tol) const
{
  return R_.equals(pose.rotation(),tol) && t_.equals(pose.translation(),tol);
}

/* ************************************************************************* */
bool assert_equal(const Pose3& A, const Pose3& B, double tol)
{
  if(A.equals(B,tol)) return true;
  printf("not equal:\n");
  A.print("A");
  B.print("B");
  return false;
}

/* ************************************************************************* */
Pose3 Pose3::transformPose_to(const Pose3& transform) const
{
		Rot3 cRv = rotation() * Rot3(transform.rotation().inverse());
		Point3 t = transform_to(transform, translation());
		
		return Pose3(cRv, t);
}

/* ************************************************************************* */
Pose3 composeTransform(const Pose3& current, const Pose3& target)
{
		// reverse operation
		Rot3 trans_rot = Rot3(target.rotation() * current.rotation().inverse()).inverse();

		// get sub
		Point3 sub = rotate(trans_rot, target.translation());
		
		// get final transform translation
		Point3 trans_pt = current.translation() - sub;
		
		return Pose3(trans_rot, trans_pt);
}

} // namespace gtsam