128 lines
4.5 KiB
C++
128 lines
4.5 KiB
C++
/**
|
|
* @file Pose3.cpp
|
|
* @brief 3D Pose
|
|
*/
|
|
|
|
#include "Pose3.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
Pose3 Pose3::exmap(const Vector& v) const {
|
|
return Pose3(R_.exmap(sub(v,0,3)), t_.exmap(sub(v,3,6)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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.translation();
|
|
Point3 r = rotate(pose.rotation().inverse(), 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
|