Skip to content

Commit 90d7857

Browse files
chutsuchutsu
committed
Update README
1 parent e7ccd56 commit 90d7857

Some content is hidden

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

44 files changed

+738
-148
lines changed

‎.gitignore

100644100755
File mode changed.

‎LICENSE

100644100755
File mode changed.

‎README.md

100644100755
+96
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,102 @@ cameras. Specifically:
1616
# Note: If you don't build in release mode the optimization will be slow.
1717

1818

19+
## Calibration Target
20+
21+
`yac` uses a grid of AprilTags, a.k.a an AprilGrid, as a calibration target.
22+
23+
![AprilGrid](docs/aprilgrid.png)
24+
25+
Click [here](docs/aprilgrid_A0.pdf)) to download and print the AprilGrid.
26+
During calibration make sure the calibration target is as flat as possible.
27+
28+
## Calibrate Monocular Camera Intrinsics
29+
30+
Example `calib_camera.yaml` configuration file:
31+
32+
ros:
33+
bag: "/data/intel_d435i/imucam-1.bag"
34+
cam0_topic: "/stereo/camera0/image"
35+
36+
calib_target:
37+
target_type: 'aprilgrid' # Target type
38+
tag_rows: 6 # Number of rows
39+
tag_cols: 6 # Number of cols
40+
tag_size: 0.088 # Size of apriltag, edge to edge [m]
41+
tag_spacing: 0.3 # Ratio of space between tags to tagSize
42+
# Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]
43+
44+
cam0:
45+
proj_model: "pinhole"
46+
dist_model: "radtan4"
47+
lens_hfov: 69.4
48+
lens_vfov: 42.5
49+
resolution: [640.0, 480.0]
50+
rate: 30.0
51+
52+
53+
## Calibrate Stereo Camera Intrinsics and Extrinsics
54+
55+
Example `calib_stereo.yaml` configuration file:
56+
57+
ros:
58+
bag: "/data/intel_d435i/imucam-1.bag"
59+
cam0_topic: "/stereo/camera0/image"
60+
cam1_topic: "/stereo/camera1/image"
61+
62+
calib_target:
63+
target_type: 'aprilgrid' # Target type
64+
tag_rows: 6 # Number of rows
65+
tag_cols: 6 # Number of cols
66+
tag_size: 0.088 # Size of apriltag, edge to edge [m]
67+
tag_spacing: 0.3 # Ratio of space between tags to tagSize
68+
# Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]
69+
70+
cam0:
71+
proj_model: "pinhole"
72+
dist_model: "radtan4"
73+
lens_hfov: 69.4
74+
lens_vfov: 42.5
75+
resolution: [640.0, 480.0]
76+
rate: 30.0
77+
78+
cam1:
79+
proj_model: "pinhole"
80+
dist_model: "radtan4"
81+
lens_hfov: 69.4
82+
lens_vfov: 42.5
83+
resolution: [640.0, 480.0]
84+
rate: 30.0
85+
86+
87+
## Calibrate Mocap Marker to Camera Extrinsics
88+
89+
Example `calib_mocap_camera.yaml` configuration file:
90+
91+
ros:
92+
train_bag: "/data/aabm/200115-vicon-train.bag"
93+
test_bag: "/data/aabm/200115-vicon-test.bag"
94+
cam0_topic: "/cam0/image"
95+
body0_topic: "/body0/pose"
96+
target0_topic: "/target0/pose"
97+
98+
calib_target:
99+
target_type: 'aprilgrid' # Target type
100+
tag_rows: 6 # Number of rows
101+
tag_cols: 6 # Number of cols
102+
tag_size: 0.088 # Size of apriltag, edge to edge [m]
103+
tag_spacing: 0.3 # Ratio of space between tags to tagSize
104+
# Example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]
105+
106+
cam0:
107+
resolution: [640.0, 480.0]
108+
proj_model: "pinhole"
109+
dist_model: "radtan4"
110+
lens_hfov: 69.4
111+
lens_vfov: 42.5
112+
rate: 30.0
113+
114+
19115
## LICENCE
20116

21117
MIT

‎docs/aprilgrid.png

229 KB
Loading

‎docs/aprilgrid_A0.pdf

36.1 KB
Binary file not shown.

‎scripts/run.sh

+2-2
Original file line numberDiff line numberDiff line change
@@ -11,8 +11,8 @@ catkin build -DCMAKE_BUILD_TYPE=Release yac yac_ros
1111

1212
source ~/catkin_ws/devel/setup.bash
1313
# roslaunch yac_ros calib_mono.launch
14-
roslaunch yac_ros calib_stereo.launch
15-
# roslaunch yac_ros calib_vicon.launch
14+
# roslaunch yac_ros calib_stereo.launch
15+
roslaunch yac_ros calib_mocap.launch
1616

1717
# Build tests
1818
# cd tests

‎yac/CMakeLists.txt

+1-1
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ ADD_LIBRARY(
2525
lib/calib_data.cpp
2626
lib/calib_mono.cpp
2727
lib/calib_stereo.cpp
28-
lib/calib_vicon_marker.cpp
28+
lib/calib_mocap_marker.cpp
2929
)
3030

3131
# TESTS

‎yac/lib/aprilgrid.cpp

100644100755
File mode changed.

‎yac/lib/aprilgrid.hpp

100644100755
File mode changed.

‎yac/lib/calib_data.cpp

100644100755
File mode changed.

‎yac/lib/calib_data.hpp

100644100755
+1-2
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ namespace yac {
1212

1313
/**
1414
* Pose parameter block
15-
**/
15+
*/
1616
struct calib_pose_t {
1717
double q[4] = {0.0, 0.0, 0.0, 1.0}; // x, y, z, w
1818
double r[3] = {0.0, 0.0, 0.0}; // x, y, z
@@ -123,7 +123,6 @@ struct calib_params_t {
123123
}
124124
};
125125

126-
127126
/**
128127
* Calibration target.
129128
*/

‎yac/lib/calib_vicon_marker.cpp ‎yac/lib/calib_mocap_marker.cpp

+49-9
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
#include "calib_vicon_marker.hpp"
1+
#include "calib_mocap_marker.hpp"
22

33
namespace yac {
44

@@ -32,11 +32,11 @@ static int process_aprilgrid(const aprilgrid_t &aprilgrid,
3232
for (size_t i = 0; i < 4; i++) {
3333
const auto kp = keypoints[i];
3434
const auto obj_pt = object_points[i];
35-
const auto residual = new vicon_marker_residual_t{proj_model, dist_model,
35+
const auto residual = new mocap_marker_residual_t{proj_model, dist_model,
3636
kp, obj_pt};
3737

3838
const auto cost_func =
39-
new ceres::AutoDiffCostFunction<vicon_marker_residual_t,
39+
new ceres::AutoDiffCostFunction<mocap_marker_residual_t,
4040
2, // Size of: residual
4141
4, // Size of: intrinsics
4242
4, // Size of: distortion
@@ -64,7 +64,7 @@ static int process_aprilgrid(const aprilgrid_t &aprilgrid,
6464
return 0;
6565
}
6666

67-
int calib_vicon_marker_solve(const aprilgrids_t &aprilgrids,
67+
int calib_mocap_marker_solve(const aprilgrids_t &aprilgrids,
6868
calib_params_t &cam,
6969
mat4s_t &T_WM,
7070
mat4_t &T_MC,
@@ -104,14 +104,14 @@ int calib_vicon_marker_solve(const aprilgrids_t &aprilgrids,
104104
problem->SetParameterization(T_WM_params[i].q,
105105
&quaternion_parameterization);
106106

107-
// Fixing the marker pose - assume vicon is calibrated and accurate
107+
// Fixing the marker pose - assume mocap is calibrated and accurate
108108
problem->SetParameterBlockConstant(T_WM_params[i].q);
109109
problem->SetParameterBlockConstant(T_WM_params[i].r);
110110
}
111111

112-
// Fix camera parameters
113-
problem->SetParameterBlockConstant(cam.proj_params.data());
114-
problem->SetParameterBlockConstant(cam.dist_params.data());
112+
// // Fix camera parameters
113+
// problem->SetParameterBlockConstant(cam.proj_params.data());
114+
// problem->SetParameterBlockConstant(cam.dist_params.data());
115115

116116
// Fix T_WF parameters
117117
// problem->SetParameterBlockConstant(T_WF_param.q);
@@ -134,6 +134,46 @@ int calib_vicon_marker_solve(const aprilgrids_t &aprilgrids,
134134
ceres::Solve(options, problem.get(), &summary);
135135
std::cout << summary.FullReport() << std::endl;
136136

137+
// Estimate covariance matrix
138+
std::vector<std::pair<const double*, const double*>> covar_blocks;
139+
covar_blocks.push_back({T_MC_param.q, T_MC_param.q});
140+
covar_blocks.push_back({T_MC_param.q, T_MC_param.r});
141+
covar_blocks.push_back({T_MC_param.r, T_WF_param.r});
142+
143+
ceres::Covariance::Options covar_options;
144+
covar_options.algorithm_type = ceres::SPARSE_QR;
145+
covar_options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
146+
ceres::Covariance covar(covar_options);
147+
const auto retval = covar.Compute(covar_blocks, problem.get());
148+
if (retval == false) {
149+
printf("Estimate covariance failed!\n");
150+
}
151+
152+
double q_MC_q_MC_covar[3 * 3] = {0};
153+
double q_MC_r_MC_covar[3 * 3] = {0};
154+
double r_MC_r_MC_covar[3 * 3] = {0};
155+
156+
covar.GetCovarianceBlockInTangentSpace(T_MC_param.q, T_MC_param.q, q_MC_q_MC_covar);
157+
covar.GetCovarianceBlockInTangentSpace(T_MC_param.q, T_MC_param.r, q_MC_r_MC_covar);
158+
covar.GetCovarianceBlockInTangentSpace(T_MC_param.r, T_MC_param.r, r_MC_r_MC_covar);
159+
160+
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> covar_rot(q_MC_q_MC_covar);
161+
auto rotx_std = (covar_rot(0, 0) < 1e-8) ? 0 : std::sqrt(covar_rot(0, 0));
162+
auto roty_std = (covar_rot(1, 1) < 1e-8) ? 0 : std::sqrt(covar_rot(1, 1));
163+
auto rotz_std = (covar_rot(2, 2) < 1e-8) ? 0 : std::sqrt(covar_rot(2, 2));
164+
print_matrix("covar_rot", covar_rot);
165+
printf("rotx_std: %f [deg]\n", rad2deg(rotx_std));
166+
printf("roty_std: %f [deg]\n", rad2deg(roty_std));
167+
printf("rotz_std: %f [deg]\n", rad2deg(rotz_std));
168+
169+
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> covar_trans(r_MC_r_MC_covar);
170+
auto rx_std = std::sqrt(covar_trans(0, 0));
171+
auto ry_std = std::sqrt(covar_trans(1, 1));
172+
auto rz_std = std::sqrt(covar_trans(2, 2));
173+
printf("rx_std: %e\n", rx_std);
174+
printf("ry_std: %e\n", ry_std);
175+
printf("rz_std: %e\n", rz_std);
176+
137177
// Finish up
138178
// -- Marker pose
139179
T_WM.clear();
@@ -148,7 +188,7 @@ int calib_vicon_marker_solve(const aprilgrids_t &aprilgrids,
148188
return 0;
149189
}
150190

151-
double evaluate_vicon_marker_cost(const aprilgrids_t &aprilgrids,
191+
double evaluate_mocap_marker_cost(const aprilgrids_t &aprilgrids,
152192
calib_params_t &cam,
153193
mat4s_t &T_WM,
154194
mat4_t &T_MC) {

0 commit comments

Comments
 (0)