Hugging Face's logo

Spaces:
pablovela5620
/
MiniDPVO
Sleeping

App Files Community
1
MiniDPVO
File size: 6,700 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
 

#ifndef Sim3_HEADER
#define Sim3_HEADER

#include <stdio.h>
#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Geometry> 

#include "common.h"
#include "so3.h"
#include "rxso3.h"


template <typename Scalar>
class Sim3 {
  public:
    const static int constexpr K = 7; // manifold dimension
    const static int constexpr N = 8; // 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 Point = Eigen::Matrix<Scalar,3,1>;
    using Point4 = Eigen::Matrix<Scalar,4,1>;
    using Data = Eigen::Matrix<Scalar,N,1>;
    using Transformation = Eigen::Matrix<Scalar,4,4>;
    using Adjoint = Eigen::Matrix<Scalar,K,K>;

    EIGEN_DEVICE_FUNC Sim3() {
      translation = Vector3::Zero();
    }

    EIGEN_DEVICE_FUNC Sim3(RxSO3<Scalar> const& rxso3, Vector3 const& t)
      : rxso3(rxso3), translation(t) {};

    EIGEN_DEVICE_FUNC Sim3(const Scalar *data) 
      : translation(data), rxso3(data+3)  {};

    EIGEN_DEVICE_FUNC Sim3<Scalar> inv() {
      return Sim3<Scalar>(rxso3.inv(), -(rxso3.inv() * translation));
    }

    EIGEN_DEVICE_FUNC Data data() const {
      Data data_vec; data_vec << translation, rxso3.data();
      return data_vec;
    }

    EIGEN_DEVICE_FUNC Sim3<Scalar> operator*(Sim3<Scalar> const& other) {
      return Sim3(rxso3 * other.rxso3, translation + rxso3 * other.translation);
    }

    EIGEN_DEVICE_FUNC Point operator*(Point const& p) const {
      return (rxso3 * p) + translation;
    }
    
    EIGEN_DEVICE_FUNC Point4 act4(Point4 const& p) const {
      Point4 p1; p1 << rxso3 * p.template segment<3>(0) + p(3) * translation , p(3);
      return p1;
    }

    EIGEN_DEVICE_FUNC Transformation Matrix() const {
      Transformation T = Transformation::Identity();
      T.template block<3,3>(0,0) = rxso3.Matrix();
      T.template block<3,1>(0,3) = translation;
      return T;
    }

    EIGEN_DEVICE_FUNC Transformation Matrix4x4() const {
      Transformation T = Transformation::Identity();
      T.template block<3,3>(0,0) = rxso3.Matrix();
      T.template block<3,1>(0,3) = translation;
      return T;
    }

    EIGEN_DEVICE_FUNC Eigen::Matrix<Scalar,8,8> orthogonal_projector() const {
      // jacobian action on a point
      Eigen::Matrix<Scalar,8,8> J = Eigen::Matrix<Scalar,8,8>::Zero();
      J.template block<3,3>(0,0) = Matrix3::Identity();
      J.template block<3,3>(0,3) = SO3<Scalar>::hat(-translation);
      J.template block<3,1>(0,6) = translation;
      J.template block<5,5>(3,3) = rxso3.orthogonal_projector();
      return J;
    }

    EIGEN_DEVICE_FUNC Adjoint Adj() const {
      Adjoint Ad = Adjoint::Identity();
      Matrix3 sR = rxso3.Matrix();
      Matrix3 tx = SO3<Scalar>::hat(translation);
      Matrix3 R = rxso3.Rotation();

      Ad.template block<3,3>(0,0) = sR;
      Ad.template block<3,3>(0,3) = tx * R;
      Ad.template block<3,1>(0,6) = -translation;
      Ad.template block<3,3>(3,3) = R;

      return Ad;
    }

    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& tau_phi_sigma) {
      Vector3 tau = tau_phi_sigma.template segment<3>(0);
      Vector3 phi = tau_phi_sigma.template segment<3>(3);
      Scalar sigma = tau_phi_sigma(6);

      Matrix3 Phi = SO3<Scalar>::hat(phi);
      Matrix3 I = Matrix3::Identity();
      
      Transformation Omega = Transformation::Zero();
      Omega.template block<3,3>(0,0) = Phi + sigma * I;
      Omega.template block<3,1>(0,3) = tau;
      
      return Omega;
    }

    EIGEN_DEVICE_FUNC  static Adjoint adj(Tangent const& tau_phi_sigma) {
      Adjoint ad = Adjoint::Zero();
      Vector3 tau = tau_phi_sigma.template segment<3>(0);
      Vector3 phi = tau_phi_sigma.template segment<3>(3);
      Scalar sigma = tau_phi_sigma(6);

      Matrix3 Tau = SO3<Scalar>::hat(tau);
      Matrix3 Phi = SO3<Scalar>::hat(phi);
      Matrix3 I = Matrix3::Identity();

      ad.template block<3,3>(0,0) = Phi + sigma * I;
      ad.template block<3,3>(0,3) = Tau;
      ad.template block<3,1>(0,6) = -tau;
      ad.template block<3,3>(3,3) = Phi;

      return ad;
    }
    

    EIGEN_DEVICE_FUNC Tangent Log() const {
      // logarithm map
      Vector4 phi_sigma = rxso3.Log();      
      Matrix3 W = RxSO3<Scalar>::calcW(phi_sigma);
      
      Tangent tau_phi_sigma; 
      tau_phi_sigma << W.inverse() * translation, phi_sigma;

      return tau_phi_sigma;
    }

    EIGEN_DEVICE_FUNC static Sim3<Scalar> Exp(Tangent const& tau_phi_sigma) {
      // exponential map
      Vector3 tau = tau_phi_sigma.template segment<3>(0);
      Vector4 phi_sigma = tau_phi_sigma.template segment<4>(3);
      
      RxSO3<Scalar> rxso3 = RxSO3<Scalar>::Exp(phi_sigma);
      Matrix3 W = RxSO3<Scalar>::calcW(phi_sigma);

      return Sim3<Scalar>(rxso3, W*tau);
    }

    EIGEN_DEVICE_FUNC static Adjoint left_jacobian(Tangent const& tau_phi_sigma) {
      // left jacobian
      Adjoint const Xi = adj(tau_phi_sigma);
      Adjoint const Xi2 = Xi * Xi;
      Adjoint const Xi4 = Xi2 * Xi2;

      return Adjoint::Identity() 
        + Scalar(1.0/2.0)*Xi
        + Scalar(1.0/6.0)*Xi2
        + Scalar(1.0/24.0)*Xi*Xi2
        + Scalar(1.0/120.0)*Xi4;
        + Scalar(1.0/720.0)*Xi*Xi4;
    }

    EIGEN_DEVICE_FUNC static Adjoint left_jacobian_inverse(Tangent const& tau_phi_sigma) {
      // left jacobian inverse
      Adjoint const Xi = adj(tau_phi_sigma);
      Adjoint const Xi2 = Xi * Xi;
      Adjoint const Xi4 = Xi2 * Xi2;

      return Adjoint::Identity() 
        - Scalar(1.0/2.0)*Xi
        + Scalar(1.0/12.0)*Xi2
        - Scalar(1.0/720.0)*Xi4;
    }

    EIGEN_DEVICE_FUNC static Eigen::Matrix<Scalar,3,7> act_jacobian(Point const& p) {
      // jacobian action on a point
      Eigen::Matrix<Scalar,3,7> J;
      J.template block<3,3>(0,0) = Matrix3::Identity();
      J.template block<3,3>(0,3) = SO3<Scalar>::hat(-p);
      J.template block<3,1>(0,6) = p;
      return J;
    }

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

  private:
    Vector3 translation;
    RxSO3<Scalar> rxso3;
};

#endif