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

267 lines
8.7 KiB
C++
Raw Blame History

/**
* @file Pose2.cpp
* @brief 2D Pose
*/
#include <boost/foreach.hpp>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Lie-inl.h>
using namespace std;
namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose2);
static const Matrix I3 = eye(3), Z12 = zeros(1,2);
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
/* ************************************************************************* */
Matrix Pose2::matrix() const {
Matrix R = r_.matrix();
R = stack(2, &R, &Z12);
Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0);
return collect(2, &R, &T);
}
/* ************************************************************************* */
void Pose2::print(const string& s) const {
cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;
}
/* ************************************************************************* */
bool Pose2::equals(const Pose2& q, double tol) const {
return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
}
/* ************************************************************************* */
#ifdef SLOW_BUT_CORRECT_EXPMAP
template<> Pose2 expmap(const Vector& xi) {
Point2 v(xi(0),xi(1));
double w = xi(2);
if (fabs(w) < 1e-5)
return Pose2(xi[0], xi[1], xi[2]);
else {
Rot2 R(Rot2::fromAngle(w));
Point2 v_ortho = R_PI_2 * v; // points towards rot center
Point2 t = (v_ortho - R.rotate(v_ortho)) / w;
return Pose2(R, t);
}
}
Vector logmap(const Pose2& p) {
const Rot2& R = p.r();
const Point2& t = p.t();
double w = R.theta();
if (fabs(w) < 1e-5)
return Vector_(3, t.x(), t.y(), w);
else {
double c_1 = R.c()-1.0, s = R.s();
double det = c_1*c_1 + s*s;
Point2 p = R_PI_2 * (R.unrotate(t) - t);
Point2 v = (w/det) * p;
return Vector_(3, v.x(), v.y(), w);
}
}
#else
/* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector& v) {
return Pose2(v[0], v[1], v[2]);
}
/* ************************************************************************* */
Vector Pose2::Logmap(const Pose2& p) {
return Vector_(3, p.x(), p.y(), p.theta());
}
#endif
/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
Matrix Pose2::AdjointMap() const {
double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
return Matrix_(3,3,
c, -s, y,
s, c, -x,
0.0, 0.0, 1.0
);
}
/* ************************************************************************* */
Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -AdjointMap();
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = point - t_;
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 = Matrix_(2, 3,
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x());
if (H2) *H2 = r_.transpose();
return q;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// TODO: inline and reuse?
if(H1) *H1 = p2.inverse().AdjointMap();
if(H2) *H2 = I3;
return (*this)*p2;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Point2 q = r_ * p;
if (H1 || H2) {
const Matrix R = r_.matrix();
const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x());
if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
if (H2) *H2 = R; // R
}
return q + t_;
}
/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// get cosines and sines from rotation matrices
const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
H1->resize(3,3);
double data[9] = {
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0, -1.0};
copy(data, data+9, H1->data().begin());
}
if (H2) *H2 = I3;
return Pose2(R,t);
}
/* ************************************************************************* */
Rot2 Pose2::bearing(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = transform_to(point, H1, H2);
if (!H1 && !H2) return Rot2::relativeBearing(d);
Matrix D_result_d;
Rot2 result = Rot2::relativeBearing(d, D_result_d);
if (H1) *H1 = D_result_d * (*H1);
if (H2) *H2 = D_result_d * (*H2);
return result;
}
/* ************************************************************************* */
double Pose2::range(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (!H1 && !H2) return transform_to(point).norm();
Point2 d = transform_to(point, H1, H2);
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
if (H1) *H1 = D_result_d * (*H1);
if (H2) *H2 = D_result_d * (*H2);
return n;
}
/* ************************************************************************* */
// Re-factor of Michael Sobers' code, in turn based on Frank Dellaert's ML code
//
// q = Pose2::transform_from(p) = t + R*p
//
// | qx | cqx + | cos -sin | | px-cpx | |cqx - cos*cpx + sin*cpy| | cos -sin | | px |
// | | = | | * | | = | | + | | * | |
// | qy | cqy + | sin cos | | py-cpy | |cqy - sin*cpx - cos*cpy| | sin cos | | py |
//
// where the cos/sin rotation matrix takes the points p-cp into the same frame as the (u,v) points
//
// This is reformulated as a linear least-squares regression problem with two parameters (cos,sin),
// using the (u,v) points as "measurements" of the angle that rotates the (x,y):
//
// | dqx | | dpx -dpy | | cos | | cos |
// | | = | | * | | = H * | |
// | dqy | | dpy dpx | | sin | | sin |
//
// The solution is: | cos | | dqx |
// | | = inv(H'H)*H'*| |
// | sin | | dqy |
//
// where the rotation angle is found by using atan2(sin,cos).
//
// As it turns out, H'H is symmetric: H'H = | sum(dpx^2 + dpy^2) 0 |
// | 0 sum(dpx^2 + dpy^2) |
//
// | dqx | | sum( dpx*dqx + dpy*dqy) |
// Also, H'*| | = | |
// | dqy | | sum(-dpy*dqx + dpx*dqy) |
//
// so that cos = sum(dpx*dqx + dpy*dqy)/D and sin = sum(-dpy*dqx + dpx*dqy)/D
// where D = sum(dpx^2 + dpy^2)
//
// We need to remove the centroids from the data sets for this to work.
//
boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
size_t n = pairs.size();
if (n<2) return boost::none; // we need at least two pairs
// calculate centroids
Point2 cp,cq;
BOOST_FOREACH(const Point2Pair& pair, pairs) {
cp += pair.first;
cq += pair.second;
}
double f = 1.0/n;
cp *= f; cq *= f;
// calculate cos and sin
double ct=0,st=0,D=0;
BOOST_FOREACH(const Point2Pair& pair, pairs) {
Point2 dq = pair.first - cp;
Point2 dp = pair.second - cq;
ct += dp.x() * dq.x() + dp.y() * dq.y();
st += dp.y() * dq.x() - dp.x() * dq.y(); // this works but is negative from formula above !! :-(
D += dp.x()*dp.x() + dp.y()*dp.y();
}
ct /= D; st /= D;
// calculate angle and translation
double theta = atan2(st,ct);
Rot2 R = Rot2::fromAngle(theta);
Point2 t = cq - R*cp;
return Pose2(R, t);
}
/* ************************************************************************* */
} // namespace gtsam
Reference in New Issue View Git Blame Copy Permalink