Skip to content

Commit 61a1aa4

Browse files
stytimstytim
committed
initial commit
0 parents  commit 61a1aa4

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

96 files changed

+32219
-0
lines changed

‎.gitignore

+16
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
##### VisualStudioCode
2+
**/.vscode/
3+
4+
build/*
5+
6+
# C++ objects and libs
7+
*.slo
8+
*.lo
9+
*.o
10+
*.a
11+
*.la
12+
*.lai
13+
*.so
14+
*.so.*
15+
*.dll
16+
*.dylib

‎CMakeLists.txt

+43
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
cmake_minimum_required(VERSION 3.15)
2+
project(calib_k4a LANGUAGES CXX)
3+
4+
set(CMAKE_CXX_STANDARD 17)
5+
set(CMAKE_CXX_STANDARD_REQUIRED ON)
6+
set(CMAKE_CXX_EXTENSIONS OFF)
7+
8+
find_package(k4a REQUIRED)
9+
find_package(k4arecord REQUIRED)
10+
find_package(Open3D REQUIRED)
11+
find_package(OpenCV REQUIRED)
12+
find_package(apriltag REQUIRED)
13+
14+
################################################################################
15+
# Source
16+
17+
set(INCLUDE_FILES
18+
include/camera_extrinsics.hpp
19+
include/calib_k4a.hpp
20+
)
21+
22+
set(SOURCE_FILES
23+
${INCLUDE_FILES}
24+
src/camera_extrinsics.cpp
25+
src/calib_k4a.cpp
26+
)
27+
28+
message(STATUS "Open3D : ${Open3D_LIBRARIES}")
29+
include_directories(${OpenCV_INCLUDE_DIRS})
30+
31+
32+
################################################################################
33+
# Targets
34+
add_executable(calib_k4a ${SOURCE_FILES})
35+
target_link_libraries(calib_k4a PUBLIC
36+
k4a # Kinect SDK
37+
k4arecord
38+
opencv_core opencv_imgproc opencv_calib3d opencv_aruco
39+
${Open3D_LIBRARIES} # Implicitly includes Eigen
40+
apriltag::apriltag
41+
stdc++fs # filesystem
42+
)
43+
target_include_directories(calib_k4a PUBLIC include)

‎LICENSE

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
BSD 3-Clause License
2+
3+
Copyright (c) 2021, Tianyu Song
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+
1. Redistributions of source code must retain the above copyright notice, this
10+
list of conditions and the following disclaimer.
11+
12+
2. 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+
3. 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.

‎README.md

+34
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
# Extrinsic Calibration for Multiple Azure Kinect Cameras
2+
3+
This calibration tool only requires 1 frame of recording mkv files from each Azure Kinect camera. It first uses color frame for [AprilTag](https://github.com/AprilRobotics/apriltag) marker or ChArUco board detection and initial camera pose estimation. Then depth image from each camera is transformed to color camera to generate colored point cloud, which is used for [Colored ICP algorithm](https://openaccess.thecvf.com/content_ICCV_2017/papers/Park_Colored_Point_Cloud_ICCV_2017_paper.pdf).
4+
5+
![Calibration](/img/image.jpg?raw=true "Left: Marker detection without ICP. Right: After ICP.")
6+
7+
8+
The final result is saved as JSON file to be used in other software and PLY file for verifying calibration conveniently in any point cloud viewing software such as MeshLab.
9+
10+
## Prerequisites
11+
12+
* Azure Kinect SDK
13+
* OpenCV
14+
* AprilTag
15+
* Open3D
16+
17+
## Building
18+
Tested on Ubuntu 18.04 and 20.04
19+
```
20+
mkdir build && cd build
21+
cmake ..
22+
make
23+
```
24+
25+
## Running
26+
```
27+
./calib_k4a <master.mkv> <sub1.mkv>...
28+
```
29+
30+
## Important Note
31+
The final extrinsic is transformed into depth camera coordinate system and also into OpenGL convention.
32+
33+
## Todo
34+
* Test with [Teaser++](https://github.com/MIT-SPARK/TEASER-plusplus) for initial pose estimation

‎img/image.jpg

1.57 MB
Loading

‎include/calib_k4a.hpp

+43
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
#pragma once
2+
#include "camera_extrinsics.hpp"
3+
4+
static void CopyIntrinsics(
5+
const k4a_calibration_camera_t& from,
6+
CameraIntrinsics& to)
7+
{
8+
to.Width = from.resolution_width;
9+
to.Height = from.resolution_height;
10+
11+
const k4a_calibration_intrinsic_parameters_t& params = from.intrinsics.parameters;
12+
to.cx = params.param.cx;
13+
to.cy = params.param.cy;
14+
to.fx = params.param.fx;
15+
to.fy = params.param.fy;
16+
to.k[0] = params.param.k1;
17+
to.k[1] = params.param.k2;
18+
to.k[2] = params.param.k3;
19+
to.k[3] = params.param.k4;
20+
to.k[4] = params.param.k5;
21+
to.k[5] = params.param.k6;
22+
to.codx = params.param.codx;
23+
to.cody = params.param.cody;
24+
to.p1 = params.param.p1;
25+
to.p2 = params.param.p2;
26+
}
27+
28+
void CalibrationFromK4a(
29+
const k4a_calibration_t& from,
30+
CameraCalibration& to)
31+
{
32+
33+
CopyIntrinsics(from.depth_camera_calibration, to.Depth);
34+
CopyIntrinsics(from.color_camera_calibration, to.Color);
35+
// Extrinsics from depth to color camera
36+
const k4a_calibration_extrinsics_t* extrinsics = &from.extrinsics[K4A_CALIBRATION_TYPE_DEPTH][K4A_CALIBRATION_TYPE_COLOR];
37+
for (int i = 0; i < 9; ++i) {
38+
to.RotationFromDepth[i] = extrinsics->rotation[i];
39+
}
40+
for (int i = 0; i < 3; ++i) {
41+
to.TranslationFromDepth[i] = extrinsics->translation[i];
42+
}
43+
}

‎include/camera_extrinsics.hpp

+168
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,168 @@
1+
#pragma once
2+
3+
#include <cstdint>
4+
#include <vector>
5+
6+
#include <Eigen/Eigen>
7+
8+
#include <apriltag.h>
9+
#include <tagStandard41h12.h>
10+
#include <apriltag_pose.h>
11+
#include <opencv2/aruco/charuco.hpp>
12+
13+
#include <cereal/archives/json.hpp>
14+
#include <open3d/geometry/PointCloud.h>
15+
16+
namespace cereal {
17+
template<class Archive>
18+
void serialize(Archive & archive,
19+
Eigen::Quaternionf& m)
20+
{
21+
archive( cereal::make_nvp("x", m.x()), cereal::make_nvp("y", m.y()),
22+
cereal::make_nvp("z", m.z()), cereal::make_nvp("w", m.w()) );
23+
};
24+
25+
template<class Archive>
26+
void serialize(Archive & archive,
27+
Eigen::Vector3f& t)
28+
{
29+
archive( cereal::make_nvp("m00", t.x()), cereal::make_nvp("m10", t.y()),
30+
cereal::make_nvp("m20", t.z()) );
31+
};
32+
}
33+
34+
struct CameraExtrinsics
35+
{
36+
Eigen::Vector3f translation;
37+
Eigen::Quaternionf rotation;
38+
39+
template <class Archive>
40+
void serialize( Archive & ar )
41+
{
42+
ar(cereal::make_nvp("translation", translation), cereal::make_nvp("rotation", rotation));
43+
}
44+
45+
};
46+
47+
struct CameraIntrinsics
48+
{
49+
// Sensor resolution
50+
int32_t Width, Height;
51+
52+
// Intrinsics
53+
float cx, cy;
54+
float fx, fy;
55+
float k[6];
56+
float codx, cody;
57+
float p1, p2;
58+
};
59+
60+
struct CameraCalibration
61+
{
62+
// Intrinsics for each camera
63+
CameraIntrinsics Color, Depth;
64+
65+
// Extrinsics transform from 3D depth camera point to 3D point relative to color camera
66+
float RotationFromDepth[3*3];
67+
float TranslationFromDepth[3];
68+
};
69+
70+
//------------------------------------------------------------------------------
71+
// Registration
72+
73+
struct FrameInfo
74+
{
75+
int16_t* PointCloudData;
76+
77+
CameraCalibration Calibration{};
78+
79+
// Accelerometer reading for extrinsics calibration
80+
float Accelerometer[3];
81+
82+
// Color image
83+
std::vector<uint8_t> ColorImage;
84+
int ColorWidth = 0, ColorHeight = 0, ColorStride = 0;
85+
86+
// Depth image
87+
std::vector<uint16_t> DepthImage;
88+
int DepthWidth = 0, DepthHeight = 0, DepthStride = 0;
89+
90+
uint32_t CameraIndex;
91+
int FrameNumber;
92+
char *filename;
93+
};
94+
95+
96+
struct AlignmentTransform
97+
{
98+
float Transform[16];
99+
bool Identity = true;
100+
inline void operator=(const Eigen::Matrix4f& src)
101+
{
102+
Identity = src.isIdentity();
103+
for (int row = 0; row < 4; ++row) {
104+
for (int col = 0; col < 4; ++col) {
105+
Transform[row * 4 + col] = src(row, col);
106+
}
107+
}
108+
}
109+
inline void Set(Eigen::Matrix4f& dest) const
110+
{
111+
if (Identity) {
112+
dest = Eigen::Matrix4f::Identity();
113+
} else {
114+
for (int row = 0; row < 4; ++row) {
115+
for (int col = 0; col < 4; ++col) {
116+
dest(row, col) = Transform[row * 4 + col];
117+
}
118+
}
119+
}
120+
}
121+
};
122+
123+
class ExtrinsicsCalibration
124+
{
125+
public:
126+
127+
apriltag_family_t *tf;
128+
apriltag_detector_t* td;
129+
cv::Ptr<cv::aruco::Dictionary> dictionary;
130+
cv::Ptr<cv::aruco::CharucoBoard> board;
131+
cv::Ptr<cv::aruco::DetectorParameters> params;
132+
133+
ExtrinsicsCalibration()
134+
{
135+
tf = tagStandard41h12_create();
136+
td = apriltag_detector_create();
137+
apriltag_detector_add_family_bits(td, tf, 1);
138+
td->quad_decimate = 1.f;
139+
td->quad_sigma = 0.8f;
140+
td->nthreads = 8;
141+
td->refine_edges = 1;
142+
td->decode_sharpening = 0.25;
143+
144+
dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
145+
board = cv::aruco::CharucoBoard::create(5, 7, 0.04f, 0.02f, dictionary);
146+
params = cv::aruco::DetectorParameters::create();
147+
}
148+
149+
~ExtrinsicsCalibration()
150+
{
151+
tagStandard41h12_destroy(tf);
152+
apriltag_detector_destroy(td);
153+
}
154+
155+
bool CalculateExtrinsics(
156+
const std::vector<FrameInfo>& frames,
157+
std::vector<AlignmentTransform>& extrinsics);
158+
159+
bool RefineExtrinsics(
160+
const std::vector<FrameInfo>& frames,
161+
std::vector<AlignmentTransform>& extrinsics);
162+
private:
163+
std::vector<std::shared_ptr<open3d::geometry::PointCloud>> full_cloud;
164+
bool GenerateFullCloudFromFrames(FrameInfo frame);
165+
bool DownSample(std::shared_ptr<open3d::geometry::PointCloud>& cloud, const double voxel_size, const int idx);
166+
167+
168+
};

0 commit comments

Comments
 (0)