Moved all common methods in new file Rot3.cpp
git-svn-id: https://svn.cc.gatech.edu/borg/gtsam/trunk@20416 898a188c-9671-0410-8e00-e3fd810bbb7frelease/4.3a0
Frank Dellaert
Richard Roberts
parent
d109c981ed
commit
dd447f2c6c
|
|
@ -0,0 +1,178 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Rot3.cpp
|
||||
* @brief Rotation, common code between Rotation matrix and Quaternion
|
||||
* @author Alireza Fathi
|
||||
* @author Christian Potthast
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
static const Matrix3 I3 = Matrix3::Identity();
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Rot3::print(const std::string& s) const {
|
||||
gtsam::print((Matrix)matrix(), s);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Vector& w) {
|
||||
double t = w.norm();
|
||||
if (t < 1e-10) return Rot3();
|
||||
return rodriguez(w/t, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot3::equals(const Rot3 & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::operator*(const Point3& p) const {
|
||||
return rotate(p);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::rotate(const Sphere2& p,
|
||||
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
|
||||
Sphere2 q = rotate(p.point3(Hp));
|
||||
if (Hp)
|
||||
(*Hp) = q.basis().transpose() * matrix() * (*Hp);
|
||||
if (HR)
|
||||
(*HR) = -q.basis().transpose() * matrix() * p.skew();
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::operator*(const Sphere2& p) const {
|
||||
return rotate(p);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SO(3) section
|
||||
Point3 Rot3::unrotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
const Matrix Rt(transpose());
|
||||
Point3 q(Rt*p.vector()); // q = Rt*p
|
||||
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
|
||||
if (H2) *H2 = Rt;
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
|
||||
Matrix3 Rot3::dexpL(const Vector3& v) {
|
||||
if(zero(v)) return eye(3);
|
||||
Matrix x = skewSymmetric(v);
|
||||
Matrix x2 = x*x;
|
||||
double theta = v.norm(), vi = theta/2.0;
|
||||
double s1 = sin(vi)/vi;
|
||||
double s2 = (theta - sin(theta))/(theta*theta*theta);
|
||||
Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
|
||||
return res;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
|
||||
Matrix3 Rot3::dexpInvL(const Vector3& v) {
|
||||
if(zero(v)) return eye(3);
|
||||
Matrix x = skewSymmetric(v);
|
||||
Matrix x2 = x*x;
|
||||
double theta = v.norm(), vi = theta/2.0;
|
||||
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
|
||||
Matrix res = eye(3) + 0.5*x - s2*x2;
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::column(int index) const{
|
||||
if(index == 3)
|
||||
return r3();
|
||||
else if(index == 2)
|
||||
return r2();
|
||||
else if(index == 1)
|
||||
return r1(); // default returns r1
|
||||
else
|
||||
throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::xyz() const {
|
||||
Matrix I;Vector3 q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::ypr() const {
|
||||
Vector3 q = xyz();
|
||||
return Vector3(q(2),q(1),q(0));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::rpy() const {
|
||||
return xyz();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3::quaternion() const {
|
||||
Quaternion q = toQuaternion();
|
||||
Vector v(4);
|
||||
v(0) = q.w();
|
||||
v(1) = q.x();
|
||||
v(2) = q.y();
|
||||
v(3) = q.z();
|
||||
return v;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix3, Vector3> RQ(const Matrix3& A) {
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3 Qx = Rot3::Rx(-x);
|
||||
Matrix3 B = A * Qx.matrix();
|
||||
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3 Qy = Rot3::Ry(-y);
|
||||
Matrix3 C = B * Qy.matrix();
|
||||
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3 Qz = Rot3::Rz(-z);
|
||||
Matrix3 R = C * Qz.matrix();
|
||||
|
||||
Vector xyz = Vector3(x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ostream &operator<<(ostream &os, const Rot3& R) {
|
||||
os << "\n";
|
||||
os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
|
||||
os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
|
||||
os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
|
||||
return os;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
@ -69,11 +69,6 @@ Rot3::Rot3(const Matrix& R) {
|
|||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Rot3::print(const std::string& s) const {
|
||||
gtsam::print((Matrix)matrix(), s);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::Rx(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
|
|
@ -148,18 +143,6 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
|
|||
-swy + C02, swx + C12, c + C22);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Vector& w) {
|
||||
double t = w.norm();
|
||||
if (t < 1e-10) return Rot3();
|
||||
return rodriguez(w/t, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot3::equals(const Rot3 & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::compose (const Rot3& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
|
|
@ -169,7 +152,9 @@ Rot3 Rot3::compose (const Rot3& R2,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::operator*(const Point3& p) const { return rotate(p); }
|
||||
Rot3 Rot3::operator*(const Rot3& R2) const {
|
||||
return Rot3(Matrix3(rot_*R2.rot_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
|
||||
|
|
@ -183,12 +168,6 @@ Rot3 Rot3::between (const Rot3& R2,
|
|||
if (H1) *H1 = -(R2.transpose()*rot_);
|
||||
if (H2) *H2 = I3;
|
||||
return Rot3(Matrix3(rot_.transpose()*R2.rot_));
|
||||
//return between_default(*this, R2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::operator*(const Rot3& R2) const {
|
||||
return Rot3(Matrix3(rot_*R2.rot_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -201,32 +180,6 @@ Point3 Rot3::rotate(const Point3& p,
|
|||
return Point3(rot_ * p.vector());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::rotate(const Sphere2& p,
|
||||
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
|
||||
Sphere2 q(rotate(p.point3(Hp)));
|
||||
if (Hp)
|
||||
(*Hp) = q.basis().transpose() * matrix() * (*Hp);
|
||||
if (HR)
|
||||
(*HR) = -q.basis().transpose() * matrix() * p.skew();
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::operator*(const Sphere2& p) const {
|
||||
return rotate(p);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SO(3) section
|
||||
Point3 Rot3::unrotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
Point3 q(rot_.transpose()*p.vector()); // q = Rt*p
|
||||
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
|
||||
if (H2) *H2 = transpose();
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
Vector3 Rot3::Logmap(const Rot3& R) {
|
||||
|
|
@ -324,32 +277,6 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
|
||||
Matrix3 Rot3::dexpL(const Vector3& v) {
|
||||
if(zero(v)) return eye(3);
|
||||
Matrix x = skewSymmetric(v);
|
||||
Matrix x2 = x*x;
|
||||
double theta = v.norm(), vi = theta/2.0;
|
||||
double s1 = sin(vi)/vi;
|
||||
double s2 = (theta - sin(theta))/(theta*theta*theta);
|
||||
Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
|
||||
return res;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
|
||||
Matrix3 Rot3::dexpInvL(const Vector3& v) {
|
||||
if(zero(v)) return eye(3);
|
||||
Matrix x = skewSymmetric(v);
|
||||
Matrix x2 = x*x;
|
||||
double theta = v.norm(), vi = theta/2.0;
|
||||
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
|
||||
Matrix res = eye(3) + 0.5*x - s2*x2;
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 Rot3::matrix() const {
|
||||
return rot_;
|
||||
|
|
@ -360,18 +287,6 @@ Matrix3 Rot3::transpose() const {
|
|||
return rot_.transpose();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::column(int index) const{
|
||||
if(index == 3)
|
||||
return r3();
|
||||
else if(index == 2)
|
||||
return r2();
|
||||
else if(index == 1)
|
||||
return r1(); // default returns r1
|
||||
else
|
||||
throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r1() const { return Point3(rot_.col(0)); }
|
||||
|
||||
|
|
@ -381,68 +296,11 @@ Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
|
|||
/* ************************************************************************* */
|
||||
Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::xyz() const {
|
||||
Matrix3 I;Vector3 q;
|
||||
boost::tie(I,q)=RQ(rot_);
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::ypr() const {
|
||||
Vector3 q = xyz();
|
||||
return Vector3(q(2),q(1),q(0));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::rpy() const {
|
||||
return xyz();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Quaternion Rot3::toQuaternion() const {
|
||||
return Quaternion(rot_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3::quaternion() const {
|
||||
Quaternion q = toQuaternion();
|
||||
Vector v(4);
|
||||
v(0) = q.w();
|
||||
v(1) = q.x();
|
||||
v(2) = q.y();
|
||||
v(3) = q.z();
|
||||
return v;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix3, Vector3> RQ(const Matrix3& A) {
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3 Qx = Rot3::Rx(-x);
|
||||
Matrix3 B = A * Qx.matrix();
|
||||
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3 Qy = Rot3::Ry(-y);
|
||||
Matrix3 C = B * Qy.matrix();
|
||||
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3 Qz = Rot3::Rz(-z);
|
||||
Matrix3 R = C * Qz.matrix();
|
||||
|
||||
Vector xyz = Vector3(x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ostream &operator<<(ostream &os, const Rot3& R) {
|
||||
os << "\n";
|
||||
os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
|
||||
os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
|
||||
os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
|
||||
return os;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -62,11 +62,6 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Quaternion& q) : quaternion_(q) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Rot3::print(const std::string& s) const {
|
||||
gtsam::print((Matrix)matrix(), s);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }
|
||||
|
||||
|
|
@ -87,18 +82,6 @@ namespace gtsam {
|
|||
Rot3 Rot3::rodriguez(const Vector& w, double theta) {
|
||||
return Quaternion(Eigen::AngleAxisd(theta, w)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::rodriguez(const Vector& w) {
|
||||
double t = w.norm();
|
||||
if (t < 1e-10) return Rot3();
|
||||
return rodriguez(w/t, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot3::equals(const Rot3 & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::compose(const Rot3& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
|
|
@ -108,9 +91,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::operator*(const Point3& p) const {
|
||||
Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z());
|
||||
return Point3(r(0), r(1), r(2));
|
||||
Rot3 Rot3::operator*(const Rot3& R2) const {
|
||||
return Rot3(quaternion_ * R2.quaternion_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -127,11 +109,6 @@ namespace gtsam {
|
|||
return between_default(*this, R2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::operator*(const Rot3& R2) const {
|
||||
return Rot3(quaternion_ * R2.quaternion_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::rotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
|
|
@ -142,17 +119,6 @@ namespace gtsam {
|
|||
return Point3(r.x(), r.y(), r.z());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SO(3) section
|
||||
Point3 Rot3::unrotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
const Matrix Rt(transpose());
|
||||
Point3 q(Rt*p.vector()); // q = Rt*p
|
||||
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
|
||||
if (H2) *H2 = Rt;
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
Vector3 Rot3::Logmap(const Rot3& R) {
|
||||
|
|
@ -180,18 +146,6 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
Matrix3 Rot3::transpose() const {return quaternion_.toRotationMatrix().transpose();}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::column(int index) const{
|
||||
if(index == 3)
|
||||
return r3();
|
||||
else if(index == 2)
|
||||
return r2();
|
||||
else if(index == 1)
|
||||
return r1(); // default returns r1
|
||||
else
|
||||
throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }
|
||||
|
||||
|
|
@ -201,55 +155,10 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::xyz() const {
|
||||
Matrix I;Vector3 q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::ypr() const {
|
||||
Vector3 q = xyz();
|
||||
return Vector3(q(2),q(1),q(0));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Rot3::rpy() const {
|
||||
Vector3 q = xyz();
|
||||
return Vector3(q(0),q(1),q(2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Quaternion Rot3::toQuaternion() const { return quaternion_; }
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix3, Vector3> RQ(const Matrix3& A) {
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3 Qx = Rot3::Rx(-x);
|
||||
Matrix3 B = A * Qx.matrix();
|
||||
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3 Qy = Rot3::Ry(-y);
|
||||
Matrix3 C = B * Qy.matrix();
|
||||
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3 Qz = Rot3::Rz(-z);
|
||||
Matrix3 R = C * Qz.matrix();
|
||||
|
||||
Vector xyz = Vector3(x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ostream &operator<<(ostream &os, const Rot3& R) {
|
||||
os << "\n";
|
||||
os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
|
||||
os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
|
||||
os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
|
||||
return os;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -16,12 +16,8 @@
|
|||
* @author Chris Beall
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/geometry/triangulation.h>
|
||||
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in New Issue