Hugging Face's logo

Spaces:
pablovela5620
/
MiniDPVO
Sleeping

App Files Community
1
MiniDPVO
File size: 6,801 Bytes 
899c526
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
 

#ifndef SO3_HEADER
#define SO3_HEADER

#include <cuda.h>
#include <stdio.h>
#include <Eigen/Dense>
#include <Eigen/Geometry> 

#include "common.h"

template <typename Scalar>
class SO3 {
  public:
    const static int constexpr K = 3; // manifold dimension
    const static int constexpr N = 4; // embedding dimension

    using Vector3 = Eigen::Matrix<Scalar,3,1>;
    using Vector4 = Eigen::Matrix<Scalar,4,1>;
    using Matrix3 = Eigen::Matrix<Scalar,3,3>;

    using Tangent = Eigen::Matrix<Scalar,K,1>;
    using Data = Eigen::Matrix<Scalar,N,1>;

    using Point = Eigen::Matrix<Scalar,3,1>;
    using Point4 = Eigen::Matrix<Scalar,4,1>;
    using Transformation = Eigen::Matrix<Scalar,3,3>;
    using Adjoint = Eigen::Matrix<Scalar,K,K>;
    using Quaternion = Eigen::Quaternion<Scalar>;

    EIGEN_DEVICE_FUNC SO3(Quaternion const& q) : unit_quaternion(q) {
      unit_quaternion.normalize();
    };

    EIGEN_DEVICE_FUNC SO3(const Scalar *data) : unit_quaternion(data) {
      unit_quaternion.normalize();
    };

    EIGEN_DEVICE_FUNC SO3() {
      unit_quaternion = Quaternion::Identity();
    }

    EIGEN_DEVICE_FUNC SO3<Scalar> inv() {
      return SO3<Scalar>(unit_quaternion.conjugate());
    }

    EIGEN_DEVICE_FUNC Data data() const {
      return unit_quaternion.coeffs();
    }

    EIGEN_DEVICE_FUNC SO3<Scalar> operator*(SO3<Scalar> const& other) {
      return SO3(unit_quaternion * other.unit_quaternion);
    }

    EIGEN_DEVICE_FUNC Point operator*(Point const& p) const {
      const Quaternion& q = unit_quaternion;
      Point uv = q.vec().cross(p);
      uv += uv;
      return p + q.w()*uv + q.vec().cross(uv);
    }

    EIGEN_DEVICE_FUNC Point4 act4(Point4 const& p) const {
      Point4 p1; p1 << this->operator*(p.template segment<3>(0)), p(3);
      return p1;
    }
    
    EIGEN_DEVICE_FUNC Adjoint Adj() const {
      return unit_quaternion.toRotationMatrix();
    }

    EIGEN_DEVICE_FUNC Transformation Matrix() const {
      return unit_quaternion.toRotationMatrix();
    }

    EIGEN_DEVICE_FUNC Eigen::Matrix<Scalar,4,4> Matrix4x4() const {
      Eigen::Matrix<Scalar,4,4> T = Eigen::Matrix<Scalar,4,4>::Identity();
      T.template block<3,3>(0,0) = Matrix();
      return T;
    }

    EIGEN_DEVICE_FUNC Eigen::Matrix<Scalar,4,4> orthogonal_projector() const {
      // jacobian action on a point
      Eigen::Matrix<Scalar,4,4> J = Eigen::Matrix<Scalar,4,4>::Zero();
      J.template block<3,3>(0,0) = 0.5 * (
        unit_quaternion.w() * Matrix3::Identity() + 
        SO3<Scalar>::hat(-unit_quaternion.vec())
      );
      
      J.template block<1,3>(3,0) = 0.5 * (-unit_quaternion.vec());
      return J;
    }

    EIGEN_DEVICE_FUNC Tangent Adj(Tangent const& a) const {
      return Adj() * a;
    }

    EIGEN_DEVICE_FUNC Tangent AdjT(Tangent const& a) const {
      return Adj().transpose() * a;
    }

    EIGEN_DEVICE_FUNC static Transformation hat(Tangent const& phi) {
      Transformation Phi;
      Phi << 
        0.0, -phi(2), phi(1), 
        phi(2), 0.0, -phi(0), 
        -phi(1), phi(0), 0.0;

      return Phi;
    }

    EIGEN_DEVICE_FUNC static Adjoint adj(Tangent const& phi) {
      return SO3<Scalar>::hat(phi);
    }

    EIGEN_DEVICE_FUNC Tangent Log() const {
      using std::abs;
      using std::atan;
      using std::sqrt;
      Scalar squared_n = unit_quaternion.vec().squaredNorm();
      Scalar w = unit_quaternion.w();

      Scalar two_atan_nbyw_by_n;

      /// Atan-based log thanks to
      ///
      /// C. Hertzberg et al.:
      /// "Integrating Generic Sensor Fusion Algorithms with Sound State
      /// Representation through Encapsulation of Manifolds"
      /// Information Fusion, 2011

      if (squared_n < EPS * EPS) {
        // If quaternion is normalized and n=0, then w should be 1;
        // w=0 should never happen here!
        Scalar squared_w = w * w;
        two_atan_nbyw_by_n =
            Scalar(2) / w - Scalar(2.0/3.0) * (squared_n) / (w * squared_w);
      } else {
        Scalar n = sqrt(squared_n);
        if (abs(w) < EPS) {
          if (w > Scalar(0)) {
            two_atan_nbyw_by_n = Scalar(PI) / n;
          } else {
            two_atan_nbyw_by_n = -Scalar(PI) / n;
          }
        } else {
          two_atan_nbyw_by_n = Scalar(2) * atan(n / w) / n;
        }
      }

      return two_atan_nbyw_by_n * unit_quaternion.vec();
    }

    EIGEN_DEVICE_FUNC static SO3<Scalar> Exp(Tangent const& phi) {
      Scalar theta2 = phi.squaredNorm();
      Scalar theta = sqrt(theta2);
      Scalar imag_factor;
      Scalar real_factor;

      if (theta < EPS) {
        Scalar theta4 = theta2 * theta2;
        imag_factor = Scalar(0.5) - Scalar(1.0/48.0) * theta2 + Scalar(1.0/3840.0) * theta4;
        real_factor = Scalar(1) - Scalar(1.0/8.0) * theta2 + Scalar(1.0/384.0) * theta4;
      } else {
        imag_factor = sin(.5 * theta) / theta;
        real_factor = cos(.5 * theta);
      }

      Quaternion q(real_factor, imag_factor*phi.x(), imag_factor*phi.y(), imag_factor*phi.z());
      return SO3<Scalar>(q);
    }
    
    EIGEN_DEVICE_FUNC static Adjoint left_jacobian(Tangent const& phi) {
      // left jacobian
      Matrix3 I = Matrix3::Identity();
      Matrix3 Phi = SO3<Scalar>::hat(phi);
      Matrix3 Phi2 = Phi * Phi;

      Scalar theta2 = phi.squaredNorm();
      Scalar theta = sqrt(theta2);

      Scalar coef1 = (theta < EPS) ? 
        Scalar(1.0/2.0) - Scalar(1.0/24.0) * theta2 : 
        (1.0 - cos(theta)) / theta2;
      
      Scalar coef2 = (theta < EPS) ? 
        Scalar(1.0/6.0) - Scalar(1.0/120.0) * theta2 : 
        (theta - sin(theta)) / (theta2 * theta); 

      return I + coef1 * Phi + coef2 * Phi2;
    }

    EIGEN_DEVICE_FUNC static Adjoint left_jacobian_inverse(Tangent const& phi) {
      // left jacobian inverse
      Matrix3 I = Matrix3::Identity();
      Matrix3 Phi = SO3<Scalar>::hat(phi);
      Matrix3 Phi2 = Phi * Phi;

      Scalar theta2 = phi.squaredNorm();
      Scalar theta = sqrt(theta2);
      Scalar half_theta = Scalar(.5) * theta ;

      Scalar coef2 = (theta < EPS) ? Scalar(1.0/12.0) : 
           (Scalar(1) -
            theta * cos(half_theta) / (Scalar(2) * sin(half_theta))) /
               (theta * theta);

      return I + Scalar(-0.5) * Phi + coef2 * Phi2;
    }

    EIGEN_DEVICE_FUNC static Eigen::Matrix<Scalar,3,3> act_jacobian(Point const& p) {
      // jacobian action on a point
      return SO3<Scalar>::hat(-p);
    }

    EIGEN_DEVICE_FUNC static Eigen::Matrix<Scalar,4,3> act4_jacobian(Point4 const& p) {
      // jacobian action on a point
      Eigen::Matrix<Scalar,4,3> J = Eigen::Matrix<Scalar,4,3>::Zero();
      J.template block<3,3>(0,0) = SO3<Scalar>::hat(-p.template segment<3>(0));
      return J;
    }

  private:
    Quaternion unit_quaternion;

};

#endif