├── .gitmodules
├── 1_new.obj
├── 2_new.obj
├── CMakeLists.txt
├── LICENSE
├── README.md
├── get_dr.cpp
├── test.py
├── util_3drotation_log_exp.cpp
└── util_3drotation_log_exp.h


/.gitmodules:
--------------------------------------------------------------------------------
1 | [submodule "pybind11"]
2 | 	path = pybind11
3 | 	url = https://github.com/pybind/pybind11.git
4 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.12)
 2 | project(my_project)
 3 | 
 4 | # force a specific python version
 5 | # set(PYTHON_LIBRARIES "/usr/lib/x86_64-linux-gnu/libpython2.7.a")
 6 | # set(PYTHON_INCLUDE_DIR "/usr/include/python2.7")
 7 | 
 8 | find_package(PythonInterp REQUIRED)
 9 | find_package(PythonLibs REQUIRED)
10 | 
11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -w -shared -std=c++11")
12 | 
13 | add_subdirectory(pybind11)
14 | 
15 | pybind11_add_module(get_dr get_dr.cpp util_3drotation_log_exp.h util_3drotation_log_exp.cpp)
16 | 
17 | 
18 | INCLUDE_DIRECTORIES("/usr/include/eigen3")
19 | INCLUDE_DIRECTORIES(${CMAKE_CURRENT_LIST_DIR})
20 | 
21 | link_directories("/usr/local/lib")
22 | 
23 | target_link_libraries(get_dr PRIVATE -lOpenMeshCore -lOpenMeshTools)
24 | 
25 | 
26 | 
27 | 
28 | 
29 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | BSD 3-Clause License
 2 | 
 3 | Copyright (c) 2019, Wooqy
 4 | All rights reserved.
 5 | 
 6 | Redistribution and use in source and binary forms, with or without
 7 | modification, are permitted provided that the following conditions are met:
 8 | 
 9 | * Redistributions of source code must retain the above copyright notice, this
10 |   list of conditions and the following disclaimer.
11 | 
12 | * Redistributions in binary form must reproduce the above copyright notice,
13 |   this list of conditions and the following disclaimer in the documentation
14 |   and/or other materials provided with the distribution.
15 | 
16 | * Neither the name of the copyright holder nor the names of its
17 |   contributors may be used to endorse or promote products derived from
18 |   this software without specific prior written permission.
19 | 
20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # get_dr_py_API
 2 | This package is available for using pybind11 to create get_dr python API.
 3 | 
 4 | Deformation Representation Feature (DR Feature) is a kind of fundamental and stable tool in geometry learning area. For more background knowledges and properties of DR feature, we suggest these papers in **Citation** section for further reading. 
 5 | 
 6 | # Dependencies
 7 | This package depends on <a href='https://www.openmesh.org'>OpenMesh</a> and <a href='http://eigen.tuxfamily.org/index.php?title=Main_Page'>Eigen</a> libraries. To run the dr computation code, please make sure you have Eigen and OpenMesh precompiled on your computer and modificate the include path in CMakeList.txt file. 
 8 | 
 9 | # Usage
10 | We recommend to use cmake to compile this package and a python API will be automatically created to compute DR features.
11 | ```
12 | git clone --recursive https://github.com/QianyiWu/get_dr_py.git
13 | cd get_dr_py
14 | ```
15 | 
16 | After obtaining this repository, run the following code in the repository path:
17 | ```
18 | mkdir build
19 | cd build
20 | cmake ..
21 | make -j4
22 | ```
23 | You will then get a .so file.
24 | The <a href='https://github.com/QianyiWu/get_dr_py/blob/master/test.py'>test.py</a> with *1_new.obj* and *2_new.obj* could be used for testing.
25 | 
26 | # Environment Tests
27 | Currently we have fully tested this package on Ubuntu 16.04 LTS environment. Windows and MacOS are not ensured working.
28 | 
29 | # Citation
30 | Please cite the following papers if it helps your research: 
31 | 
32 | <a href="https://arxiv.org/abs/1902.09887">Disentangled Representation Learning for 3D Face Shape</a>
33 | 
34 |     @inproceedings{Jiang2019Disentangled
35 |           title={Disentangled Representation Learning for 3D Face Shape},
36 |           author={Jiang, Zi-Hang and Wu, Qianyi and Chen, Keyu and Zhang, Juyong}
37 |           booktitle={IEEE Conference on Computer Vision and Pattern Recognition (CVPR)},
38 |           year={2019},
39 |     }
40 | 
41 | <a href="http://geometrylearning.com/paper/Sparse2018.pdf">Sparse Data Driven Mesh Deformation</a>
42 | 
43 |     @article{Gao2017SparseDD,
44 |           title={Sparse Data Driven Mesh Deformation},
45 |           author={Lin Gao and Yu-Kun Lai and Jie Yang and Ling-Xiao Zhang and Leif Kobbelt and Shihong Xia},
46 |           journal={CoRR},
47 |           year={2017},
48 |           volume={abs/1709.01250}
49 |     }
50 | 
51 | <a href="https://arxiv.org/abs/1803.06802v2">Alive Caricature from 2D to 3D</a>
52 | 
53 |     @inproceedings{wu2018alive,
54 |       title={Alive Caricature from 2D to 3D},
55 |       author={Wu, Qianyi and Zhang, Juyong and Lai, Yu-Kun and Zheng, Jianmin and Cai, Jianfei},
56 |       booktitle={IEEE Conference on Computer Vision and Pattern Recognition (CVPR)},
57 |       year={2018},
58 |     }
59 | 


--------------------------------------------------------------------------------
/get_dr.cpp:
--------------------------------------------------------------------------------
  1 | #include "util_3drotation_log_exp.h"
  2 | #include <OpenMesh/Core/IO/MeshIO.hh>
  3 | #include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
  4 | #include <Eigen/Dense>
  5 | #include <Eigen/Sparse>
  6 | #include <string>
  7 | #include <iostream>
  8 | #include <pybind11/numpy.h>
  9 | #include <pybind11/pybind11.h>
 10 | #include <vector>;
 11 | 
 12 | namespace py=pybind11;
 13 | 
 14 | struct TriTraits : public OpenMesh::DefaultTraits
 15 | {
 16 |   /// Use double precision points
 17 |   typedef OpenMesh::Vec3d Point;
 18 |   /// Use double precision Normals
 19 |   typedef OpenMesh::Vec3d Normal;
 20 |   /// Use double precision TexCood2D
 21 |   typedef OpenMesh::Vec2d TexCoord2D;
 22 | 
 23 |   /// Use RGBA Color
 24 |   typedef OpenMesh::Vec4f Color;
 25 | 
 26 |     /// Status
 27 |     VertexAttributes(OpenMesh::Attributes::Status);
 28 |     FaceAttributes(OpenMesh::Attributes::Status);
 29 |     EdgeAttributes(OpenMesh::Attributes::Status);
 30 | 
 31 | //    VertexAttributes(
 32 | //            OpenMesh::Attributes::
 33 | //            )
 34 | };
 35 | 
 36 | /// Simple Name for Mesh
 37 | typedef OpenMesh::TriMesh_ArrayKernelT<TriTraits>  TriMesh;
 38 | 
 39 | class DR_feature{
 40 | public:
 41 |     DR_feature();
 42 | 	void read_ref_mesh(std::string ref_mesh_name);
 43 | 	void read_defor_mesh(std::string defor_mesh_name);
 44 | 	std::vector<double> get_feature(std::string ref_mesh_name, std::string defor_mesh_name);
 45 | 
 46 | 
 47 | private:
 48 | 	TriMesh ref_mesh_;
 49 | 	TriMesh defor_mesh_;
 50 | 
 51 | 	OpenMesh::EPropHandleT<double> LB_weights;
 52 | 	OpenMesh::VPropHandleT<Eigen::Matrix3d> T_matrixs;
 53 | private:
 54 | 	void compute_ref_LB_weight();
 55 |     void compute_Ti(TriMesh::VertexHandle v_it,TriMesh::VertexHandle v_to_it);
 56 | };
 57 | 
 58 | DR_feature::DR_feature(){
 59 | 	ref_mesh_.add_property(LB_weights);
 60 | 	defor_mesh_.add_property(T_matrixs);
 61 | }
 62 | 
 63 | void DR_feature::read_ref_mesh(std::string ref_mesh_name){
 64 | 	if(!OpenMesh::IO::read_mesh(ref_mesh_, ref_mesh_name)){
 65 |         std::cout<<"read_ref_mesh_ : read file wrong!!!"<<std::endl;
 66 |         return ;
 67 |     }
 68 | 
 69 |     compute_ref_LB_weight();
 70 | }
 71 | 
 72 | void DR_feature::compute_ref_LB_weight(){
 73 | 	TriMesh::EdgeIter e_it, e_end(ref_mesh_.edges_end());
 74 |     TriMesh::HalfedgeHandle    h0, h1, h2;
 75 |     TriMesh::VertexHandle      v0, v1;
 76 |     TriMesh::Point             p0, p1, p2, d0, d1;
 77 |     TriMesh::Scalar w;
 78 |     for (e_it=ref_mesh_.edges_begin(); e_it!=e_end; e_it++)
 79 |     {
 80 |         w  = 0.0;
 81 |         if(ref_mesh_.is_boundary(*e_it))
 82 |         {
 83 |             h0 = ref_mesh_.halfedge_handle(e_it.handle(),0);
 84 |             if(ref_mesh_.is_boundary(h0))
 85 |                 h0 = ref_mesh_.opposite_halfedge_handle(h0);
 86 | 
 87 |             v0 = ref_mesh_.to_vertex_handle(h0);
 88 |             v1 = ref_mesh_.from_vertex_handle(h0);
 89 |             p0 = ref_mesh_.point(v0);
 90 |             p1 = ref_mesh_.point(v1);
 91 |             h1 = ref_mesh_.next_halfedge_handle(h0);
 92 |             p2 = ref_mesh_.point(ref_mesh_.to_vertex_handle(h1));
 93 |             d0 = (p0-p2).normalize();
 94 |             d1 = (p1-p2).normalize();
 95 |             w += 2.0 / tan(acos(std::min(0.99, std::max(-0.99, (d0|d1)))));
 96 |             w = std::max(0.0, w);
 97 | 
 98 |             if(std::isnan(w))
 99 |                 std::cout<<"Some weight NAN"<<std::endl;
100 |             ref_mesh_.property(LB_weights,e_it) = w;
101 |             continue;
102 |         }
103 |         h0 = ref_mesh_.halfedge_handle(e_it.handle(),0);
104 |         v0 = ref_mesh_.to_vertex_handle(h0);
105 |         p0 = ref_mesh_.point(v0);
106 | 
107 |         h1 = ref_mesh_.opposite_halfedge_handle(h0);
108 |         v1 = ref_mesh_.to_vertex_handle(h1);
109 |         p1 = ref_mesh_.point(v1);
110 | 
111 |         h2 = ref_mesh_.next_halfedge_handle(h0);
112 |         p2 = ref_mesh_.point(ref_mesh_.to_vertex_handle(h2));
113 |         d0 = (p0 - p2).normalize();
114 |         d1 = (p1 - p2).normalize();
115 |         w += 1.0/ tan(acos(std::max(-1.0, std::min(1.0, dot(d1,d0) ))));
116 | 
117 |         h2 = ref_mesh_.next_halfedge_handle(h1);
118 |         p2 = ref_mesh_.point(ref_mesh_.to_vertex_handle(h2));
119 |         d0 = (p0 - p2).normalize();
120 |         d1 = (p1 - p2).normalize();
121 |         w += 1.0 / tan(acos(std::max(-1.0, std::min(1.0, dot(d1,d0)))));
122 | 
123 |         if(std::isnan(w))
124 |             std::cout<<"Some weight is NAN"<<std::endl;
125 |         w = std::max(0.0, w);
126 |         ref_mesh_.property(LB_weights,e_it) = w;
127 |     }
128 | }
129 | 
130 | void DR_feature::read_defor_mesh(std::string _filename){
131 | 	if(!OpenMesh::IO::read_mesh(defor_mesh_,_filename))
132 |     {
133 |         std::cout<<"read_defor_mesh : read file wrong"<<std::endl;
134 |         return ;
135 |     }
136 | 
137 |     TriMesh::FaceIter f_it = defor_mesh_.faces_begin();
138 |     for(;f_it!=defor_mesh_.faces_end();f_it++)
139 |     {
140 |         TriMesh::FaceHandle f_h = *f_it;
141 |         TriMesh::FaceVertexIter fe_it0,fe_it1;
142 |         fe_it0 = ref_mesh_.fv_iter(f_h);
143 |         fe_it1 = defor_mesh_.fv_iter(f_h);
144 |         int v00,v01,v02,v10,v11,v12;
145 |         v00 = (*fe_it0).idx();fe_it0++;
146 |         v01 = (*fe_it0).idx();fe_it0++;
147 |         v02 = (*fe_it0).idx();fe_it0++;
148 | 
149 |         v10 = (*fe_it1).idx();fe_it1++;
150 |         v11 = (*fe_it1).idx();fe_it1++;
151 |         v12 = (*fe_it1).idx();fe_it1++;
152 | 
153 |         if(v00 == v10)
154 |         {
155 |             if(v01!=v11||v02!=v12)
156 |             {
157 |                 std::cout<<"defor and ref are not compatible!!!"<<std::endl;
158 |                 return;
159 |             }
160 |         }
161 |         else if(v00 == v11)
162 |         {
163 |             if(v01!=v12||v02!=v10)
164 |             {
165 |                 std::cout<<"defor and ref are not compatible!!!"<<std::endl;
166 |                 return;
167 |             }
168 |         }
169 |         else if(v00 == v12)
170 |         {
171 |             if(v01!=v10||v02!=v11)
172 |             {
173 |                 std::cout<<"defor and ref are not compatible!!!"<<std::endl;
174 |                 return;
175 |             }
176 |         }
177 |         else
178 |         {
179 |             std::cout<<"defor and ref are not compatible!!!"<<std::endl;
180 |             return;
181 |         }
182 |     }
183 | 
184 |     TriMesh::VertexIter v_it, v_to_it;
185 |     for(v_it=ref_mesh_.vertices_begin(),v_to_it=defor_mesh_.vertices_begin()
186 |         ;v_it!=ref_mesh_.vertices_end()&&v_to_it!=defor_mesh_.vertices_end()
187 |         ;v_it++,v_to_it++)
188 |     {
189 |         if((*v_it).idx()!=(*v_to_it).idx())
190 |             std::cout<<"DR_feature::compute_ref_to_defor_Tmatrixs different topology!!!"<<std::endl;
191 |         compute_Ti(*v_it,*v_to_it);
192 |     }
193 | 
194 | }
195 | 
196 | void DR_feature::compute_Ti(TriMesh::VertexHandle v_it, TriMesh::VertexHandle v_to_it){
197 |     if(v_it.idx()!=v_to_it.idx())
198 |     	std::cout<<"compute_Ti correspond is wrong!!!"<<std::endl;
199 |     TriMesh::VertexEdgeIter veiter=ref_mesh_.ve_iter(v_it);
200 |     int v_id = v_it.idx();
201 |     TriMesh::Point p0,p1;
202 |     p0 = ref_mesh_.point(v_it);
203 |     p1 = defor_mesh_.point(v_to_it);
204 |     Eigen::Matrix3d L,RI;
205 |     L.setZero();
206 |     RI.setZero();
207 |     double tolerance = 1.0e-6;
208 |     TriMesh::HalfedgeHandle h_e=ref_mesh_.halfedge_handle(v_it);
209 |     TriMesh::VertexHandle test_v = ref_mesh_.to_vertex_handle(h_e);
210 |     TriMesh::Point tp0,tp1;
211 |     tp0 = ref_mesh_.point(v_it); tp1 = ref_mesh_.point(test_v);
212 |     double scale=1.0;
213 |     if(((tp0[0]-tp1[0])*(tp0[0]-tp1[0])+(tp0[1]-tp1[1])*(tp0[1]-tp1[1])+(tp0[2]-tp1[2])*(tp0[2]-tp1[2]))<0.1)
214 |         scale = 100;
215 | 
216 |     for(;veiter.is_valid();veiter++)
217 |     {
218 |         double weight = 1.0;
219 |         int to_id;
220 |         TriMesh::VertexHandle to_v=ref_mesh_.to_vertex_handle(ref_mesh_.halfedge_handle(*veiter, 0));
221 |         if(to_v.idx()==v_id)
222 |             to_v = ref_mesh_.from_vertex_handle(ref_mesh_.halfedge_handle(*veiter, 0));
223 |         to_id = to_v.idx();
224 |         Eigen::Vector3d eij0,eij1;
225 |         TriMesh::Point q0,q1;
226 |         q0 = ref_mesh_.point(to_v);
227 |         q1 = defor_mesh_.point(to_v);
228 |         eij0(0) = p0[0]-q0[0];
229 |         eij0(1) = p0[1]-q0[1];
230 |         eij0(2) = p0[2]-q0[2];
231 |         eij0*=weight*scale;
232 | 
233 |         eij1(0) = p1[0]-q1[0];
234 |         eij1(1) = p1[1]-q1[1];
235 |         eij1(2) = p1[2]-q1[2];
236 |         eij1*=weight*scale;
237 | 
238 |         L+=eij1*eij0.transpose();
239 |         RI+=eij0*eij0.transpose();
240 |     }
241 |     Eigen::Matrix3d T;
242 |     if(fabs(RI.determinant())>tolerance)
243 |          T = L*RI.inverse();
244 |     else
245 |     {
246 |         Eigen::JacobiSVD<Eigen::MatrixXd> svd(RI, Eigen::ComputeThinU | Eigen::ComputeThinV);
247 |         Eigen::Matrix3d U,V;
248 |         U=svd.matrixU();
249 |         V=svd.matrixV();
250 |         Eigen::Matrix3d S_inv(svd.singularValues().asDiagonal());
251 |         for(int i=0;i<3;i++)
252 |         {
253 |             if(fabs(S_inv(i,i))>tolerance)
254 |                 S_inv(i,i)=1.0/S_inv(i,i);
255 |             else
256 |                 S_inv(i,i)=0.0;
257 |         }
258 |         T = L*V*S_inv*U.transpose();
259 |     }
260 |     defor_mesh_.property(T_matrixs,v_it) = T;
261 | }
262 | 
263 | 
264 | std::vector<double> DR_feature::get_feature(std::string ref_mesh_name, std::string defor_mesh_name){
265 | 	read_ref_mesh(ref_mesh_name);
266 | 	read_defor_mesh(defor_mesh_name);
267 | 
268 | 	std::vector<double> dr_feature;
269 | 
270 | 	TriMesh::VertexIter v_it;
271 |     for(v_it=defor_mesh_.vertices_begin(); v_it!=defor_mesh_.vertices_end(); v_it++)
272 |     {
273 |         Eigen::Matrix3d T = defor_mesh_.property(T_matrixs,*v_it);
274 |         Eigen::JacobiSVD<Eigen::MatrixXd> svd(T, Eigen::ComputeThinU | Eigen::ComputeThinV);
275 |         Eigen::Matrix3d U,V;
276 |         U=svd.matrixU();
277 |         V=svd.matrixV();
278 |         Eigen::Matrix3d S(svd.singularValues().asDiagonal());
279 |         Eigen::Matrix3d Temp=Eigen::Matrix3d::Identity();
280 |         Temp(2,2) = (U*V.transpose()).determinant();
281 |         Eigen::Matrix3d R=U*Temp*V.transpose();
282 |         Eigen::Matrix3d Scale = V*Temp*S*V.transpose();
283 |         Eigen::Matrix3d logR = rotation_log_exp::log(R);
284 | 
285 |         dr_feature.push_back(Scale(0, 0));
286 |         dr_feature.push_back(Scale(0, 1));
287 |         dr_feature.push_back(Scale(0, 2));
288 |         dr_feature.push_back(Scale(1, 1));
289 |         dr_feature.push_back(Scale(1, 2));
290 |         dr_feature.push_back(Scale(2, 2));
291 |         dr_feature.push_back(logR(0, 1));
292 |         dr_feature.push_back(logR(0, 2));
293 |         dr_feature.push_back(logR(1, 2));
294 |     }
295 |     return dr_feature;
296 | }
297 | 
298 | py::array_t<double> get_dr(std::string ref_mesh_name, std::string defor_mesh_name){
299 | 	DR_feature feature;
300 | 	std::vector<double> temp = feature.get_feature(ref_mesh_name, defor_mesh_name);
301 | 
302 |     auto result = py::array_t<double>(temp.size());
303 |     auto result_buffer = result.request();
304 |     double *result_ptr = (double *)result_buffer.ptr;
305 | 
306 |     std::memcpy(result_ptr, temp.data(), temp.size()*sizeof(double));
307 | 
308 |     return result;
309 | 
310 | }
311 | 
312 |  PYBIND11_MODULE(get_dr, m){
313 |     m.doc() = "get dr";
314 |     m.def("get_dr", &get_dr, "get_dr"); 
315 |  }


--------------------------------------------------------------------------------
/test.py:
--------------------------------------------------------------------------------
1 | import sys
2 | sys.path.append('build')
3 | from get_dr import *
4 | import numpy as np
5 | import time
6 | print(get_dr('1_new.obj', '2_new.obj'))
7 | 


--------------------------------------------------------------------------------
/util_3drotation_log_exp.cpp:
--------------------------------------------------------------------------------
  1 | #include "util_3drotation_log_exp.h"
  2 | #include <iostream>
  3 | 
  4 | Eigen::Matrix3d rotation_log_exp::exp(double angle, const Eigen::Vector3d &axis)
  5 | {
  6 |     if(fabs(axis.norm()-1.0)>log_exp_tolerance)
  7 |     {
  8 |         std::cout<<"rotation_log_exp::exp1::input axis isn't an unit direction!!!"<<std::endl;
  9 |         return Eigen::Matrix3d::Identity();
 10 |     }
 11 |     Eigen::Matrix3d cross_axis,temp;
 12 |     temp.setZero();
 13 |     temp(2,1)=axis(0);
 14 |     temp(0,2)=axis(1);
 15 |     temp(1,0)=axis(2);
 16 |     cross_axis = temp - temp.transpose();
 17 |     return exp(angle,cross_axis);
 18 | }
 19 | 
 20 | Eigen::Matrix3d rotation_log_exp::exp(double angle, const Eigen::Matrix3d &cross_axis)
 21 | {
 22 |     Eigen::Matrix3d test=cross_axis+cross_axis.transpose();
 23 |     if(test.norm()>log_exp_tolerance)
 24 |     {
 25 |         std::cout<<"rotation_log_exp::exp2::input matrix isn't skew-matrix!!!"<<std::endl;
 26 |         return Eigen::Matrix3d::Identity();
 27 |     }
 28 | 
 29 |     return Eigen::Matrix3d::Identity()+sin(angle)*cross_axis+(1.0-cos(angle))*cross_axis*cross_axis;
 30 | }
 31 | 
 32 | Eigen::Matrix3d rotation_log_exp::exp(const Eigen::Matrix3d &angle_cross_axis)
 33 | {
 34 |     Eigen::Matrix3d test=angle_cross_axis+angle_cross_axis.transpose();
 35 |     if(test.norm()>log_exp_tolerance)
 36 |     {
 37 |         std::cout<<"rotation_log_exp::exp3::input matrix isn't skew-matrix!!!"<<std::endl;
 38 |         return Eigen::Matrix3d::Identity();
 39 |     }
 40 | 
 41 |     Eigen::Vector3d angle_axis;
 42 |     angle_axis(0)=angle_cross_axis(2,1);
 43 |     angle_axis(1)=angle_cross_axis(0,2);
 44 |     angle_axis(2)=angle_cross_axis(1,0);
 45 |     double angle=sqrt(angle_axis(0)*angle_axis(0)+angle_axis(1)*angle_axis(1)+angle_axis(2)*angle_axis(2));
 46 |     if(angle<log_exp_tolerance)
 47 |         return Eigen::Matrix3d::Identity();
 48 |     Eigen::Matrix3d cross_axis=angle_cross_axis/angle;
 49 | 
 50 |     return exp(angle,cross_axis);
 51 | }
 52 | 
 53 | Eigen::Matrix3d rotation_log_exp::exp(const Eigen::Vector3d &angle_axis)
 54 | {
 55 |     double angle = sqrt(angle_axis(0)*angle_axis(0)+angle_axis(1)*angle_axis(1)+angle_axis(2)*angle_axis(2));
 56 |     if(angle<log_exp_tolerance)
 57 |     {
 58 |         return Eigen::Matrix3d::Identity();
 59 |     }
 60 |     Eigen::Vector3d axis = angle_axis/angle;
 61 | 
 62 |     Eigen::Matrix3d cross_axis,temp;
 63 |     temp.setZero();
 64 |     temp(2,1)=axis(0);
 65 |     temp(0,2)=axis(1);
 66 |     temp(1,0)=axis(2);
 67 |     cross_axis = temp - temp.transpose();
 68 |     return exp(angle,cross_axis);
 69 | }
 70 | 
 71 | void rotation_log_exp::log(const Eigen::Matrix3d &rotation_matrix, double &angle, Eigen::Vector3d &axis)
 72 | {
 73 |     if((rotation_matrix.transpose()*rotation_matrix-Eigen::Matrix3d::Identity()).norm()>log_exp_tolerance)
 74 |     {
 75 |         std::cout<<"rotation_log_exp::log::rotation_matrix isn't an orthogonal matrix!!!"<<std::endl;
 76 |         return;
 77 |     }
 78 |     double csin = (rotation_matrix.trace()-1.0)/2.0;
 79 |     if(csin<-1.0||csin>1.0)
 80 |     {
 81 |         if(fabs(csin-1.0)>log_exp_tolerance&&fabs(csin+1.0)>log_exp_tolerance)
 82 |         {
 83 |             std::cout<<"rotation_log_exp::log::csin "<<csin<<" range is wrong!!!"<<std::endl;
 84 |             return;
 85 |         }
 86 |         else
 87 |             csin=std::max(std::min(1.0,csin),-1.0);
 88 |     }
 89 |     double tangle = acos(csin);
 90 |     if(fabs(tangle)<log_exp_tolerance)
 91 |     {
 92 |         angle = 0.0;
 93 |         axis=Eigen::Vector3d::Zero();
 94 |         return;
 95 |     }
 96 |     if(fabs(tangle-M_PI)<log_exp_tolerance)//axis has 2 possible, return 1
 97 |     {
 98 |         angle = M_PI;
 99 |         Eigen::Matrix3d B = (rotation_matrix+Eigen::Matrix3d::Identity())/2.0;
100 |         double k1 = sqrt(B(0,0));//here has 2 possible, + or -
101 |         double k2,k3;
102 |         if(k1*B(0,1)>0.0) k2 = sqrt(B(1,1));
103 |         else k2 = -sqrt(B(1,1));
104 |         if(k1*B(0,2)>0.0) k3 = sqrt(B(2,2));
105 |         else k3 = -sqrt(B(2,2));
106 |         axis(0)=k1;axis(1)=k2;axis(2)=k3;
107 |         return ;
108 |     }
109 | 
110 |     //tangle has 2 possible, 0~pi or pi~2pi
111 |     Eigen::Vector3d taxis;
112 |     taxis(0)=rotation_matrix(2,1)-rotation_matrix(1,2);
113 |     taxis(1)=rotation_matrix(0,2)-rotation_matrix(2,0);
114 |     taxis(2)=rotation_matrix(1,0)-rotation_matrix(0,1);
115 |     Eigen::Vector3d t_axis=taxis/(2.0*sin(tangle));
116 |     double sinv = sin(tangle);
117 |     double r01 = (1.0-csin)*t_axis(0)*t_axis(1)-t_axis(2)*sinv;
118 |     double r02 = (1.0-csin)*t_axis(0)*t_axis(2)+t_axis(1)*sinv;
119 |     double r10 = (1.0-csin)*t_axis(0)*t_axis(1)+t_axis(2)*sinv;
120 |     double r12 = (1.0-csin)*t_axis(1)*t_axis(2)-t_axis(0)*sinv;
121 |     double r20 = (1.0-csin)*t_axis(0)*t_axis(2)-t_axis(1)*sinv;
122 |     double r21 = (1.0-csin)*t_axis(1)*t_axis(2)+t_axis(0)*sinv;
123 |     double check = (rotation_matrix(0,1)-r01)*(rotation_matrix(0,1)-r01)+(rotation_matrix(0,2)-r02)*(rotation_matrix(0,2)-r02)
124 |             +(rotation_matrix(1,0)-r10)*(rotation_matrix(1,0)-r10)+(rotation_matrix(1,2)-r12)*(rotation_matrix(1,2)-r12)
125 |             +(rotation_matrix(2,0)-r20)*(rotation_matrix(2,0)-r20)+(rotation_matrix(2,1)-r21)*(rotation_matrix(2,1)-r21);
126 | 
127 |     if(check<log_exp_tolerance)
128 |     {
129 |         angle = tangle;
130 |         axis=t_axis;
131 |         return;
132 |     }
133 |     else
134 |     {
135 |         std::cout<<"rotation_log_exp::log angle is large than PI"<<std::endl;
136 |         tangle = 2*M_PI - tangle;
137 |         t_axis = taxis/(2.0*sin(tangle));
138 |         sinv = sin(tangle);
139 |         r01 = (1.0-csin)*t_axis(0)*t_axis(1)-t_axis(2)*sinv;
140 |         r02 = (1.0-csin)*t_axis(0)*t_axis(2)+t_axis(1)*sinv;
141 |         r10 = (1.0-csin)*t_axis(0)*t_axis(1)+t_axis(2)*sinv;
142 |         r12 = (1.0-csin)*t_axis(1)*t_axis(2)-t_axis(0)*sinv;
143 |         r20 = (1.0-csin)*t_axis(0)*t_axis(2)-t_axis(1)*sinv;
144 |         r21 = (1.0-csin)*t_axis(1)*t_axis(2)+t_axis(0)*sinv;
145 |         check = (rotation_matrix(0,1)-r01)*(rotation_matrix(0,1)-r01)+(rotation_matrix(0,2)-r02)*(rotation_matrix(0,2)-r02)
146 |                 +(rotation_matrix(1,0)-r10)*(rotation_matrix(1,0)-r10)+(rotation_matrix(1,2)-r12)*(rotation_matrix(1,2)-r12)
147 |                 +(rotation_matrix(2,0)-r20)*(rotation_matrix(2,0)-r20)+(rotation_matrix(2,1)-r21)*(rotation_matrix(2,1)-r21);
148 |         if(check>log_exp_tolerance)
149 |             std::cout<<"rotation_log_exp::log::computation is wrong!!!"<<std::endl;
150 |         angle = tangle;
151 |         axis=t_axis;
152 |         return;
153 |     }
154 | }
155 | 
156 | void rotation_log_exp::log(const Eigen::Matrix3d &rotation_matrix, double &angle, Eigen::Matrix3d &cross_axis)
157 | {
158 |     Eigen::Vector3d axis;
159 |     log(rotation_matrix,angle,axis);
160 | 
161 |     Eigen::Matrix3d temp;
162 |     temp.setZero();
163 |     temp(2,1)=axis(0);
164 |     temp(0,2)=axis(1);
165 |     temp(1,0)=axis(2);
166 |     cross_axis = temp - temp.transpose();
167 | }
168 | 
169 | Eigen::Matrix3d rotation_log_exp::log(const Eigen::Matrix3d &rotation_matrix)
170 | {
171 |     double angle;
172 |     Eigen::Matrix3d angle_cross_axis;
173 |     log(rotation_matrix,angle,angle_cross_axis);
174 |     return angle*angle_cross_axis;
175 | }
176 | 
177 | Eigen::Matrix3d rotation_log_exp::rotation_so3(const Eigen::Vector3d &angle_axis)
178 | {
179 |     Eigen::Matrix3d cross_axis,temp;
180 |     temp.setZero();
181 |     temp(2,1)=angle_axis(0);
182 |     temp(0,2)=angle_axis(1);
183 |     temp(1,0)=angle_axis(2);
184 |     cross_axis = temp - temp.transpose();
185 |     return cross_axis;
186 | }
187 | 
188 | Eigen::Matrix3d rotation_log_exp::rotation_so3(double angle, const Eigen::Vector3d &axis)
189 | {
190 |     if(fabs(axis.norm()-1.0)>log_exp_tolerance)
191 |     {
192 |         std::cout<<"rotation_log_exp::exp1::input axis isn't an unit direction!!!"<<std::endl;
193 |         return Eigen::Matrix3d::Identity();
194 |     }
195 | 
196 |     Eigen::Matrix3d cross_axis,temp;
197 |     temp.setZero();
198 |     temp(2,1)=axis(0);
199 |     temp(0,2)=axis(1);
200 |     temp(1,0)=axis(2);
201 |     cross_axis = temp - temp.transpose();
202 |     cross_axis *= angle;
203 |     return cross_axis;
204 | }
205 | 
206 | void rotation_log_exp::so3_to_angle_axis(const Eigen::Matrix3d &r, double &angle, Eigen::Vector3d &axis)
207 | {
208 |     axis(0)=r(2,1);
209 |     axis(1)=r(0,2);
210 |     axis(2)=r(1,0);
211 |     angle = sqrt(axis(0)*axis(0)+axis(1)*axis(1)+axis(2)*axis(2));
212 |     if(angle<log_exp_tolerance)
213 |         axis*=0.0;
214 |     else
215 |         axis/=angle;
216 | }
217 | 


--------------------------------------------------------------------------------
/util_3drotation_log_exp.h:
--------------------------------------------------------------------------------
 1 | #ifndef UTIL_3DROTATION_LOG_EXP
 2 | #define UTIL_3DROTATION_LOG_EXP
 3 | #include <Eigen/Dense>
 4 | 
 5 | 
 6 | //rotation matrix log and exp operation, reference Axis–angle representation on wiki
 7 | namespace rotation_log_exp
 8 | {
 9 | static double log_exp_tolerance = 1.0e-6;
10 | Eigen::Matrix3d exp(const Eigen::Vector3d &angle_axis);
11 | Eigen::Matrix3d exp(double angle,const Eigen::Vector3d &axis);
12 | Eigen::Matrix3d exp(double angle,const Eigen::Matrix3d &cross_axis);
13 | Eigen::Matrix3d exp(const Eigen::Matrix3d &angle_cross_axis);
14 | 
15 | void log(const Eigen::Matrix3d &rotation_matrix,double &angle,Eigen::Vector3d &axis);
16 | void log(const Eigen::Matrix3d &rotation_matrix,double &angle,Eigen::Matrix3d &cross_axis);
17 | Eigen::Matrix3d log(const Eigen::Matrix3d &rotation_matrix);
18 | 
19 | Eigen::Matrix3d rotation_so3(const Eigen::Vector3d &angle_axis);
20 | Eigen::Matrix3d rotation_so3(double angle,const Eigen::Vector3d &axis);
21 | 
22 | void so3_to_angle_axis(const Eigen::Matrix3d &r, double &angle, Eigen::Vector3d &axis);
23 | }
24 | 
25 | 
26 | #endif // ROTATION_H
27 | 


--------------------------------------------------------------------------------