diff --git a/include/basalt/optimization/accumulator.h b/include/basalt/optimization/accumulator.h index 2e12356..c5507a8 100644 --- a/include/basalt/optimization/accumulator.h +++ b/include/basalt/optimization/accumulator.h @@ -220,16 +220,17 @@ class SparseHashAccumulator { b.template segment(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 triplets; triplets.reserve(hash_map.size() * 36 + b.rows()); - for (int i = 0; i < b.rows(); i++) { - triplets.emplace_back(i, i, 0.000001); - } + if (alpha > 0) + for (int i = 0; i < b.rows(); i++) { + triplets.emplace_back(i, i, alpha); + } for (const auto& kv : hash_map) { // if (kv.first[2] != kv.second.rows()) std::cerr << "rows mismatch" << diff --git a/include/basalt/optimization/linearize.h b/include/basalt/optimization/linearize.h index 6c2a2e7..dccb264 100644 --- a/include/basalt/optimization/linearize.h +++ b/include/basalt/optimization/linearize.h @@ -134,11 +134,10 @@ struct LinearizeBase { template inline void linearize_point(const Eigen::Vector2d& corner_pos, int corner_id, const Eigen::Matrix4d& T_c_w, const CamT& cam, - PoseCalibH& cph, double& error, + PoseCalibH* cph, double& error, int& num_points, double& reproj_err) const { Eigen::Matrix d_r_d_p; Eigen::Matrix d_r_d_param; - Eigen::Matrix 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 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 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 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; diff --git a/include/basalt/optimization/poses_linearize.h b/include/basalt/optimization/poses_linearize.h index 510f229..78bf660 100644 --- a/include/basalt/optimization/poses_linearize.h +++ b/include/basalt/optimization/poses_linearize.h @@ -117,7 +117,7 @@ struct LinearizePosesOpt : public LinearizeBase { 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 { } }; +template +struct ComputeErrorPosesOpt : public LinearizeBase { + static const int POSE_SIZE = LinearizeBase::POSE_SIZE; + + typedef Sophus::SE3 SE3; + typedef Eigen::Matrix Vector2; + typedef Eigen::Matrix Vector3; + typedef Eigen::Matrix Vector4; + typedef Eigen::Matrix Vector6; + + typedef Eigen::Matrix Matrix3; + typedef Eigen::Matrix Matrix6; + + typedef Eigen::Matrix VectorX; + typedef Eigen::Matrix MatrixX; + + typedef typename Eigen::vector::const_iterator + AprilgridCornersDataIter; + + typedef typename LinearizeBase::CalibCommonData CalibCommonData; + + Scalar error; + Scalar reprojection_error; + int num_points; + + size_t opt_size; + + const Eigen::unordered_map& timestam_to_pose; + + ComputeErrorPosesOpt( + size_t opt_size, + const Eigen::unordered_map& 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& 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 diff --git a/include/basalt/optimization/poses_optimize.h b/include/basalt/optimization/poses_optimize.h index d0ad7a3..f57ebd9 100644 --- a/include/basalt/optimization/poses_optimize.h +++ b/include/basalt/optimization/poses_optimize.h @@ -60,7 +60,7 @@ class PosesOptimization { typename Eigen::vector::const_iterator; public: - PosesOptimization() {} + PosesOptimization() : lambda(1e-6), min_lambda(1e-6), max_lambda(10) {} bool initializeIntrinsics( size_t cam_id, const Eigen::vector &corners, @@ -146,19 +146,57 @@ 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; - for (auto &kv : timestam_to_pose) { - kv.second *= Sophus::expd(inc.segment(offset_poses[kv.first])); - } + bool step = false; + int max_iter = 10; - for (size_t i = 0; i < calib->T_i_c.size(); i++) { - calib->T_i_c[i] *= Sophus::expd(inc.segment(offset_T_i_c[i])); - } + while (!step && max_iter > 0) { + Eigen::unordered_map timestam_to_pose_backup = + timestam_to_pose; + Eigen::vector T_i_c_backup = calib->T_i_c; + Eigen::vector> intrinsics_backup = + calib->intrinsics; - 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())); + Eigen::VectorXd inc = -lopt.accum.solve(lambda); + + for (auto &kv : timestam_to_pose) { + kv.second *= + Sophus::expd(inc.segment(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(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 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--; } } @@ -241,6 +279,8 @@ class PosesOptimization { typename LinearizePosesOpt< Scalar, SparseHashAccumulator>::CalibCommonData ccd; + Scalar lambda, min_lambda, max_lambda; + size_t problem_size; std::unordered_map offset_poses; diff --git a/include/basalt/optimization/spline_linearize.h b/include/basalt/optimization/spline_linearize.h index d396aa2..0190b4a 100644 --- a/include/basalt/optimization/spline_linearize.h +++ b/include/basalt/optimization/spline_linearize.h @@ -395,7 +395,7 @@ struct LinearizeSplineOpt : public LinearizeBase { 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); }