From db3da6757720d23ae6474c7d61e00f64ba825a79 Mon Sep 17 00:00:00 2001 From: raphaelchang Date: Thu, 12 Sep 2019 18:45:24 -0400 Subject: [PATCH] import and restyle lambda twist --- CMakeLists.txt | 7 +- src/odometry/lambda_twist.h | 426 ++++++++++++++++++++++++++++++++++++ src/odometry/pnp.cc | 15 ++ src/odometry/pnp.h | 18 ++ src/util/math_util.h | 80 ++++--- src/util/tf_util.h | 107 ++++----- 6 files changed, 571 insertions(+), 82 deletions(-) create mode 100644 src/odometry/lambda_twist.h create mode 100644 src/odometry/pnp.cc create mode 100644 src/odometry/pnp.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a59405..b7beebe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,6 +66,7 @@ add_library(omni_slam_eval_lib src/feature/detector.cc src/feature/matcher.cc src/reconstruction/triangulator.cc + src/odometry/pnp.cc src/optimization/bundle_adjuster.cc src/util/hdf_file.cc ) @@ -95,7 +96,7 @@ target_link_libraries(omni_slam_tracking_eval_node PUBLIC omni_slam_eval_lib ${HDF5_CXX_LIBRARIES} ${OpenMP_CXX_FLAGS} ${PCL_LIBRARIES} - ceres + ${CERES_LIBRARIES} ) target_link_libraries(omni_slam_matching_eval_node PUBLIC omni_slam_eval_lib @@ -104,7 +105,7 @@ target_link_libraries(omni_slam_matching_eval_node PUBLIC omni_slam_eval_lib ${HDF5_CXX_LIBRARIES} ${OpenMP_CXX_FLAGS} ${PCL_LIBRARIES} - ceres + ${CERES_LIBRARIES} ) target_link_libraries(omni_slam_reconstruction_eval_node PUBLIC omni_slam_eval_lib @@ -113,7 +114,7 @@ target_link_libraries(omni_slam_reconstruction_eval_node PUBLIC omni_slam_eval_l ${HDF5_CXX_LIBRARIES} ${OpenMP_CXX_FLAGS} ${PCL_LIBRARIES} - ceres + ${CERES_LIBRARIES} ) target_compile_options(omni_slam_tracking_eval_node PUBLIC ${OpenMP_CXX_FLAGS}) diff --git a/src/odometry/lambda_twist.h b/src/odometry/lambda_twist.h new file mode 100644 index 0000000..bb132be --- /dev/null +++ b/src/odometry/lambda_twist.h @@ -0,0 +1,426 @@ +#ifndef _LAMBDA_TWIST_H_ +#define _LAMBDA_TWIST_H_ + +#include +#include "util/math_util.h" + +using namespace Eigen; + +namespace omni_slam +{ +namespace odometry +{ +namespace LambdaTwist +{ + +template +constexpr T GetNumericLimit() +{ + return 1e-13; +} + +template<> constexpr float GetNumericLimit() +{ + // abs limit is 9 digits + return 1e-7; +} + +template<> constexpr double GetNumericLimit() +{ + // abs limit is 17 digits + return 1e-13; +} + +template<> constexpr long double GetNumericLimit() +{ + // abs limit is 21 digits + return 1e-15 ; +} + +template +T SolveCubic(T b, T c, T d) +{ + T r0; + // not monotonic + if (b * b >= 3.0 * c) + { + // h has two stationary points, compute them + T v = std::sqrt(b * b - 3.0 * c); + T t1 = (-b - v) / 3.0; + + // Check if h(t1) > 0, in this case make a 2-order approx of h around t1 + T k = ((t1 + b) * t1 + c) * t1 + d; + + if (k > 0.0) + { + //Find leftmost root of 0.5*(r0 -t1)^2*(6*t1+2*b) + k = 0 + r0 = t1 - std::sqrt(-k / (3.0 * t1 + b)); + } else + { + T t2 = (-b + v) / (3.0); + k = ((t2 + b) * t2 + c) * t2 + d; + //Find rightmost root of 0.5*(r0 -t2)^2*(6*t2+2*b) + k1 = 0 + r0 = t2 + std::sqrt(-k / (3.0 * t2 + b)); + } + } + else + { + r0 = -b / 3.0; + if (std::abs(((T(3.0) * r0 + T(2.0) * b) * r0 + c)) < 1e-4) + { + r0 += 1; + } + + } + + /* Do ITER Newton-Raphson iterations */ + /* Break if position of root changes less than 1e.16 */ + T fx,fpx; + for (unsigned int cnt = 0; cnt < iterations; ++cnt) + { + fx = (((r0 + b) * r0 + c) * r0 + d); + + if ((cnt < 7 || std::abs(fx) > GetNumericLimit())) + { + fpx = ((T(3.0) * r0 + T(2.0) * b) * r0 + c); + + r0 -= fx / fpx; + } + else + { + break; + } + } + + return r0; +} + +template +void SolveEigenWithKnownZero(Matrix x, Matrix &E, Matrix &L) +{ + // one eigenvalue is known to be 0. + //the known one... + L(2)=0; + + Matrix v3; + v3 << x(3) * x(7) - x(6) * x(4), + x(6) * x(1) - x(7) * x(0), + x(4) * x(0) - x(3) * x(1); + v3.normalize(); + + T x01_squared = x(0, 1) * x(0, 1); + + // get the two other... + T b = -x(0, 0) - x(1, 1) - x(2, 2); + T c = -x01_squared - x(0, 2) * x(0, 2) - x(1, 2) * x(1, 2) + + x(0, 0) * (x(1, 1) + x(2, 2)) + x(1, 1) * x(2, 2); + T e1, e2; + util::MathUtil::Roots(b, c, e1, e2); + + if (std::abs(e1) < std::abs(e2)) + { + std::swap(e1, e2); + } + L(0) = e1; + L(1) = e2; + + T mx0011 = -x(0, 0) * x(1, 1); + T prec_0 = x(0, 1) * x(1, 2) - x(0, 2) * x(1, 1); + T prec_1 = x(0, 1) * x(0, 2) - x(0, 0) * x(1, 2); + + T e = e1; + T tmp = 1.0 / (e *(x(0, 0) + x(1, 1)) + mx0011 - e * e + x01_squared); + T a1 = -(e * x(0, 2) + prec_0) * tmp; + T a2 = -(e * x(1, 2) + prec_1) * tmp; + T rnorm = ((T)1.0) / std::sqrt(a1 * a1 + a2 * a2 + 1.0); + a1 *= rnorm; + a2 *= rnorm; + Matrix v1(a1, a2, rnorm); + + T tmp2 = 1.0 / (e2 * (x(0, 0) + x(1, 1)) + mx0011 - e2 * e2 + x01_squared); + T a21 = -(e2 * x(0, 2) + prec_0) * tmp2; + T a22 = -(e2 * x(1, 2) + prec_1) * tmp2; + T rnorm2 = 1.0 / std::sqrt(a21 * a21 + a22 * a22 + 1.0); + a21 *= rnorm2; + a22 *= rnorm2; + Matrix v2(a21, a22, rnorm2); + + E = Matrix(v1[0], v2[0], v3[0], v1[1], v2[1], v3[1], v1[2], v2[2], v3[2]); +} + +template +void GaussNewtonRefineL(Matrix &L, T a12, T a13, T a23, T b12, T b13, T b23 ){ + + for(int i=0; i < iterations; ++i) + { + T l1 = L(0); + T l2 = L(1); + T l3 = L(2); + T r1 = l1 * l1 + l2 * l2 + b12 * l1 * l2 - a12; + T r2 = l1 * l1 + l3 * l3 + b13 * l1 * l3 - a13; + T r3 = l2 * l2 + l3 * l3 + b23 * l2 * l3 - a23; + + if (std::abs(r1) + std::abs(r2) + std::abs(r3) < 1e-10) + { + break; + } + + T dr1dl1 = 2.0 * l1 + b12 * l2; + T dr1dl2 = 2.0 * l2 + b12 * l1; + T dr2dl1 = 2.0 * l1 + b13 * l3; + T dr2dl3 = 2.0 * l3 + b13 * l1; + T dr3dl2 = 2.0 * l2 + b23 * l3; + T dr3dl3 = 2.0 * l3 + b23 * l2; + + Matrix r; + r << r1, r2, r3; + + { + T v0 = dr1dl1; + T v1 = dr1dl2; + T v3 = dr2dl1; + T v5 = dr2dl3; + T v7 = dr3dl2; + T v8 = dr3dl3; + T det = 1.0 / (-v0 * v5 * v7 - v1 * v3 * v8); + + Matrix Ji; + Ji << -v5 * v7, -v1 * v8, v1 * v5, + -v3 * v8, v0 * v8, -v0 * v5, + v3 * v7, -v0 * v7, -v1 * v3; + Matrix L1 = L - det * (Ji * r); + + { + T l1 = L1(0); + T l2 = L1(1); + T l3 = L1(2); + T r11 = l1 * l1 + l2 * l2 + b12 * l1 * l2 - a12; + T r12 = l1 * l1 + l3 * l3 + b13 * l1 * l3 - a13; + T r13 = l2 * l2 + l3 * l3 + b23 * l2 * l3 - a23; + if(std::abs(r11) + std::abs(r12) + std::abs(r13) > std::abs(r1) + std::abs(r2) + std::abs(r3)) + { + break; + } + else + { + L = L1; + } + } + } + } +} + +template +int P3P( Matrix y1, Matrix y2, Matrix y3, Matrix x1, Matrix x2, Matrix x3, std::vector>& Rs, std::vector>& Ts) +{ + y1.normalize(); + y2.normalize(); + y3.normalize(); + + + T b12 = -2.0 * (y1.dot(y2)); + T b13 = -2.0 * (y1.dot(y3)); + T b23 = -2.0 * (y2.dot(y3)); + + Matrix d12 = x1 - x2; + Matrix d13 = x1 - x3; + Matrix d23 = x2 - x3; + Matrix d12xd13 = d12.cross(d13); + + T a12 = d12.squaredNorm(); + T a13 = d13.squaredNorm(); + T a23 = d23.squaredNorm(); + + T c31 = -0.5 * b13; + T c23 = -0.5 * b23; + T c12 = -0.5 * b12; + T blob = c12 * c23 * c31 - 1.0; + + T s31_squared = 1.0 - c31 * c31; + T s23_squared = 1.0 - c23 * c23; + T s12_squared = 1.0 - c12 * c12; + + T p3 = (a13 * (a23 * s31_squared - a13 * s23_squared)); + + T p2 = 2.0 * blob * a23 * a13 + a13 * (2.0 * a12 + a13) * s23_squared + a23 * (a23 - a12) * s31_squared; + + T p1 = a23 * (a13 - a23) * s12_squared - a12 * a12 * s23_squared - 2.0 * a12 * (blob * a23 + a13 * s23_squared); + + T p0 = a12 * (a12 * s23_squared - a23 * s12_squared); + + T g=0; + + p3 = 1.0 / p3; + p2 *= p3; + p1 *= p3; + p0 *= p3; + + // get sharpest real root of above... + g = SolveCubic(p2, p1, p0); + + T A00 = a23 * (1.0- g); + T A01 = (a23 * b12) * 0.5; + T A02 = (a23 * b13 * g) * -0.5; + T A11 = a23 - a12 + a13 * g; + T A12 = b23 * (a13 * g - a12) * 0.5; + T A22 = g * (a13 - a23) - a12; + + Matrix A; + A << A00, A01, A02, + A01, A11, A12, + A02, A12, A22; + + // get sorted eigenvalues and eigenvectors given that one should be zero... + Matrix V; + Matrix L; + + SolveEigenWithKnownZero(A, V, L); + + T v = std::sqrt(std::max(T(0), -L(1) / L(0))); + + int valid = 0; + std::vector> Ls(4); + + { + T s = v; + + T w2 = T(1.0) / (s * V(1) - V(0)); + T w0 = (V(3) - s * V(4)) * w2; + T w1 = (V(6) - s * V(7)) * w2; + + T a = T(1.0) / ((a13 - a12) * w1 * w1 - a12 * b13 * w1 - a12); + T b = (a13 * b12 * w1 - a12 * b13 * w0 - T(2.0) * w0 * w1 * (a12 - a13)) * a; + T c = ((a13 - a12) * w0 * w0 + a13 * b12 * w0 + a13) * a; + + if(b * b - 4.0 * c >= 0) + { + T tau1, tau2; + util::MathUtil::Roots(b, c, tau1, tau2); + if(tau1 > 0) + { + T tau = tau1; + T d = a23 / (tau * (b23 + tau) + T(1.0)); + T l2 = std::sqrt(d); + T l3 = tau * l2; + + T l1 = w0 * l2 + w1 * l3; + if(l1 >= 0) + { + Ls[valid] << l1, l2, l3; + ++valid; + } + } + if(tau2 > 0) + { + T tau = tau2; + T d = a23 / (tau * (b23 + tau) + T(1.0)); + T l2 = std::sqrt(d); + T l3 = tau * l2; + T l1 = w0 * l2 + w1 * l3; + if(l1 >= 0) + { + Ls[valid] << l1, l2, l3; + ++valid; + } + } + } + } + + { + T s=-v; + T w2 = T(1.0) / ( s * V(0, 1) - V(0, 0)); + T w0 = (V(1, 0) - s * V(1, 1)) * w2; + T w1 = (V(2, 0) - s * V(2, 1)) * w2; + + T a = T(1.0) / ((a13 - a12) * w1 * w1 - a12 * b13 * w1 - a12); + T b = (a13 * b12 * w1 - a12 * b13 * w0 - T(2.0) * w0 * w1 * (a12 - a13)) * a; + T c = ((a13 - a12) * w0 * w0 + a13 * b12 * w0 + a13) * a; + + if(b * b - 4.0 * c >= 0){ + T tau1, tau2; + + util::MathUtil::Roots(b, c, tau1, tau2); + if(tau1 > 0) + { + T tau = tau1; + T d = a23 / (tau * (b23 + tau) + T(1.0)); + if(d > 0) + { + T l2 = std::sqrt(d); + + T l3 = tau * l2; + + T l1 = w0 * l2 + w1 * l3; + if(l1 >= 0) + { + Ls[valid] << l1, l2, l3; + ++valid; + } + } + } + if (tau2 > 0){ + T tau = tau2; + T d = a23 / (tau * (b23 + tau) + T(1.0)); + if (d > 0) + { + T l2 = std::sqrt(d); + + T l3 = tau * l2; + + T l1 = w0 * l2 + w1 * l3; + if (l1 >= 0) + { + Ls[valid] << l1, l2, l3; + ++valid; + } + } + } + } + } + + for(int i = 0; i < valid; ++i) + { + GaussNewtonRefineL(Ls[i], a12, a13, a23, b12, b13, b23); + } + + Matrix ry1, ry2, ry3; + Matrix yd1; + Matrix yd2; + Matrix yd1xd2; + Matrix X; + X << d12(0),d13(0),d12xd13(0), + d12(1),d13(1),d12xd13(1), + d12(2),d13(2),d12xd13(2); + X=X.inverse(); + + Rs.clear(); + Rs.resize(4); + Ts.clear(); + Ts.resize(4); + for(int i = 0; i < valid; ++i) + { + ry1 = y1 * Ls(i)(0); + ry2 = y2 * Ls(i)(1); + ry3 = y3 * Ls(i)(2); + + yd1 = ry1 - ry2; + yd2 = ry1 - ry3; + yd1xd2 = yd1.cross(yd2); + + Matrix Y; + Y << yd1(0), yd2(0), yd1xd2(0), + yd1(1), yd2(1), yd1xd2(1), + yd1(2), yd2(2), yd1xd2(2); + + Rs[i] = Y * X; + Ts[i] = (ry1 - Rs[i] * x1); + } + + return valid; +} + +} +} +} + +#endif /* _LAMBDA_TWIST_H_ */ diff --git a/src/odometry/pnp.cc b/src/odometry/pnp.cc new file mode 100644 index 0000000..94fb38d --- /dev/null +++ b/src/odometry/pnp.cc @@ -0,0 +1,15 @@ +#include "pnp.h" +#include "lambda_twist.h" + +namespace omni_slam +{ +namespace odometry +{ + +PNP::PNP() +{ +} + +} +} + diff --git a/src/odometry/pnp.h b/src/odometry/pnp.h new file mode 100644 index 0000000..48bc96d --- /dev/null +++ b/src/odometry/pnp.h @@ -0,0 +1,18 @@ +#ifndef _PNP_H_ +#define _PNP_H_ + +namespace omni_slam +{ +namespace odometry +{ + +class PNP +{ +public: + PNP(); +}; + +} +} + +#endif /* _PNP_H_ */ diff --git a/src/util/math_util.h b/src/util/math_util.h index a790be1..8c49265 100644 --- a/src/util/math_util.h +++ b/src/util/math_util.h @@ -5,39 +5,63 @@ namespace omni_slam { namespace util { +namespace MathUtil +{ + +template +inline T FastAtan2(T y, T x) +{ + const T ONEQTR_PI = M_PI / 4.0; + const T THRQTR_PI = 3.0 * M_PI / 4.0; + T r, angle; + T abs_y = std::fabs(y) + 1e-10f; + if (x < 0.0f) + { + r = (x + abs_y) / (abs_y - x); + angle = THRQTR_PI; + } + else + { + r = (x - abs_y) / (x + abs_y); + angle = ONEQTR_PI; + } + angle += (0.1963f * r * r - 0.9817f) * r; + if (y < 0.0f) + { + return -angle; + } + else + { + return angle; + } +} -class MathUtil +template +inline bool Roots(T b, T c,T &r1, T &r2) { -public: - template - static T FastAtan2(T y, T x) + T v = b * b - 4.0 * c; + if (v < 0.) + { + r1 = r2 = 0.5 * b; + return false; + } + + T y = std::sqrt(v); + + if (b < 0) + { + r1 = 0.5 * (-b + y); + r2 = 0.5 * (-b - y); + } + else { - const T ONEQTR_PI = M_PI / 4.0; - const T THRQTR_PI = 3.0 * M_PI / 4.0; - T r, angle; - T abs_y = fabs(y) + 1e-10f; - if (x < 0.0f) - { - r = (x + abs_y) / (abs_y - x); - angle = THRQTR_PI; - } - else - { - r = (x - abs_y) / (x + abs_y); - angle = ONEQTR_PI; - } - angle += (0.1963f * r * r - 0.9817f) * r; - if (y < 0.0f) - { - return -angle; - } - else - { - return angle; - } + r1 = 2.0 * c / (-b + y); + r2 = 2.0 * c / (-b - y); } -}; + return true; +} +} } } #endif /* _MATH_UTIL_H_ */ diff --git a/src/util/tf_util.h b/src/util/tf_util.h index 42419d8..66f75a8 100644 --- a/src/util/tf_util.h +++ b/src/util/tf_util.h @@ -9,60 +9,65 @@ namespace omni_slam { namespace util { +namespace TFUtil +{ + +template +inline Matrix CameraFrameToWorldFrame(Matrix cameraFramePt) +{ + Matrix worldFramePt; + worldFramePt << cameraFramePt(0), cameraFramePt(2), -cameraFramePt(1); + return worldFramePt; +} -class TFUtil +template +inline Matrix WorldFrameToCameraFrame(Matrix worldFramePt) { -public: - template - static Matrix CameraFrameToWorldFrame(Matrix cameraFramePt) - { - Matrix worldFramePt; - worldFramePt << cameraFramePt(0), cameraFramePt(2), -cameraFramePt(1); - return worldFramePt; - } - template - static Matrix WorldFrameToCameraFrame(Matrix worldFramePt) - { - Matrix cameraFramePt; - cameraFramePt << worldFramePt(0), -worldFramePt(2), worldFramePt(1); - return cameraFramePt; - } - template - static Matrix InversePoseMatrix(Matrix poseMat) - { - Matrix rot = poseMat.template block<3, 3>(0, 0); - Matrix t = poseMat.template block<3, 1>(0, 3); - Matrix invMat; - invMat.template block<3, 3>(0, 0) = rot.transpose(); - invMat.template block<3, 1>(0, 3) = -rot.transpose() * t; - return invMat; - } - template - static Matrix GetRotationFromPoseMatrix(Matrix poseMat) - { - return poseMat.template block<3, 3>(0, 0); - } - template - static Matrix TransformPoint(Matrix tf, Matrix pt) - { - Matrix pt_h = pt.homogeneous(); - return tf * pt_h; - } - template - static Matrix RotatePoint(Matrix rot, Matrix pt) - { - return rot * pt; - } - template - static Matrix QuaternionTranslationToPoseMatrix(Quaternion quat, Matrix translation) - { - Matrix pose; - pose.template block<3, 3>(0, 0) = quat.normalized().toRotationMatrix(); - pose.template block<3, 1>(0, 3) = translation; - return pose; - } -}; + Matrix cameraFramePt; + cameraFramePt << worldFramePt(0), -worldFramePt(2), worldFramePt(1); + return cameraFramePt; +} + +template +inline Matrix InversePoseMatrix(Matrix poseMat) +{ + Matrix rot = poseMat.template block<3, 3>(0, 0); + Matrix t = poseMat.template block<3, 1>(0, 3); + Matrix invMat; + invMat.template block<3, 3>(0, 0) = rot.transpose(); + invMat.template block<3, 1>(0, 3) = -rot.transpose() * t; + return invMat; +} + +template +inline Matrix GetRotationFromPoseMatrix(Matrix poseMat) +{ + return poseMat.template block<3, 3>(0, 0); +} +template +inline Matrix TransformPoint(Matrix tf, Matrix pt) +{ + Matrix pt_h = pt.homogeneous(); + return tf * pt_h; +} + +template +inline Matrix RotatePoint(Matrix rot, Matrix pt) +{ + return rot * pt; +} + +template +inline Matrix QuaternionTranslationToPoseMatrix(Quaternion quat, Matrix translation) +{ + Matrix pose; + pose.template block<3, 3>(0, 0) = quat.normalized().toRotationMatrix(); + pose.template block<3, 1>(0, 3) = translation; + return pose; +} + +} } }