1. LCCT 알고리즘 개요
이 문서는 카메라와 라이다 센서 간의 외부 캘리브레이션 기법 중 하나인 'Fast Extrinsic Calibration of a Laser Rangefinder to a Camera' 논문에서 제시된 LCCT(Lidar-Camera Calibration with Chessboard Target) 알고리즘의 주요 단계를 정리합니다. 전체적인 알고리즘 흐름은 다음과 같습니다.
2. 데이터 전처리
LCCT 알고리즘은 외부 파라미터 최적화를 두 단계로 나누어 수행합니다. 전처리 단계에서는 캘리브레이션에 사용될 체스보드 패턴의 이미지와 해당 패턴의 3D 포인트 클라우드를 준비합니다. 체스보드 포인트 클라우드는 수동 또는 자동 분할 기법을 통해 획득할 수 있습니다.
2.1. 카메라 좌표계 내 체스보드 평면 정보 추출
카메라 이미지에서 체스보드 코너를 감지하고, 이 코너들을 사용하여 카메라 좌표계 내에서 체스보드 평면의 법선 벡터와 원점으로부터의 거리를 계산합니다. 이 과정은 일반적으로 다음과 같은 단계를 포함합니다:
- 이미지에서 체스보드 코너 검출.
- PnP(Perspective-n-Point) 알고리즘을 사용하여 월드 좌표계(체스보드 기준)에서 카메라로의 외부 파라미터(회전 행렬 R, 평행 이동 벡터 t)를 추정.
- 체스보드 코너를 내부 파라미터를 통해 카메라 좌표계로 투영.
2.1.1. 법선 벡터 계산
카메라 좌표계에서의 평면 법선 벡터 \\( \mathbf{n}_c \\)는 월드 좌표계의 Z축 방향 벡터 \\( [0,0,1]^T \\)를 카메라 회전 행렬 \\( R \\)로 변환하여 얻을 수 있습니다. 즉, \\( \mathbf{n}_c = R \cdot [0,0,1]^T \\)이며, 이는 회전 행렬 \\( R \\)의 세 번째 열 벡터와 같습니다.
2.1.2. 원점으로부터의 거리 계산
평면 방정식은 일반적으로 \\( \mathbf{n}_c \cdot \mathbf{X}_c + d_c = 0 \\) 형태로 표현됩니다. 여기서 \\( \mathbf{X}_c \\)는 카메라 좌표계 내의 임의의 점이며, \\( d_c \\)는 카메라 원점으로부터 평면까지의 거리입니다. 이 거리 \\( d_c \\)는 \\( d_c = -\mathbf{n}_c \cdot \mathbf{t} \\)로 계산할 수 있습니다. 여기서 \\( \mathbf{t} \\)는 체스보드 월드 좌표계 원점에서 카메라 좌표계 원점까지의 평행 이동 벡터입니다.
2.1.3. 구현 예시 (C++)
아래 C++ 코드는 카메라 이미지에서 체스보드 코너를 감지하고 PnP를 사용하여 체스보드 평면의 법선 벡터와 원점으로부터의 거리를 계산하는 과정을 보여줍니다. 픽셀 좌표는 이미 카메라 좌표로 변환되었다고 가정합니다.
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <vector>
#include <Eigen/Dense> // Using Eigen for vector operations
// 체스보드 평면 정보를 담을 구조체
struct PlaneInfo {
Eigen::Vector3f normal; // 법선 벡터
float distance; // 원점으로부터의 거리
};
// obj_points: 체스보드 코너의 3D 월드 좌표
// img_points_list: 각 이미지의 2D 코너 픽셀 좌표 리스트
// camera_intrinsic_mat: 카메라 내부 파라미터 행렬
// dist_coeffs: 왜곡 계수
// out_plane_info_list: 계산된 평면 정보가 저장될 리스트
void extractCameraPlaneInfo(
const std::vector& obj_points,
const std::vector>& img_points_list,
const cv::Mat& camera_intrinsic_mat,
const cv::Mat& dist_coeffs,
std::vector<PlaneInfo>& out_plane_info_list)
{
out_plane_info_list.clear();
for (const auto& current_img_points : img_points_list) {
cv::Mat rvec_est, tvec_est; // 추정된 회전 및 평행 이동 벡터
// PnP를 사용하여 카메라 외부 파라미터 추정
cv::solvePnP(obj_points, current_img_points, camera_intrinsic_mat, dist_coeffs,
rvec_est, tvec_est, false, cv::SOLVEPNP_ITERATIVE);
cv::Mat rot_mat;
cv::Rodrigues(rvec_est, rot_mat); // 회전 벡터를 회전 행렬로 변환
Eigen::Vector3f normal_cam_coord;
// 회전 행렬의 세 번째 열이 카메라 좌표계에서의 법선 벡터
normal_cam_coord << rot_mat.at<double>(0, 2),
rot_mat.at<double>(1, 2),
rot_mat.at<double>(2, 2);
// 거리 계산: d = -n_c * t
// tvec_est는 카메라 좌표계에서의 월드 원점 (체스보드 원점)
float distance_cam_coord = -normal_cam_coord.dot(Eigen::Vector3f(
tvec_est.at<double>(0, 0), tvec_est.at<double>(1, 0), tvec_est.at<double>(2, 0)));
out_plane_info_list.push_back({normal_cam_coord, distance_cam_coord});
}
}
2.2. 라이다 좌표계 내 체스보드 평면 정보 추출
분할된 체스보드 포인트 클라우드를 사용하여 라이다 좌표계 내에서 평면을 피팅하고 해당 평면의 법선 벡터와 원점으로부터의 거리를 추정합니다. 일반적으로 TLS(Total Least Squares) 방법을 사용한 평면 피팅을 통해 수행됩니다.
2.2.1. 구현 예시 (C++)
아래 코드는 주어진 3D 포인트들로부터 평면 방정식을 추정하는 TLS 기반 함수입니다. 평면 방정식은 \\( ax + by + cz + d = 0 \\) 형태의 계수 \\( [a, b, c, d]^T \\)로 반환됩니다.
#include <Eigen/Dense>
#include <vector>
#include <numeric> // For std::accumulate
// 3D 포인트 리스트에서 평면 계수를 추정하는 함수
// points: 라이다 좌표계의 3D 체스보드 포인트
// out_coeffs: 추정된 평면 계수 (a, b, c, d)
// 반환값: 0 성공, -1 실패 (포인트 부족)
int estimatePlaneFromPoints(const std::vector& points,
Eigen::Vector4f& out_coeffs) {
if (points.size() < 4) { // 최소 4개의 점이 필요
return -1;
}
Eigen::Vector3f centroid = Eigen::Vector3f::Zero();
for (const auto& pt : points) {
centroid += pt;
}
centroid /= static_cast<float>(points.size());
// 공분산 행렬 A 계산
Eigen::MatrixXf A(points.size(), 3);
for (size_t i = 0; i < points.size(); ++i) {
A.row(i) = (points[i] - centroid).transpose();
}
// SVD를 사용하여 A.transpose() * A의 고유 벡터 계산
// A.transpose() * A의 가장 작은 고유값에 해당하는 고유 벡터가 평면의 법선 벡터
Eigen::JacobiSVD svd(A.transpose() * A, Eigen::ComputeFullU | Eigen::ComputeFullV);
// 가장 작은 고유값에 해당하는 고유 벡터는 V 행렬의 마지막 열
Eigen::Vector3f normal_vec = svd.matrixV().col(2); // V의 마지막 열이 법선 벡터
// d 계산: normal_vec . centroid + d = 0 => d = -normal_vec . centroid
float d = -normal_vec.dot(centroid);
// 법선 벡터의 방향을 일관성 있게 유지 (옵션)
// 예를 들어, Z축 방향이 양수가 되도록 조정
if (normal_vec.z() < 0) {
normal_vec *= -1.0f;
d *= -1.0f;
}
out_coeffs << normal_vec.x(), normal_vec.y(), normal_vec.z(), d;
return 0;
}
주의: 평면 계수 \\( d \\)의 부호는 법선 벡터의 방향에 따라 달라지므로, 일관된 기준을 적용해야 합니다.
3. 1단계: 초기 외부 파라미터 추정
이 단계에서는 전처리에서 얻은 카메라 및 라이다 좌표계에서의 체스보드 평면 정보를 바탕으로 카메라와 라이다 간의 초기 회전 행렬과 평행 이동 벡터를 계산합니다.
3.1. 입력 데이터
초기 추정을 위한 입력 데이터는 다음과 같습니다:
- 각 체스보드 포즈에 대한 카메라 좌표계에서의 평면 법선 벡터와 카메라 원점으로부터의 거리.
- 각 체스보드 포즈에 대한 라이다 좌표계에서의 평면 법선 벡터와 라이다 원점으로부터의 거리.
3.2. 초기 평행 이동 벡터 계산
카메라에서 라이다까지의 평행 이동 벡터 \\( \mathbf{t}_{CL} \\)를 추정합니다. 라이다 좌표계에서의 평면까지의 거리 \\( \alpha_l \\)와 카메라 좌표계에서의 거리 \\( \alpha_c \\)는 다음과 같은 관계를 가집니다: 라이다 좌표계의 점이 \\( \mathbf{x}_l \\)일 때, 카메라 좌표계로 변환된 점 \\( \mathbf{R}\mathbf{x}_l + \mathbf{t}_{CL} \\)가 평면 \\( \theta_c \cdot \mathbf{X}_c + \alpha_c = 0 \\) 위에 있다고 가정하면, \\( \theta_c \cdot (\mathbf{R}\mathbf{x}_l + \mathbf{t}_{CL}) + \alpha_c = 0 \\)입니다.
간략화된 목표 함수는 다음과 같습니다: 카메라 원점에서 체스보드 평면까지의 거리에서 라이다 원점에서 체스보드 평면까지의 거리를 뺀 값이, 라이다의 카메라 좌표계 위치 \\( \mathbf{t}_{CL} \\)의 평면 법선 벡터 방향 성분과 같다는 가정 하에 오차를 최소화합니다.
최적화 목표 함수는 다음과 같습니다:
\[ \min_{\mathbf{t}_{CL}} \sum_{i} (\alpha_{l,i} - (\alpha_{c,i} - \mathbf{\theta}_{c,i}^T \mathbf{t}_{CL}))^2 \]
이를 행렬 형태로 정리하면:
\[ \mathbf{t}_{CL,1} = \text{arg}\min_{\mathbf{t}_{CL}} || \mathbf{\Theta}_c^T \mathbf{t}_{CL} - (\mathbf{\alpha}_c - \mathbf{\alpha}_l) ||_F^2 \]
이 식의 해는 다음과 같습니다:
\[ \mathbf{t}_{CL,1} = (\mathbf{\Theta}_c \mathbf{\Theta}_c^T)^{-1} \mathbf{\Theta}_c (\mathbf{\alpha}_c - \mathbf{\alpha}_l) \]
여기서 \\( \mathbf{\Theta}_c \\)는 각 평면의 정규화된 카메라 법선 벡터들을 쌓은 행렬이며, \\( \mathbf{\alpha}_c \\)와 \\( \mathbf{\alpha}_l \\)는 각 평면의 거리 벡터입니다.
3.3. 초기 회전 행렬 계산
라이다 좌표계의 체스보드 평면 법선 벡터가 회전 행렬 \\( \mathbf{R}_{CL} \\)에 의해 카메라 좌표계로 변환되었을 때, 카메라 좌표계에서의 법선 벡터와 일치해야 합니다. 즉, 두 벡터의 내적값이 1(코사인 값)이 되어야 합니다.
최적화 목표 함수는 다음과 같습니다:
\[ \mathbf{R}_{CL,1} = \text{arg}\max_{\mathbf{R}_{CL}} \sum_{i} \mathbf{\theta}_{c,i}^T (\mathbf{R}_{CL} \mathbf{\theta}_{l,i}) \]
여기서 \\( \mathbf{R}_{CL} \\)은 \\( \mathbf{R}_{CL}^T \mathbf{R}_{CL} = \mathbf{I} \\) 및 \\( \det(\mathbf{R}_{CL}) = 1 \\) 조건을 만족하는 회전 행렬입니다. 스칼라를 최대화하는 이 문제는 Orthogonal Procrustes Problem (OPP)으로 변환되며, 다음의 폐쇄형 해를 가집니다:
\[ \mathbf{R}_{CL,1} = \mathbf{V}\mathbf{U}^T \]
여기서 \\( \mathbf{U}\mathbf{S}\mathbf{V}^T \\)는 \\( \mathbf{\Theta}_l \mathbf{\Theta}_c^T \\)의 SVD(Singular Value Decomposition) 결과입니다.
3.4. 구현 예시 (C++)
아래 코드는 위에서 설명한 초기 회전 행렬 및 평행 이동 벡터를 계산하는 함수입니다.
#include <Eigen/Dense>
#include <vector>
#include <opencv2/core.hpp> // For cv::Mat if mixed with OpenCV
// normal_vec_cam: 카메라 좌표계의 (법선 벡터 x, y, z, 거리 d) 리스트
// normal_vec_lidar: 라이다 좌표계의 (법선 벡터 x, y, z, 거리 d) 리스트
// out_rot_mat: 계산된 초기 회전 행렬 (라이다 -> 카메라)
// out_trans_vec: 계산된 초기 평행 이동 벡터 (라이다 -> 카메라)
// 반환값: 0 성공, -1 실패 (데이터 부족)
int estimateInitialExtrinsics(
const std::vector& normal_vec_cam,
const std::vector& normal_vec_lidar,
Eigen::Matrix3f& out_rot_mat, Eigen::Vector3f& out_trans_vec) {
const int num_planes = (int)normal_vec_cam.size();
if (normal_vec_cam.size() != normal_vec_lidar.size() || num_planes < 3) { // 최소 3개의 평면 필요
// 최소 캘리브레이션 포즈는 3개, 안정성을 위해 5개 이상 권장
return -1;
}
Eigen::MatrixXf theta_c_mat(num_planes, 3);
Eigen::VectorXf alpha_c_vec(num_planes);
Eigen::MatrixXf theta_l_mat(num_planes, 3);
Eigen::VectorXf alpha_l_vec(num_planes);
for (int i = 0; i < num_planes; ++i) {
theta_c_mat.row(i) = normal_vec_cam[i].head<3>();
alpha_c_vec(i) = normal_vec_cam[i][3];
theta_l_mat.row(i) = normal_vec_lidar[i].head<3>();
alpha_l_vec(i) = normal_vec_lidar[i][3];
}
// 평행 이동 벡터 t 계산: t = (theta_c^T * theta_c)^-1 * theta_c^T * (alpha_c - alpha_l)
// Eigen은 A.transpose() * A 대신 A.colPivHouseholderQr().solve(b) 같은 안정적인 방법 권장
// 하지만 간결성을 위해 일반적인 형태를 따름.
Eigen::MatrixXf A_trans_A = theta_c_mat.transpose() * theta_c_mat;
if (A_trans_A.determinant() == 0) return -1; // 역행렬 존재 여부 확인
out_trans_vec = A_trans_A.inverse() * theta_c_mat.transpose() * (alpha_c_vec - alpha_l_vec);
// 회전 행렬 R 계산: R = V * U^T from SVD(theta_l * theta_c^T)
Eigen::MatrixXf M = theta_l_mat.transpose() * theta_c_mat; // SVD 대상 행렬
Eigen::JacobiSVD svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
out_rot_mat = svd.matrixV() * svd.matrixU().transpose();
// 회전 행렬이 제대로 된 회전을 나타내는지 확인 (det = 1)
if (out_rot_mat.determinant() < 0) {
// 특이값 분해 결과가 오른손 좌표계를 따르지 않을 경우
// 마지막 열 벡터의 부호를 반전시켜 det=1을 만듦.
Eigen::Matrix3f V_prime = svd.matrixV();
V_prime.col(2) *= -1;
out_rot_mat = V_prime * svd.matrixU().transpose();
}
return 0;
}
4. 2단계: 파라미터 최적화
1단계에서 얻은 초기 외부 파라미터를 바탕으로, 라이다 포인트 클라우드를 직접 사용하여 더욱 정밀한 최적화를 수행합니다. 이 단계의 목표는 라이다 좌표계의 점들이 변환된 후 카메라 좌표계 내의 체스보드 평면에 정확히 일치하도록 \\( \mathbf{R}_{CL} \\)과 \\( \mathbf{t}_{CL} \\)를 미세 조정하는 것입니다.
4.1. 내점(Inlier) 선택
라이다 포인트 클라우드에서 각 점이 이전에 추정된 체스보드 평면에 얼마나 가까운지 계산하여, 설정된 임계값(예: 거리 중간값)을 초과하는 아웃라이어(outlier) 점들을 제거합니다. 이 과정을 통해 노이즈의 영향을 줄이고 최적화의 견고성을 높입니다.
4.2. 최적화 목표 함수
라이다 좌표계의 체스보드 포인트 \\( \mathbf{x}_{l,i}^{(j)} \\)를 외부 파라미터 \\( \mathbf{R}_{CL}, \mathbf{t}_{CL} \\)를 사용하여 카메라 좌표계로 변환한 후, 해당 점이 카메라 좌표계에서의 체스보드 평면 \\( \mathbf{\theta}_{c,i}^T \mathbf{X}_c + \alpha_{c,i} = 0 \\)에 얼마나 잘 일치하는지를 측정하여 오차를 최소화합니다. 여기서 \\( i \\)는 이미지 인덱스를, \\( j \\)는 각 이미지에 대한 라이다 포인트 인덱스를 나타냅니다.
최적화 함수는 다음과 같습니다:
\[ \text{arg}\min_{\mathbf{R}_{CL},\mathbf{t}_{CL}} \sum_{i=1}^{n} \frac{1}{m(i)} \sum_{j=1}^{m(i)} (\mathbf{\theta}_{c,i}^T(\mathbf{R}_{CL}\mathbf{x}_{l,i}^{(j)} + \mathbf{t}_{CL}) - \alpha_{c,i})^2 \]
이 비선형 최소제곱 문제는 Ceres Solver나 g2o와 같은 최적화 라이브러리를 사용하여 해결할 수 있습니다.
4.3. 캘리브레이션 결과 평가
이 알고리즘은 일반적인 카메라 캘리브레이션에서 사용되는 리프로젝션 오차와 같은 정량적인 평가 지표를 명시적으로 제시하지 않습니다. 하지만 캘리브레이션의 품질을 평가하기 위해서는, 변환된 라이다 포인트가 카메라 이미지의 체스보드 경계와 얼마나 잘 일치하는지 시각적으로 확인하거나, 추가적인 메트릭을 도입할 수 있습니다. 예를 들어, OpenCalib과 같은 프로젝트에서 제시하는 바와 같이, 특수하게 설계된 타겟(예: 빈 영역을 포함하는 타겟)을 사용하여 PnP 결과를 얻고, 이를 통해 리프로젝션 오차를 계산하여 캘리브레이션의 정확도를 정량적으로 평가할 수 있습니다. 노이즈가 많은 점들로 인해 체스보드 가장자리에서 오차가 발생할 수 있으므로, 타겟 설계와 인라이어 필터링이 중요합니다.