148 lines
4.2 KiB
C++
148 lines
4.2 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file Rot2.cpp
|
|
* @date Dec 9, 2009
|
|
* @author Frank Dellaert
|
|
* @brief 2D Rotations
|
|
*/
|
|
|
|
#include <gtsam/geometry/Rot2.h>
|
|
#include <iostream>
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
Rot2 Rot2::fromCosSin(double c, double s) {
|
|
Rot2 R(c, s);
|
|
return R.normalize();
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Rot2 Rot2::atan2(double y, double x) {
|
|
Rot2 R(x, y);
|
|
return R.normalize();
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Rot2 Rot2::Random(std::mt19937& rng) {
|
|
uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
|
|
double angle = randomAngle(rng);
|
|
return fromAngle(angle);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void Rot2::print(const string& s) const {
|
|
cout << s << ": " << theta() << endl;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
bool Rot2::equals(const Rot2& R, double tol) const {
|
|
return std::abs(c_ - R.c_) <= tol && std::abs(s_ - R.s_) <= tol;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Rot2& Rot2::normalize() {
|
|
double scale = c_*c_ + s_*s_;
|
|
if(std::abs(scale-1.0) > 1e-10) {
|
|
scale = 1 / sqrt(scale);
|
|
c_ *= scale;
|
|
s_ *= scale;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Rot2 Rot2::Expmap(const Vector1& v, OptionalJacobian<1, 1> H) {
|
|
if (H)
|
|
*H = I_1x1;
|
|
if (v.isZero())
|
|
return (Rot2());
|
|
else
|
|
return Rot2::fromAngle(v(0));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector1 Rot2::Logmap(const Rot2& r, OptionalJacobian<1, 1> H) {
|
|
if (H)
|
|
*H = I_1x1;
|
|
Vector1 v;
|
|
v << r.theta();
|
|
return v;
|
|
}
|
|
/* ************************************************************************* */
|
|
Matrix2 Rot2::matrix() const {
|
|
Matrix2 rvalue_;
|
|
rvalue_ << c_, -s_, s_, c_;
|
|
return rvalue_;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Matrix2 Rot2::transpose() const {
|
|
Matrix2 rvalue_;
|
|
rvalue_ << c_, s_, -s_, c_;
|
|
return rvalue_;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// see doc/math.lyx, SO(2) section
|
|
Point2 Rot2::rotate(const Point2& p, OptionalJacobian<2, 1> H1,
|
|
OptionalJacobian<2, 2> H2) const {
|
|
const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
|
|
if (H1) *H1 << -q.y(), q.x();
|
|
if (H2) *H2 = matrix();
|
|
return q;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// see doc/math.lyx, SO(2) section
|
|
Point2 Rot2::unrotate(const Point2& p,
|
|
OptionalJacobian<2, 1> H1, OptionalJacobian<2, 2> H2) const {
|
|
const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
|
|
if (H1) *H1 << q.y(), -q.x();
|
|
if (H2) *H2 = transpose();
|
|
return q;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
|
|
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
|
|
if(std::abs(n) > 1e-5) {
|
|
if (H) {
|
|
*H << -y / d2, x / d2;
|
|
}
|
|
return Rot2::fromCosSin(x / n, y / n);
|
|
} else {
|
|
if (H) *H << 0.0, 0.0;
|
|
return Rot2();
|
|
}
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Rot2 Rot2::ClosestTo(const Matrix2& M) {
|
|
Eigen::JacobiSVD<Matrix2> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
|
|
const Matrix2& U = svd.matrixU();
|
|
const Matrix2& V = svd.matrixV();
|
|
const double det = (U * V.transpose()).determinant();
|
|
Matrix2 M_prime = (U * Vector2(1, det).asDiagonal() * V.transpose());
|
|
|
|
double c = M_prime(0, 0);
|
|
double s = M_prime(1, 0);
|
|
return Rot2::fromCosSin(c, s);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
} // gtsam
|