通常视觉slam中landmark为一个世界系下的点，可以参数化为一个向量\(P = [x,y,z]^T\)或者参数化为第一次观测到的图像坐标加一维逆深度。
对于使用marker的slam来说，通常的做法并不可取。对于一个marker来说，四个特征点的位置是具有特殊的约束（四个坐标点之间的距离固定，并且在一个平面内）。因此在这里将marker作为一个整体参数化为一个三维空间下的旋转加平移。
Bundle Adjustment
相关定义如下：
- marker特征点在marker坐标系为\(P^m\)
- marker 在相机坐标系坐标为\([t^c_m, R^c_m]\)
- 相机在世界坐标系坐标为\([t^w_c, R^w_c]\)
cost function
cost function定义为相机系下的marker特征点投影的预测值和观测值做减法，如下：
\[
f([t^c_m, R^c_m]) = \pi((R^c_m * P^m + t^c_m)) - p^c
\]
其中\(\pi()\)为投影函数。
jacobian
jacobian为costfunction对求解变量的导数，可以根据链式法则分解为两部分：
归一化相机坐标\(p=[u, v]^T\)相机坐标系点\(P^c = [x,y,z]^T\)
\[
{\frac{\partial f(p)}{\partial P^c}}^{2\times3} = \
\left(\begin{array}{lll}{\frac{\partial u}{\partial x}} & {\frac{\partial u}{\partial y}} & {\frac{\partial u}{\partial z}} \\ {\frac{\partial v}{\partial x}} & {\frac{\partial v}{\partial y}} & {\frac{\partial v}{\partial z}}\end{array}\right)=\left(\begin{array}{ccc}{\frac{1}{z}} & {0} & {\frac{-x}{z^{2}}} \\ {0} & {\frac{1}{z}} & {\frac{-y}{z^{2}}}\end{array}\right)
\]
相机坐标系点\(P^c=[x,y,z]^T\)marker的平移和旋转
\[
\begin{aligned}
\frac{\partial f\left(q_{m}^{c}\right)}{\partial q_{m}^{c}} &= \lim _{\delta \theta_{m}^{c} \rightarrow 0} \frac{ {R}_{m}^{c} exp(\left[\delta \theta_{m}^{c}\right]_{\times}) \left( p^m \right) - {R}_{m}^{c} \left( p^m \right) }{\delta \theta_{m}^{c}} \\
&\approx \lim_{\delta \theta_{m}^{c} \rightarrow 0} \frac{ {R}_{m}^{c} (I^{3 \times 3} + \left[ \delta \theta_{m}^{c} \right]_\times) \left( p^m \right) - {R}_{m}^{c} \left( p^m \right) }{\delta \theta_{m}^{c}} (泰勒展开)\\
&= \lim _{\delta \theta_{m}^{c} \rightarrow 0} \frac{ {R}_{m}^{c} \left[ \delta \theta_{m}^{c} \right]_\times \left( p^m \right) }{\delta \theta_{m}^{c}} \\
&= \lim _{\delta \theta_{m}^{c} \rightarrow 0} \frac{ -{R}_{m}^{c} \delta \theta_{m}^{c} \left[ \left( p^m \right)\right]_\times }{\delta \theta_{m}^{c}} (叉乘性质)\\
&= -{R}_{m}^{c} \left[ \left( p^m \right)\right]_\times \\
\end{aligned}
\]
Demo
此demo读取一帧图像，首先进行PNP计算当前相机位姿，再通过BundleAdjustment优化。图中黄色为PNP的结果叠加上噪声后重投影，绿色为BundleAdjustment优化的结果重投影。
class MarkerProjectionCostfunction : public ceres::SizedCostFunction<ceres::DYNAMIC, 7, 7>
{
public:
MarkerProjectionCostfunction(const std::vector<cv::Point2f>& obs,
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> corners, const Eigen::Matrix2d& information)
: obs_(obs)
, corners_(corners)
{
set_num_residuals(corners.size() * 2);
}
virtual ~MarkerProjectionCostfunction() {}
public:
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
//Eigen::Vector3d twb(parameters[0][0], parameters[0][1], parameters[0][2]);
//Eigen::Quaterniond rwb(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
Eigen::Vector3d tbm(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Quaterniond rbm(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
Eigen::Map<Eigen::VectorXd> residual(residuals, num_residuals());
std::vector<Eigen::Matrix<double, 2, 3>> reduces;
int size = corners_.size();
for (int pos = 0; pos < size; pos++){
auto& pm = corners_[pos];
Eigen::Vector3d pb = rbm * pm + tbm;
Eigen::Matrix<double, 2, 3> reduce;
reduce << 1. / pb.z(), 0, -pb.x() / (pb.z() * pb.z()),
0, 1. / pb.z(), -pb.y() / (pb.z() * pb.z());
reduces.push_back(reduce);
residual.segment(pos * 2, 2) = (pb / pb.z()).head<2>() - Eigen::Vector2d(obs_[pos].x, obs_[pos].y);
}
if (jacobians) {
if (jacobians[0]) {
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[0], num_residuals(), 7);
jacobian.setZero();
}
if (jacobians[1]) {
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[1], num_residuals(), 7);
for (int pos = 0; pos < size; pos++) {
Eigen::Matrix<double, 3, 6> jaco;
jaco.leftCols<3>() = Eigen::Matrix3d::Identity();
jaco.rightCols<3>() = -1. * rbm.toRotationMatrix() * Sophus::SO3d::hat(corners_[pos]);
jacobian.block<2, 7>(pos * 2, 0).leftCols<6>() = reduces[pos] * jaco;
jacobian.block<2, 7>(pos * 2, 0).rightCols<1>().setZero();
}
}
}
return true;
}
public:
std::vector<cv::Point2f> obs_;
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> corners_;
};
bool bundleAdjuster()
{
// read images
cv::Mat image = cv::imread("image.png", cv::IMREAD_GRAYSCALE);
if (image.empty()) {
return false;
}
// debug
cv::Mat output = image.clone();
cv::cvtColor(output, output, CV_GRAY2RGB);
// detect
auto detector = boost::make_shared<AprilTagDetector>("tag36h11", 0.);
Markers markers;
detector->find(image, markers);
if (markers.empty()) {
return false;
}
for (auto& marker : markers) {
cv::line(output, marker.corners[0], marker.corners[1], cv::Scalar(0, 255, 0));
cv::line(output, marker.corners[1], marker.corners[2], cv::Scalar(0, 0, 255));
cv::line(output, marker.corners[2], marker.corners[3], cv::Scalar(0, 0, 255));
cv::line(output, marker.corners[3], marker.corners[0], cv::Scalar(255, 0, 0));
std::string label = cv::format("#%d", marker.id);
cv::putText(output, label, marker.center, cv::FONT_HERSHEY_COMPLEX_SMALL, 1., cv::Scalar(255, 0, 0), 1);
}
// undistort
double fx = 442.743649;
double s = 0;
double cx = 301.858989;
double fy = 443.371918;
double cy = 257.307880;
double k1 = -0.307555;
double k2 = 0.086253;
double k3 = 0;
double p1 = 0.000436;
double p2 = -0.000926;
cv::Mat intrinsic_matrix = cv::Mat::eye(3, 3, CV_64F);
intrinsic_matrix.at<double>(0, 0)/* fx */ = fx;
intrinsic_matrix.at<double>(0, 1)/* s */ = s;
intrinsic_matrix.at<double>(0, 2)/* cx */ = cx;
intrinsic_matrix.at<double>(1, 1)/* fy */ = fy;
intrinsic_matrix.at<double>(1, 2)/* cy */ = cy;
cv::Mat distortion_coeffs = cv::Mat::zeros(5, 1, CV_64F);
distortion_coeffs.at<double>(0, 0) = k1;
distortion_coeffs.at<double>(1, 0) = k2;
distortion_coeffs.at<double>(2, 0) = p1;
distortion_coeffs.at<double>(3, 0) = p2;
distortion_coeffs.at<double>(4, 0) = k3;
std::vector<cv::Point2f> observation;
cv::undistortPoints(markers[0].corners, observation, intrinsic_matrix, distortion_coeffs, cv::Mat(), intrinsic_matrix);
for (auto &point : observation) {
//point.x += (3. * (float)((std::rand() % 10000) - 5000) / 5000.);
//point.y += (3. * (float)((std::rand() % 10000) - 5000) / 5000.);
point.x = (point.x - cx) / fx;
point.y = (point.y - cy) / fy;
}
{
std::vector<cv::Point2f> reprojects;
std::vector<cv::Point3f> objects;
cv::convertPointsToHomogeneous(observation, objects);
cv::projectPoints(objects, cv::Vec3f(0., 0., 0.), cv::Vec3f(0., 0., 0.), intrinsic_matrix, distortion_coeffs, reprojects);
}
// pnp
Eigen::Matrix4d twb = Eigen::Matrix4d::Identity();
Eigen::Matrix4d tbm = Eigen::Matrix4d::Identity();
{
std::vector<cv::Point3f> objects;
objects.push_back(cv::Point3f(-0.08, -0.08, 0.));
objects.push_back(cv::Point3f(0.08, -0.08, 0.));
objects.push_back(cv::Point3f(0.08, 0.08, 0.));
objects.push_back(cv::Point3f(-0.08, 0.08, 0.));
cv::Mat r, t, rot;
cv::Mat intrinsic = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
if (!cv::solvePnP(objects, observation, intrinsic, cv::Mat(), r, t)) {
return false;
}
cv::Rodrigues(r, rot);
{
cv::Mat noise = (cv::Mat_<double>(3, 1) <<
(0.5 * (float)((std::rand() % 10000) - 5000) / 5000.),
(0.5 * (float)((std::rand() % 10000) - 5000) / 5000.),
(0.5 * (float)((std::rand() % 10000) - 5000) / 5000.));
t += noise;
}
{
cv::Mat noise = (cv::Mat_<double>(3, 1) <<
(0.5 * (float)((std::rand() % 10000) - 5000) / 5000.),
(0.5 * (float)((std::rand() % 10000) - 5000) / 5000.),
(0.5 * (float)((std::rand() % 10000) - 5000) / 5000.));
r += noise;
}
cv::Mat trans = cv::Mat::eye(4, 4, CV_32F);
rot.copyTo(trans.rowRange(0, 3).colRange(0, 3));
t.copyTo(trans.rowRange(0, 3).colRange(3, 4));
cv::cv2eigen(trans, tbm);
std::cout << "tbm" << std::endl << tbm << std::endl;
std::vector<cv::Point2f> reprojects;
cv::projectPoints(objects, r, t, intrinsic_matrix, distortion_coeffs, reprojects);
std::cout << "reprojection: " << std::endl;
for (int pos = 0; pos < reprojects.size(); pos ++) {
auto& point = reprojects[pos];
auto& origin = markers[0].corners[pos];
std::printf("reprojection: (%8.5f,%8.5f)->(%8.5f,%8.5f) = (%8.5f,%8.5f).\n", origin.x, origin.y, point.x, point.y, origin.x-point.x, origin.y-point.y);
cv::circle(output, point, 2, cv::Scalar(0, 255, 255), -1);
}
}
// bundle adjuster
{
ceres::Problem problem;
ceres::LossFunction* loss = new ceres::HuberLoss(1.0);
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> corners;
corners.push_back(Eigen::Vector3d(-0.08, -0.08, 0.));
corners.push_back(Eigen::Vector3d(0.08, -0.08, 0.));
corners.push_back(Eigen::Vector3d(0.08, 0.08, 0.));
corners.push_back(Eigen::Vector3d(-0.08, 0.08, 0.));
Eigen::Matrix<double, 7, 1, Eigen::DontAlign> vtwb;
vtwb.segment(0, 3) = twb.block<3, 1>(0, 3);
vtwb.segment(3, 4) = Eigen::Quaterniond(twb.block<3, 3>(0, 0)).coeffs();
problem.AddParameterBlock(vtwb.data(), 7, new SpecialEuclideanGroupPlus());
problem.SetParameterBlockConstant(vtwb.data());
Eigen::Matrix<double, 7, 1, Eigen::DontAlign> vtbm;
vtbm.segment(0, 3) = tbm.block<3, 1>(0, 3);
vtbm.segment(3, 4) = Eigen::Quaterniond(tbm.block<3, 3>(0, 0)).coeffs();
problem.AddParameterBlock(vtbm.data(), 7, new SpecialEuclideanGroupPlus());
auto factor = new MarkerProjectionCostfunction(observation, corners, Eigen::Matrix2d::Identity());
problem.AddResidualBlock(factor, loss, vtwb.data(), vtbm.data());
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = 10000;
options.max_solver_time_in_seconds = 1.;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::printf("%s\n", summary.FullReport().c_str());
/**/
Eigen::Matrix4d ttbm = Eigen::Matrix4d::Identity();
ttbm.block<3, 3>(0, 0) = Eigen::Quaterniond(vtbm.segment(3, 4).data()).normalized().toRotationMatrix();
ttbm.block<3, 1>(0, 3) = vtbm.segment(0, 3);
std::cout << "ttbm" << std::endl << ttbm << std::endl;
{
std::vector<cv::Point3f> objects;
for (auto &corner : corners) {
Eigen::Vector3d object = ttbm.block<3, 3>(0, 0) * corner + ttbm.block<3, 1>(0, 3);
objects.push_back(cv::Point3f(object.x(), object.y(), object.z()));
}
std::vector<cv::Point2f> reprojects;
cv::projectPoints(objects, cv::Vec3f(0., 0., 0.), cv::Vec3f(0., 0., 0.), intrinsic_matrix, distortion_coeffs, reprojects);
std::cout << "bundle adjuster reprojection: " << std::endl;
for (int pos = 0; pos < reprojects.size(); pos++) {
auto& point = reprojects[pos];
auto& origin = markers[0].corners[pos];
std::printf("reprojection: (%8.5f,%8.5f)->(%8.5f,%8.5f) = (%8.5f,%8.5f).\n", origin.x, origin.y, point.x, point.y, origin.x - point.x, origin.y - point.y);
cv::circle(output, point, 1, cv::Scalar(0, 255, 0), -1);
}
}
}
cv::imshow("view", output); cv::waitKey(0);
return true;
}
Reference
[1] <视觉slam十四讲> [2] https://blog.csdn.net/heyijia0327/article/details/60143160