Skip to content

Commit 9b16cda

Browse files
feixhfeixh
committed
improve geometry routines and installation script
1 parent 6203674 commit 9b16cda

7 files changed

Lines changed: 81 additions & 29 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ message(STATUS "Found Eigen at ${EIGEN3_INCLUDE_DIR}")
3636

3737
link_directories(
3838
${PROJECT_SOURCE_DIR}/lib
39-
${PROJECT_SOURCE_DIR}/jsoncpp/lib
39+
${PROJECT_SOURCE_DIR}/thirdparty/jsoncpp/lib
4040
/usr/local/lib
4141
/usr/lib
4242
/usr/x86_64-linux-gnu
@@ -76,7 +76,7 @@ add_executable(undistort_images example/undistort_images.cpp)
7676
target_link_libraries(undistort_images visma core)
7777

7878

79-
set(build_evaluation True)
79+
set(build_evaluation False)
8080
if (${build_evaluation})
8181
include_directories(
8282
${PROJECT_SOURCE_DIR}/thirdparty/Open3D/include/Open3D)

‎core/rodrigues.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
#include "math.h"
44

55
namespace feh {
6+
67
template <typename T>
78
Eigen::Matrix<T, 3, 3> hat(const Eigen::Matrix<T, 3, 1> &u) {
89
return (Eigen::Matrix<T, 3, 3>{}
@@ -190,3 +191,4 @@ Eigen::Matrix<T, 3, 3> projectSO3(const Eigen::Matrix<T, 3, 3> &R) {
190191
}
191192

192193
}
194+

‎core/se3.h‎

Lines changed: 49 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -7,18 +7,18 @@
77

88
namespace feh {
99

10-
template<typename T>
10+
template<typename Type>
1111
class SO3Type {
1212
public:
13-
using BaseType = Eigen::Matrix<T, 3, 3>;
14-
using PointType = Eigen::Matrix<T, 3, 1>;
13+
using BaseType = Eigen::Matrix<Type, 3, 3>;
14+
using PointType = Eigen::Matrix<Type, 3, 1>;
1515
using AxisType = PointType;
1616

1717
SO3Type() : R_{BaseType::Identity()} {}
1818

19-
SO3Type(const BaseType &R) : R_{R} {}
19+
SO3Type(const Eigen::Matrix<Type, 3, 3> &R):R_{R} {}
2020

21-
SO3Type(const AxisType &axis, T angle) :
21+
SO3Type(const AxisType &axis, Type angle) :
2222
R_{rodrigues(AxisType{axis / axis.norm() * angle})} {}
2323

2424
SO3Type operator*(const SO3Type &other) {
@@ -54,6 +54,19 @@ class SO3Type {
5454
return BaseType{svd.matrixU() * BaseType::Identity() * svd.matrixV().transpose()};
5555
}
5656

57+
template <typename TT>
58+
SO3Type<TT> cast() const {
59+
return R_.template cast<TT>();
60+
}
61+
62+
// factory methods
63+
template <typename Derived>
64+
static SO3Type from_matrix(const Eigen::MatrixBase<Derived> &other) {
65+
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3);
66+
Eigen::Matrix<Type, 3, 3> R = other.template cast<Type>();
67+
return R;
68+
}
69+
5770
private:
5871
BaseType R_;
5972
};
@@ -65,6 +78,12 @@ class SE3Type {
6578

6679
SE3Type() : R_{}, T_{0, 0, 0} {}
6780

81+
/*
82+
SE3Type(const Eigen::Matrix<Type, 3, 4> &RT):
83+
R_{RT.template block<3, 3>(0, 0)},
84+
T_{RT.template block<3, 1>(0, 3)} {}
85+
*/
86+
6887
SE3Type(const Eigen::Matrix<Type, 4, 4> &RT):
6988
R_{RT.template block<3, 3>(0, 0)},
7089
T_{RT.template block<3, 1>(0, 3)} {}
@@ -111,6 +130,31 @@ class SE3Type {
111130
return out;
112131
};
113132

133+
template <typename TT>
134+
SE3Type<TT> cast() {
135+
return SE3Type<TT>(R_.cast<TT>(), T_.cast<TT>());
136+
}
137+
138+
// factory methods
139+
template <typename Derived>
140+
static SE3Type from_matrix3x4(const Eigen::MatrixBase<Derived> &other) {
141+
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 4);
142+
Eigen::Matrix<Type, 3, 3> R = other.template block<3, 3>(0, 0).template cast<Type>();
143+
Eigen::Matrix<Type, 3, 1> T = other.template block<3, 1>(0, 3).template cast<Type>();
144+
return {R, T};
145+
}
146+
147+
template <typename Derived1, typename Derived2>
148+
static SE3Type from_RT(const Eigen::MatrixBase<Derived1> &Rin,
149+
const Eigen::MatrixBase<Derived2> &Tin) {
150+
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived1, 3, 3);
151+
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived2, 3, 1);
152+
153+
Eigen::Matrix<Type, 3, 3> R = Rin.template cast<Type>();
154+
Eigen::Matrix<Type, 3, 1> T = Tin.template cast<Type>();
155+
return {R, T};
156+
}
157+
114158
private:
115159
SO3Type<Type> R_;
116160
Eigen::Matrix<Type, 3, 1> T_;

‎example/example_dump.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
#include <memory>
22
#include <fstream>
3+
#include <iostream>
34

45
// unix
56
#include "sys/stat.h"

‎example/example_load.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,6 @@
1+
#include <iostream>
12
#include <memory>
3+
24
#include "dataloader.h"
35
#include "utils.h"
46

‎install.sh‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
#!/bin/bash
2+
PROJECT_DIR=$(pwd)
3+
echo $PROJECT_DIR
4+
5+
cd $PROJECT_DIR/thirdparty/jsoncpp
6+
mkdir build
7+
cd build
8+
cmake .. -DCMAKE_INSTALL_PREFIX=.. -DBUILD_SHARED_LIBS=ON
9+
make -j
10+
make install
11+
12+
cd $PROJECT_DIR
13+
mkdir -p build
14+
cd build
15+
cmake ..
16+
make -j
17+
cd $PROJECT_DIR

‎src/dataloader.cpp‎

Lines changed: 8 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -1,31 +1,14 @@
1-
#include "dataloader.h"
1+
#include <iostream>
22

33
// 3rd party
44
#include "json/json.h"
55
#include "opencv2/imgproc/imgproc.hpp"
66

77
// own
88
#include "utils.h"
9+
#include "dataloader.h"
910

1011
namespace feh {
11-
////////////////////////////////////////////////////////////////////////////////
12-
////////////////////////////////////////////////////////////////////////////////
13-
// HELPERS
14-
////////////////////////////////////////////////////////////////////////////////
15-
////////////////////////////////////////////////////////////////////////////////
16-
Mat4f SE3FromArray(float *data) {
17-
Mat4f out;
18-
out.block<3, 4>(0, 0) = Eigen::Map<Eigen::Matrix<float, 3, 4, Eigen::RowMajor>>(data);
19-
out(3, 3) = 1.0f;
20-
return out;
21-
}
22-
23-
Mat4f SE3FromArray(double *data) {
24-
Mat4d out;
25-
out.block<3, 4>(0, 0) = Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>>(data);
26-
out(3, 3) = 1.0f;
27-
return out.cast<float>();
28-
}
2912

3013
bool LoadEdgeMap(const std::string &filename, cv::Mat &edge) {
3114
vlslam_pb::EdgeMap edgemap;
@@ -117,7 +100,9 @@ bool VlslamDatasetLoader::Grab(int i,
117100
std::cout << i << "\n";
118101

119102
vlslam_pb::Packet *packet_ptr(dataset_.mutable_packets(i));
120-
gwc = SE3f(SE3FromArray(packet_ptr->mutable_gwc()->mutable_data()));
103+
gwc = SE3::from_matrix3x4(
104+
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>>(
105+
packet_ptr->mutable_gwc()->mutable_data()));
121106

122107
// gravity alignment rotation
123108
Vec3f Wg(packet_ptr->wg(0), packet_ptr->wg(1), 0);
@@ -182,8 +167,9 @@ std::unordered_map<int64_t, std::array<ftype, 3>> VlslamDatasetLoader::GrabSpars
182167
std::unordered_map<int64_t, std::array<ftype, 3>> out;
183168
vlslam_pb::Packet *packet_ptr = dataset_.mutable_packets(i);
184169

185-
186-
auto gwc = SE3f(SE3FromArray(packet_ptr->mutable_gwc()->mutable_data()));
170+
auto gwc = SE3::from_matrix3x4(
171+
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>>(
172+
packet_ptr->mutable_gwc()->mutable_data()));
187173
auto gcw = gwc.inv();
188174

189175

0 commit comments

Comments
 (0)