Skip to content

Commit 8c2a47e

Browse files
cheindcheind
committed
Adding simple se3 trajectory interpolation.
Not yet working correctly.
1 parent 2daaa2d commit 8c2a47e

File tree

5 files changed

+241
-3
lines changed

5 files changed

+241
-3
lines changed

‎CMakeLists.txt‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,16 +12,18 @@ endif()
1212
set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
1313
set(DEFORM_LINK_TARGETS)
1414

15-
set(EIGEN_INCLUDE_DIR "../eigen" CACHE PATH "Where is the include directory of Eigen located")
16-
find_package(OpenMesh REQUIRED)
15+
find_package(Eigen3 REQUIRED)
16+
find_package(OpenMesh QUIET)
1717
find_package(OpenSceneGraph QUIET COMPONENTS osgViewer osgFX osgGA)
18+
find_package(Sophus QUIET)
1819

19-
include_directories("inc" ${OPENMESH_INCLUDE_DIR} ${EIGEN_INCLUDE_DIR})
20+
include_directories("inc" ${OPENMESH_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ${SOPHUS_INCLUDE_DIR})
2021
list(APPEND DEFORM_LINK_TARGETS ${OPENMESH_LIBRARIES})
2122

2223
add_library(deform
2324
inc/deform/arap.h
2425
inc/deform/openmesh_adapter.h
26+
inc/deform/trajectory.h
2527
src/unused.cpp
2628
)
2729
set_target_properties(deform PROPERTIES LINKER_LANGUAGE CXX)
@@ -30,6 +32,7 @@ add_executable(deform_tests
3032
tests/catch.hpp
3133
tests/accessor.h
3234
tests/test_cotan.cpp
35+
tests/test_trajectory.cpp
3336
)
3437
target_link_libraries(deform_tests deform ${DEFORM_LINK_TARGETS})
3538

‎cmake/FindEigen3.cmake‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
if(EIGEN3_INCLUDE_DIR)
2+
set(EIGEN3_FOUND TRUE)
3+
else()
4+
find_path(EIGEN3_INCLUDE_DIR NAMES
5+
Eigen/Core
6+
PATHS
7+
${CMAKE_INSTALL_PREFIX}/include
8+
${CMAKE_CURRENT_SOURCE_DIR}/../eigen
9+
PATH_SUFFIXES eigen3 eigen
10+
)
11+
include( FindPackageHandleStandardArgs)
12+
find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR)
13+
mark_as_advanced(EIGEN3_INCLUDE_DIR)
14+
endif()

‎cmake/FindSophus.cmake‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
if(SOPHUS_INCLUDE_DIR)
2+
set(SOPHUS_FOUND TRUE)
3+
else()
4+
find_path(SOPHUS_INCLUDE_DIR NAMES
5+
sophus/se3.hpp
6+
PATHS
7+
${CMAKE_INSTALL_PREFIX}/include
8+
${CMAKE_CURRENT_SOURCE_DIR}/../sophus
9+
PATH_SUFFIXES sophus
10+
)
11+
include( FindPackageHandleStandardArgs)
12+
find_package_handle_standard_args(Sophus DEFAULT_MSG SOPHUS_INCLUDE_DIR)
13+
mark_as_advanced(SOPHUS_INCLUDE_DIR)
14+
endif()

‎inc/deform/trajectory.h‎

Lines changed: 128 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,128 @@
1+
/**
2+
This file is part of mesh-deform.
3+
4+
Copyright(C) 2016 Christoph Heindl
5+
All rights reserved.
6+
7+
This software may be modified and distributed under the terms
8+
of the BSD license.See the LICENSE file for details.
9+
*/
10+
11+
#ifndef DEFORM_TRAJECTORY_H
12+
#define DEFORM_TRAJECTORY_H
13+
14+
#include <Eigen/Dense>
15+
#include <Eigen/Geometry>
16+
#include <Eigen/StdVector>
17+
18+
#ifdef _WIN32
19+
#pragma warning (push)
20+
#pragma warning (disable : 4244)
21+
#endif
22+
23+
#include <sophus/sophus.hpp>
24+
#include <sophus/se3.hpp>
25+
26+
#ifdef _WIN32
27+
#pragma warning (pop)
28+
#endif
29+
30+
namespace deform {
31+
32+
template<class PrecisionType = typename MeshType::Scalar>
33+
class TrajectorySE3 {
34+
public:
35+
36+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
37+
38+
/** Floating point precision used in calculations. */
39+
typedef PrecisionType Scalar;
40+
41+
/** Group type */
42+
typedef Sophus::SE3Group<Scalar> SE3Group;
43+
44+
/** Transformation matrix type. */
45+
typedef Eigen::Transform<Scalar, 3, Eigen::Affine> Transform;
46+
47+
/**
48+
Construct empty trajectory
49+
*/
50+
TrajectorySE3(Scalar timeStep)
51+
:_timeStep(timeStep)
52+
{
53+
_C <<
54+
6, 0, 0, 0,
55+
5, 3, -3, 1,
56+
1, 3, 3, -2,
57+
0, 0, 0, 1;
58+
_C *= Scalar(1) / 6;
59+
}
60+
61+
void addKeyframe(const Transform &transform) {
62+
_keyframes.push_back(transform);
63+
const size_t n = _keyframes.size();
64+
65+
if (n > 1) {
66+
Transform delta = _keyframes[n - 2].inverse(Eigen::Isometry) * _keyframes[n - 1];
67+
68+
SE3Group g(delta.matrix());
69+
_omegas.push_back(g.log());
70+
71+
if (n == 2) {
72+
Transform deltainv = delta.inverse(Eigen::Isometry);
73+
SE3Group g(deltainv.matrix());
74+
_omegas[0] = g.log();
75+
}
76+
} else {
77+
_omegas.push_back(SE3Group::Tangent::Zero()); // Will be fixed when second keyframe is given.
78+
}
79+
}
80+
81+
Transform operator()(Scalar time) const {
82+
83+
const Scalar t0(0);
84+
const Scalar s = (time - t0) / _timeStep;
85+
86+
const int i = static_cast<int>(std::floor(s));
87+
eigen_assert(i >= 0);
88+
89+
const Scalar u = s - Scalar(i);
90+
const Eigen::Matrix<Scalar, 4, 1> b = _C * Eigen::Matrix<Scalar, 4, 1>(Scalar(1), u, u*u, u*u*u);
91+
92+
Transform trans = previousTransform(i);
93+
for (int j = 1; j < 4; ++j) {
94+
SE3Group::Tangent o = omega(i + j);
95+
std::cout << "omega is " << o.transpose() << std::endl;
96+
std::cout << "b is" << b(j) << std::endl;
97+
std::cout << "omega * b is " << (b(j) * o).transpose() << std::endl;
98+
99+
trans = trans * SE3Group::exp(b(j) * o).affine3();
100+
}
101+
102+
return trans;
103+
}
104+
105+
private:
106+
107+
typename SE3Group::Tangent omega(int idx) const {
108+
if ((size_t)idx < _omegas.size())
109+
return _omegas[idx];
110+
else
111+
return _omegas.back();
112+
}
113+
114+
Transform previousTransform(int idx) const {
115+
if (idx == 0)
116+
return SE3Group::exp(_omegas[0]).affine3() * _keyframes[0];
117+
else
118+
return _keyframes[idx - 1];
119+
}
120+
121+
Scalar _timeStep;
122+
std::vector<Transform, Eigen::aligned_allocator<Transform> > _keyframes;
123+
std::vector<typename SE3Group::Tangent, Eigen::aligned_allocator<typename SE3Group::Tangent> > _omegas;
124+
Eigen::Matrix<Scalar, 4, 4> _C;
125+
};
126+
}
127+
128+
#endif

‎tests/test_trajectory.cpp‎

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
/**
2+
This file is part of mesh-deform.
3+
4+
Copyright(C) 2016 Christoph Heindl
5+
All rights reserved.
6+
7+
This software may be modified and distributed under the terms
8+
of the BSD license.See the LICENSE file for details.
9+
*/
10+
11+
#include "catch.hpp"
12+
13+
#include <deform/trajectory.h>
14+
#include <iostream>
15+
16+
17+
TEST_CASE("trajectory")
18+
{
19+
typedef deform::TrajectorySE3<float> Trajectory;
20+
21+
22+
Trajectory path(1.f);
23+
24+
// time 0
25+
Trajectory::Transform trans = Trajectory::Transform::Identity();
26+
path.addKeyframe(trans);
27+
28+
// time 1
29+
trans *= Eigen::Translation3f(0, 0, 1);
30+
path.addKeyframe(trans);
31+
32+
// time 2
33+
trans *= Eigen::Translation3f(0, 0, 1);
34+
path.addKeyframe(trans);
35+
36+
// time 3
37+
trans *= Eigen::Translation3f(0, 0, 1);
38+
path.addKeyframe(trans);
39+
40+
// time 4
41+
trans = trans * Eigen::Translation3f(0, 0, 1) * Eigen::AngleAxisf(M_PI / 4, Eigen::Vector3f::UnitX());
42+
std::cout << "Trans" << trans.matrix() << std::endl;
43+
path.addKeyframe(trans);
44+
45+
46+
// time 5
47+
trans *= Eigen::Translation3f(0, 0, 1);
48+
path.addKeyframe(trans);
49+
50+
// time 6
51+
trans *= Eigen::Translation3f(0, 0, 1);
52+
path.addKeyframe(trans);
53+
54+
// time 7
55+
trans *= Eigen::Translation3f(0, 0, 1);
56+
path.addKeyframe(trans);
57+
58+
// time 8
59+
trans *= Eigen::Translation3f(0, 0, 1);
60+
path.addKeyframe(trans);
61+
62+
/*
63+
for (float time = 0.f; time <= 6; time += 0.5f) {
64+
std::cout << time << std::endl;
65+
std::cout << path(time).matrix() << std::endl;
66+
std::cout << "---------------------" << std::endl;
67+
}
68+
*/
69+
70+
//std::cout << path(3.5f).matrix() << std::endl;
71+
72+
std::cout << "Transform is" << std::endl;
73+
std::cout << path(4.f).matrix() << std::endl;
74+
//std::cout << path(4.5f).matrix() << std::endl;
75+
//std::cout << path(5.0f).matrix() << std::endl;
76+
77+
//REQUIRE(sp.isApprox(expected, 1e-4f));
78+
}
79+

0 commit comments

Comments
 (0)