ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

LM for camera calibration

This commit is contained in:
Vladyslav Usenko 2019-06-27 13:35:12 +02:00
parent d8de56af10
commit 14dd14fb02
5 changed files with 165 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
include/basalt/optimization/accumulator.h
View File

@ -220,15 +220,16 @@ class SparseHashAccumulator {
b.template segment<ROWS>(i) += data;
}
inline VectorX solve() const {
inline VectorX solve(Scalar alpha = 1e-6) const {
SparseMatrix sm(b.rows(), b.rows());
auto t1 = std::chrono::high_resolution_clock::now();
std::vector<T> triplets;
triplets.reserve(hash_map.size() * 36 + b.rows());
if (alpha > 0)
for (int i = 0; i < b.rows(); i++) {
triplets.emplace_back(i, i, 0.000001);
triplets.emplace_back(i, i, alpha);
}
for (const auto& kv : hash_map) {

38
include/basalt/optimization/linearize.h
View File

@ -134,11 +134,10 @@ struct LinearizeBase {
template <class CamT>
inline void linearize_point(const Eigen::Vector2d& corner_pos, int corner_id,
const Eigen::Matrix4d& T_c_w, const CamT& cam,
PoseCalibH<CamT::N>& cph, double& error,
PoseCalibH<CamT::N>* cph, double& error,
int& num_points, double& reproj_err) const {
Eigen::Matrix<double, 2, 4> d_r_d_p;
Eigen::Matrix<double, 2, CamT::N> d_r_d_param;
Eigen::Matrix<double, 4, 6> d_point_d_xi;
BASALT_ASSERT_STREAM(
corner_id < int(common_data.aprilgrid_corner_pos_3d->size()),
@ -147,28 +146,39 @@ struct LinearizeBase {
Eigen::Vector4d point3d =
T_c_w * common_data.aprilgrid_corner_pos_3d->at(corner_id);
d_point_d_xi.topLeftCorner<3, 3>().setIdentity();
d_point_d_xi.topRightCorner<3, 3>() = -Sophus::SO3d::hat(point3d.head<3>());
d_point_d_xi.row(3).setZero();
Eigen::Vector2d proj;
bool valid = cam.project(point3d, proj, &d_r_d_p, &d_r_d_param);
bool valid;
if (cph) {
valid = cam.project(point3d, proj, &d_r_d_p, &d_r_d_param);
} else {
valid = cam.project(point3d, proj);
}
if (!valid) return;
Eigen::Matrix<double, 2, 6> d_r_d_xi = d_r_d_p * d_point_d_xi;
Eigen::Vector2d residual = proj - corner_pos;
double e = residual.norm();
double huber_weight =
e < common_data.huber_thresh ? 1.0 : common_data.huber_thresh / e;
cph.H_pose_accum += huber_weight * d_r_d_xi.transpose() * d_r_d_xi;
cph.b_pose_accum += huber_weight * d_r_d_xi.transpose() * residual;
if (cph) {
Eigen::Matrix<double, 4, 6> d_point_d_xi;
cph.H_intr_pose_accum += huber_weight * d_r_d_param.transpose() * d_r_d_xi;
cph.H_intr_accum += huber_weight * d_r_d_param.transpose() * d_r_d_param;
cph.b_intr_accum += huber_weight * d_r_d_param.transpose() * residual;
d_point_d_xi.topLeftCorner<3, 3>().setIdentity();
d_point_d_xi.topRightCorner<3, 3>() =
-Sophus::SO3d::hat(point3d.head<3>());
d_point_d_xi.row(3).setZero();
Eigen::Matrix<double, 2, 6> d_r_d_xi = d_r_d_p * d_point_d_xi;
cph->H_pose_accum += huber_weight * d_r_d_xi.transpose() * d_r_d_xi;
cph->b_pose_accum += huber_weight * d_r_d_xi.transpose() * residual;
cph->H_intr_pose_accum +=
huber_weight * d_r_d_param.transpose() * d_r_d_xi;
cph->H_intr_accum += huber_weight * d_r_d_param.transpose() * d_r_d_param;
cph->b_intr_accum += huber_weight * d_r_d_param.transpose() * residual;
}
error += huber_weight * e * e * (2 - huber_weight);
reproj_err += e;

85
include/basalt/optimization/poses_linearize.h
View File

@ -117,7 +117,7 @@ struct LinearizePosesOpt : public LinearizeBase<Scalar> {
for (size_t i = 0; i < acd.corner_pos.size(); i++) {
this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
T_c_w_m, cam, cph, err, num_inliers,
T_c_w_m, cam, &cph, err, num_inliers,
reproj_err);
}
@ -175,6 +175,89 @@ struct LinearizePosesOpt : public LinearizeBase<Scalar> {
}
};
template <typename Scalar>
struct ComputeErrorPosesOpt : public LinearizeBase<Scalar> {
static const int POSE_SIZE = LinearizeBase<Scalar>::POSE_SIZE;
typedef Sophus::SE3<Scalar> SE3;
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 6, 6> Matrix6;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef typename Eigen::vector<AprilgridCornersData>::const_iterator
AprilgridCornersDataIter;
typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
Scalar error;
Scalar reprojection_error;
int num_points;
size_t opt_size;
const Eigen::unordered_map<int64_t, SE3>& timestam_to_pose;
ComputeErrorPosesOpt(
size_t opt_size,
const Eigen::unordered_map<int64_t, SE3>& timestam_to_pose,
const CalibCommonData& common_data)
: opt_size(opt_size), timestam_to_pose(timestam_to_pose) {
this->common_data = common_data;
error = 0;
reprojection_error = 0;
num_points = 0;
}
ComputeErrorPosesOpt(const ComputeErrorPosesOpt& other, tbb::split)
: opt_size(other.opt_size), timestam_to_pose(other.timestam_to_pose) {
this->common_data = other.common_data;
error = 0;
reprojection_error = 0;
num_points = 0;
}
void operator()(const tbb::blocked_range<AprilgridCornersDataIter>& r) {
for (const AprilgridCornersData& acd : r) {
std::visit(
[&](const auto& cam) {
SE3 T_w_i = timestam_to_pose.at(acd.timestamp_ns);
SE3 T_w_c =
T_w_i * this->common_data.calibration->T_i_c[acd.cam_id];
SE3 T_c_w = T_w_c.inverse();
Eigen::Matrix4d T_c_w_m = T_c_w.matrix();
double err = 0;
double reproj_err = 0;
int num_inliers = 0;
for (size_t i = 0; i < acd.corner_pos.size(); i++) {
this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
T_c_w_m, cam, nullptr, err, num_inliers,
reproj_err);
}
error += err;
reprojection_error += reproj_err;
num_points += num_inliers;
},
this->common_data.calibration->intrinsics[acd.cam_id].variant);
}
}
void join(ComputeErrorPosesOpt& rhs) {
error += rhs.error;
reprojection_error += rhs.reprojection_error;
num_points += rhs.num_points;
}
}; // namespace basalt
} // namespace basalt
#endif

48
include/basalt/optimization/poses_optimize.h
View File

@ -60,7 +60,7 @@ class PosesOptimization {
typename Eigen::vector<AprilgridCornersData>::const_iterator;
public:
PosesOptimization() {}
PosesOptimization() : lambda(1e-6), min_lambda(1e-6), max_lambda(10) {}
bool initializeIntrinsics(
size_t cam_id, const Eigen::vector<Eigen::Vector2d> &corners,
@ -146,20 +146,58 @@ class PosesOptimization {
num_points = lopt.num_points;
reprojection_error = lopt.reprojection_error;
Eigen::VectorXd inc = -lopt.accum.solve();
std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
<< lopt.num_points << std::endl;
bool step = false;
int max_iter = 10;
while (!step && max_iter > 0) {
Eigen::unordered_map<int64_t, Sophus::SE3d> timestam_to_pose_backup =
timestam_to_pose;
Eigen::vector<SE3> T_i_c_backup = calib->T_i_c;
Eigen::vector<GenericCamera<Scalar>> intrinsics_backup =
calib->intrinsics;
Eigen::VectorXd inc = -lopt.accum.solve(lambda);
for (auto &kv : timestam_to_pose) {
kv.second *= Sophus::expd(inc.segment<POSE_SIZE>(offset_poses[kv.first]));
kv.second *=
Sophus::expd(inc.segment<POSE_SIZE>(offset_poses[kv.first]));
}
for (size_t i = 0; i < calib->T_i_c.size(); i++) {
calib->T_i_c[i] *= Sophus::expd(inc.segment<POSE_SIZE>(offset_T_i_c[i]));
calib->T_i_c[i] *=
Sophus::expd(inc.segment<POSE_SIZE>(offset_T_i_c[i]));
}
for (size_t i = 0; i < calib->intrinsics.size(); i++) {
auto &c = calib->intrinsics[i];
c.applyInc(inc.segment(offset_cam_intrinsics[i], c.getN()));
}
ComputeErrorPosesOpt<double> eopt(problem_size, timestam_to_pose, ccd);
tbb::parallel_reduce(april_range, eopt);
if (eopt.error <= lopt.error) {
std::cout << "\t[ACCEPTED] lambda:" << lambda
<< " Error: " << eopt.error << " num points "
<< eopt.num_points << std::endl;
lambda = std::max(min_lambda, lambda / 2);
step = true;
} else {
std::cout << "\t[REJECTED] lambda:" << lambda
<< " Error: " << eopt.error << " num points "
<< eopt.num_points << std::endl;
lambda = std::min(max_lambda, 2 * lambda);
timestam_to_pose = timestam_to_pose_backup;
calib->T_i_c = T_i_c_backup;
calib->intrinsics = intrinsics_backup;
}
max_iter--;
}
}
void recompute_size() {
@ -241,6 +279,8 @@ class PosesOptimization {
typename LinearizePosesOpt<
Scalar, SparseHashAccumulator<Scalar>>::CalibCommonData ccd;
Scalar lambda, min_lambda, max_lambda;
size_t problem_size;
std::unordered_map<int64_t, size_t> offset_poses;

2
include/basalt/optimization/spline_linearize.h
View File

@ -395,7 +395,7 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
for (size_t i = 0; i < acd.corner_pos.size(); i++) {
this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
T_c_w_m, cam, cph, err, num_inliers,
T_c_w_m, cam, &cph, err, num_inliers,
reproj_err);
}