pi/ov_secondary_linux
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Files
master
ov_secondary_linux/camera_models/include/camodocal/gpl/EigenUtils.h

419 lines
11 KiB
C++
Raw Permalink Blame History

#ifndef EIGENUTILS_H
#define EIGENUTILS_H
#include <eigen3/Eigen/Dense>
#include "ceres/rotation.h"
#include "camodocal/gpl/gpl.h"
namespace camodocal
{
// Returns the 3D cross product skew symmetric matrix of a given 3D vector
template<typename T>
Eigen::Matrix<T, 3, 3> skew(const Eigen::Matrix<T, 3, 1>& vec)
{
return (Eigen::Matrix<T, 3, 3>() << T(0), -vec(2), vec(1),
vec(2), T(0), -vec(0),
-vec(1), vec(0), T(0)).finished();
}
template<typename Derived>
typename Eigen::MatrixBase<Derived>::PlainObject sqrtm(const Eigen::MatrixBase<Derived>& A)
{
Eigen::SelfAdjointEigenSolver<typename Derived::PlainObject> es(A);
return es.operatorSqrt();
}
template<typename T>
Eigen::Matrix<T, 3, 3> AngleAxisToRotationMatrix(const Eigen::Matrix<T, 3, 1>& rvec)
{
T angle = rvec.norm();
if (angle == T(0))
{
return Eigen::Matrix<T, 3, 3>::Identity();
}
Eigen::Matrix<T, 3, 1> axis;
axis = rvec.normalized();
Eigen::Matrix<T, 3, 3> rmat;
rmat = Eigen::AngleAxis<T>(angle, axis);
return rmat;
}
template<typename T>
Eigen::Quaternion<T> AngleAxisToQuaternion(const Eigen::Matrix<T, 3, 1>& rvec)
{
Eigen::Matrix<T, 3, 3> rmat = AngleAxisToRotationMatrix<T>(rvec);
return Eigen::Quaternion<T>(rmat);
}
template<typename T>
void AngleAxisToQuaternion(const Eigen::Matrix<T, 3, 1>& rvec, T* q)
{
Eigen::Quaternion<T> quat = AngleAxisToQuaternion<T>(rvec);
q[0] = quat.x();
q[1] = quat.y();
q[2] = quat.z();
q[3] = quat.w();
}
template<typename T>
Eigen::Matrix<T, 3, 1> RotationToAngleAxis(const Eigen::Matrix<T, 3, 3> & rmat)
{
Eigen::AngleAxis<T> angleaxis;
angleaxis.fromRotationMatrix(rmat);
return angleaxis.angle() * angleaxis.axis();
}
template<typename T>
void QuaternionToAngleAxis(const T* const q, Eigen::Matrix<T, 3, 1>& rvec)
{
Eigen::Quaternion<T> quat(q[3], q[0], q[1], q[2]);
Eigen::Matrix<T, 3, 3> rmat = quat.toRotationMatrix();
Eigen::AngleAxis<T> angleaxis;
angleaxis.fromRotationMatrix(rmat);
rvec = angleaxis.angle() * angleaxis.axis();
}
template<typename T>
Eigen::Matrix<T, 3, 3> QuaternionToRotation(const T* const q)
{
T R[9];
ceres::QuaternionToRotation(q, R);
Eigen::Matrix<T, 3, 3> rmat;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
rmat(i,j) = R[i * 3 + j];
}
}
return rmat;
}
template<typename T>
void QuaternionToRotation(const T* const q, T* rot)
{
ceres::QuaternionToRotation(q, rot);
}
template<typename T>
Eigen::Matrix<T,4,4> QuaternionMultMatLeft(const Eigen::Quaternion<T>& q)
{
return (Eigen::Matrix<T,4,4>() << q.w(), -q.z(), q.y(), q.x(),
q.z(), q.w(), -q.x(), q.y(),
-q.y(), q.x(), q.w(), q.z(),
-q.x(), -q.y(), -q.z(), q.w()).finished();
}
template<typename T>
Eigen::Matrix<T,4,4> QuaternionMultMatRight(const Eigen::Quaternion<T>& q)
{
return (Eigen::Matrix<T,4,4>() << q.w(), q.z(), -q.y(), q.x(),
-q.z(), q.w(), q.x(), q.y(),
q.y(), -q.x(), q.w(), q.z(),
-q.x(), -q.y(), -q.z(), q.w()).finished();
}
/// @param theta - rotation about screw axis
/// @param d - projection of tvec on the rotation axis
/// @param l - screw axis direction
/// @param m - screw axis moment
template<typename T>
void AngleAxisAndTranslationToScrew(const Eigen::Matrix<T, 3, 1>& rvec,
const Eigen::Matrix<T, 3, 1>& tvec,
T& theta, T& d,
Eigen::Matrix<T, 3, 1>& l,
Eigen::Matrix<T, 3, 1>& m)
{
theta = rvec.norm();
if (theta == 0)
{
l.setZero();
m.setZero();
std::cout << "Warning: Undefined screw! Returned 0. " << std::endl;
}
l = rvec.normalized();
Eigen::Matrix<T, 3, 1> t = tvec;
d = t.transpose() * l;
// point on screw axis - projection of origin on screw axis
Eigen::Matrix<T, 3, 1> c;
c = 0.5 * (t - d * l + (1.0 / tan(theta / 2.0) * l).cross(t));
// c and hence the screw axis is not defined if theta is either 0 or M_PI
m = c.cross(l);
}
template<typename T>
Eigen::Matrix<T, 3, 3> RPY2mat(T roll, T pitch, T yaw)
{
Eigen::Matrix<T, 3, 3> m;
T cr = cos(roll);
T sr = sin(roll);
T cp = cos(pitch);
T sp = sin(pitch);
T cy = cos(yaw);
T sy = sin(yaw);
m(0,0) = cy * cp;
m(0,1) = cy * sp * sr - sy * cr;
m(0,2) = cy * sp * cr + sy * sr;
m(1,0) = sy * cp;
m(1,1) = sy * sp * sr + cy * cr;
m(1,2) = sy * sp * cr - cy * sr;
m(2,0) = - sp;
m(2,1) = cp * sr;
m(2,2) = cp * cr;
return m;
}
template<typename T>
void mat2RPY(const Eigen::Matrix<T, 3, 3>& m, T& roll, T& pitch, T& yaw)
{
roll = atan2(m(2,1), m(2,2));
pitch = atan2(-m(2,0), sqrt(m(2,1) * m(2,1) + m(2,2) * m(2,2)));
yaw = atan2(m(1,0), m(0,0));
}
template<typename T>
Eigen::Matrix<T, 4, 4> homogeneousTransform(const Eigen::Matrix<T, 3, 3>& R, const Eigen::Matrix<T, 3, 1>& t)
{
Eigen::Matrix<T, 4, 4> H;
H.setIdentity();
H.block(0,0,3,3) = R;
H.block(0,3,3,1) = t;
return H;
}
template<typename T>
Eigen::Matrix<T, 4, 4> poseWithCartesianTranslation(const T* const q, const T* const p)
{
Eigen::Matrix<T, 4, 4> pose = Eigen::Matrix<T, 4, 4>::Identity();
T rotation[9];
ceres::QuaternionToRotation(q, rotation);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
pose(i,j) = rotation[i * 3 + j];
}
}
pose(0,3) = p[0];
pose(1,3) = p[1];
pose(2,3) = p[2];
return pose;
}
template<typename T>
Eigen::Matrix<T, 4, 4> poseWithSphericalTranslation(const T* const q, const T* const p, const T scale = T(1.0))
{
Eigen::Matrix<T, 4, 4> pose = Eigen::Matrix<T, 4, 4>::Identity();
T rotation[9];
ceres::QuaternionToRotation(q, rotation);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
pose(i,j) = rotation[i * 3 + j];
}
}
T theta = p[0];
T phi = p[1];
pose(0,3) = sin(theta) * cos(phi) * scale;
pose(1,3) = sin(theta) * sin(phi) * scale;
pose(2,3) = cos(theta) * scale;
return pose;
}
// Returns the Sampson error of a given essential matrix and 2 image points
template<typename T>
T sampsonError(const Eigen::Matrix<T, 3, 3>& E,
const Eigen::Matrix<T, 3, 1>& p1,
const Eigen::Matrix<T, 3, 1>& p2)
{
Eigen::Matrix<T, 3, 1> Ex1 = E * p1;
Eigen::Matrix<T, 3, 1> Etx2 = E.transpose() * p2;
T x2tEx1 = p2.dot(Ex1);
// compute Sampson error
T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0)));
return err;
}
// Returns the Sampson error of a given rotation/translation and 2 image points
template<typename T>
T sampsonError(const Eigen::Matrix<T, 3, 3>& R,
const Eigen::Matrix<T, 3, 1>& t,
const Eigen::Matrix<T, 3, 1>& p1,
const Eigen::Matrix<T, 3, 1>& p2)
{
// construct essential matrix
Eigen::Matrix<T, 3, 3> E = skew(t) * R;
Eigen::Matrix<T, 3, 1> Ex1 = E * p1;
Eigen::Matrix<T, 3, 1> Etx2 = E.transpose() * p2;
T x2tEx1 = p2.dot(Ex1);
// compute Sampson error
T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0)));
return err;
}
// Returns the Sampson error of a given rotation/translation and 2 image points
template<typename T>
T sampsonError(const Eigen::Matrix<T, 4, 4>& H,
const Eigen::Matrix<T, 3, 1>& p1,
const Eigen::Matrix<T, 3, 1>& p2)
{
Eigen::Matrix<T, 3, 3> R = H.block(0, 0, 3, 3);
Eigen::Matrix<T, 3, 1> t = H.block(0, 3, 3, 1);
return sampsonError(R, t, p1, p2);
}
template<typename T>
Eigen::Matrix<T, 3, 1>
transformPoint(const Eigen::Matrix<T, 4, 4>& H, const Eigen::Matrix<T, 3, 1>& P)
{
Eigen::Matrix<T, 3, 1> P_trans = H.block(0, 0, 3, 3) * P + H.block(0, 3, 3, 1);
return P_trans;
}
template<typename T>
Eigen::Matrix<T, 4, 4>
estimate3DRigidTransform(const std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >& points1,
const std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >& points2)
{
// compute centroids
Eigen::Matrix<T, 3, 1> c1, c2;
c1.setZero(); c2.setZero();
for (size_t i = 0; i < points1.size(); ++i)
{
c1 += points1.at(i);
c2 += points2.at(i);
}
c1 /= points1.size();
c2 /= points1.size();
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> X(3, points1.size());
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Y(3, points1.size());
for (size_t i = 0; i < points1.size(); ++i)
{
X.col(i) = points1.at(i) - c1;
Y.col(i) = points2.at(i) - c2;
}
Eigen::Matrix<T, 3, 3> H = X * Y.transpose();
Eigen::JacobiSVD< Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix<T, 3, 3> U = svd.matrixU();
Eigen::Matrix<T, 3, 3> V = svd.matrixV();
if (U.determinant() * V.determinant() < 0.0)
{
V.col(2) *= -1.0;
}
Eigen::Matrix<T, 3, 3> R = V * U.transpose();
Eigen::Matrix<T, 3, 1> t = c2 - R * c1;
return homogeneousTransform(R, t);
}
template<typename T>
Eigen::Matrix<T, 4, 4>
estimate3DRigidSimilarityTransform(const std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >& points1,
const std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > >& points2)
{
// compute centroids
Eigen::Matrix<T, 3, 1> c1, c2;
c1.setZero(); c2.setZero();
for (size_t i = 0; i < points1.size(); ++i)
{
c1 += points1.at(i);
c2 += points2.at(i);
}
c1 /= points1.size();
c2 /= points1.size();
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> X(3, points1.size());
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Y(3, points1.size());
for (size_t i = 0; i < points1.size(); ++i)
{
X.col(i) = points1.at(i) - c1;
Y.col(i) = points2.at(i) - c2;
}
Eigen::Matrix<T, 3, 3> H = X * Y.transpose();
Eigen::JacobiSVD< Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix<T, 3, 3> U = svd.matrixU();
Eigen::Matrix<T, 3, 3> V = svd.matrixV();
if (U.determinant() * V.determinant() < 0.0)
{
V.col(2) *= -1.0;
}
Eigen::Matrix<T, 3, 3> R = V * U.transpose();
std::vector<Eigen::Matrix<T, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<T, 3, 1> > > rotatedPoints1(points1.size());
for (size_t i = 0; i < points1.size(); ++i)
{
rotatedPoints1.at(i) = R * (points1.at(i) - c1);
}
double sum_ss = 0.0, sum_tt = 0.0;
for (size_t i = 0; i < points1.size(); ++i)
{
sum_ss += (points1.at(i) - c1).squaredNorm();
sum_tt += (points2.at(i) - c2).dot(rotatedPoints1.at(i));
}
double scale = sum_tt / sum_ss;
Eigen::Matrix<T, 3, 3> sR = scale * R;
Eigen::Matrix<T, 3, 1> t = c2 - sR * c1;
return homogeneousTransform(sR, t);
}
}
#endif
Reference in New Issue View Git Blame Copy Permalink