diff --git a/include/basalt/optimization/poses_optimize.h b/include/basalt/optimization/poses_optimize.h index f57ebd9..2883875 100644 --- a/include/basalt/optimization/poses_optimize.h +++ b/include/basalt/optimization/poses_optimize.h @@ -155,9 +155,7 @@ class PosesOptimization { 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; + Calibration calib_backup = *calib; Eigen::VectorXd inc = -lopt.accum.solve(lambda); @@ -179,22 +177,26 @@ class PosesOptimization { 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 { + if (eopt.error > lopt.error) { 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; + *calib = calib_backup; + } else { + std::cout << "\t[ACCEPTED] lambda:" << lambda + << " Error: " << eopt.error << " num points " + << eopt.num_points << std::endl; + + lambda = std::max(min_lambda, lambda / 2); + + error = eopt.error; + num_points = eopt.num_points; + reprojection_error = eopt.reprojection_error; + + step = true; } max_iter--; } diff --git a/include/basalt/optimization/spline_linearize.h b/include/basalt/optimization/spline_linearize.h index 0190b4a..4c8fea3 100644 --- a/include/basalt/optimization/spline_linearize.h +++ b/include/basalt/optimization/spline_linearize.h @@ -631,6 +631,206 @@ struct LinearizeSplineOpt : public LinearizeBase { EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; +template +struct ComputeErrorSplineOpt : public LinearizeBase { + 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 Matrix24; + + typedef Eigen::Matrix VectorX; + typedef Eigen::Matrix MatrixX; + + typedef Se3Spline SplineT; + + typedef typename Eigen::deque::const_iterator PoseDataIter; + typedef typename Eigen::deque::const_iterator GyroDataIter; + typedef typename Eigen::deque::const_iterator AccelDataIter; + typedef typename Eigen::deque::const_iterator + AprilgridCornersDataIter; + typedef + typename Eigen::deque::const_iterator MocapPoseDataIter; + + // typedef typename LinearizeBase::PoseCalibH PoseCalibH; + typedef typename LinearizeBase::CalibCommonData CalibCommonData; + + Scalar error; + Scalar reprojection_error; + int num_points; + + size_t opt_size; + + const SplineT* spline; + + ComputeErrorSplineOpt(size_t opt_size, const SplineT* spl, + const CalibCommonData& common_data, + const SplineT* spl_lin = nullptr) + : opt_size(opt_size), spline(spl) { + this->common_data = common_data; + + error = 0; + reprojection_error = 0; + num_points = 0; + + BASALT_ASSERT(spline); + } + + ComputeErrorSplineOpt(const ComputeErrorSplineOpt& other, tbb::split) + : opt_size(other.opt_size), spline(other.spline) { + this->common_data = other.common_data; + error = 0; + reprojection_error = 0; + num_points = 0; + } + + void operator()(const tbb::blocked_range& r) { + for (const PoseData& pm : r) { + int64_t time_ns = pm.timestamp_ns; + + BASALT_ASSERT_STREAM( + time_ns >= spline->minTimeNs(), + "time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs()); + + // Residual from current value of spline + Vector3 residual_pos = + spline->positionResidual(time_ns, pm.data.translation()); + Vector3 residual_rot = + spline->orientationResidual(time_ns, pm.data.so3()); + + // std::cout << "J_pos.start_idx " << J_pos.start_idx << std::endl; + + const Scalar& pose_var_inv = this->common_data.pose_var_inv; + + error += pose_var_inv * + (residual_pos.squaredNorm() + residual_rot.squaredNorm()); + } + } + + void operator()(const tbb::blocked_range& r) { + // size_t num_knots = spline.numKnots(); + // size_t bias_block_offset = POSE_SIZE * num_knots; + + for (const AccelData& pm : r) { + int64_t t = pm.timestamp_ns; + + // std::cout << "time " << t << std::endl; + // std::cout << "sline.minTime() " << spline.minTime() << std::endl; + + BASALT_ASSERT_STREAM( + t >= spline->minTimeNs(), + "t " << t << " spline.minTime() " << spline->minTimeNs()); + BASALT_ASSERT_STREAM( + t <= spline->maxTimeNs(), + "t " << t << " spline.maxTime() " << spline->maxTimeNs()); + + Vector3 residual = spline->accelResidual( + t, pm.data, this->common_data.calibration->calib_accel_bias, + *(this->common_data.g)); + + const Scalar& accel_var_inv = this->common_data.accel_var_inv; + + error += accel_var_inv * residual.squaredNorm(); + } + } + + void operator()(const tbb::blocked_range& r) { + // size_t num_knots = spline.numKnots(); + // size_t bias_block_offset = POSE_SIZE * num_knots; + + for (const GyroData& pm : r) { + int64_t t_ns = pm.timestamp_ns; + + BASALT_ASSERT(t_ns >= spline->minTimeNs()); + BASALT_ASSERT(t_ns <= spline->maxTimeNs()); + + const Scalar& gyro_var_inv = this->common_data.gyro_var_inv; + + Vector3 residual = spline->gyroResidual( + t_ns, pm.data, this->common_data.calibration->calib_gyro_bias); + + error += gyro_var_inv * residual.squaredNorm(); + } + } + + void operator()(const tbb::blocked_range& r) { + for (const AprilgridCornersData& acd : r) { + std::visit( + [&](const auto& cam) { + int64_t time_ns = acd.timestamp_ns + + this->common_data.calibration->cam_time_offset_ns; + + if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs()) + return; + + SE3 T_w_i = spline->pose(time_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 operator()(const tbb::blocked_range& r) { + for (const MocapPoseData& pm : r) { + int64_t time_ns = + pm.timestamp_ns + + this->common_data.mocap_calibration->mocap_time_offset_ns; + + if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs()) + continue; + + BASALT_ASSERT_STREAM( + time_ns >= spline->minTimeNs(), + "time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs()); + + const SE3 T_moc_w = this->common_data.mocap_calibration->T_moc_w; + const SE3 T_i_mark = this->common_data.mocap_calibration->T_i_mark; + + const SE3 T_w_i = spline->pose(time_ns); + const SE3 T_moc_mark = T_moc_w * T_w_i * T_i_mark; + + const SE3 T_mark_moc_meas = pm.data.inverse(); + + Vector6 residual = Sophus::logd(T_mark_moc_meas * T_moc_mark); + + const Scalar& mocap_var_inv = this->common_data.mocap_var_inv; + + error += mocap_var_inv * residual.squaredNorm(); + } + } + + void join(ComputeErrorSplineOpt& rhs) { + error += rhs.error; + reprojection_error += rhs.reprojection_error; + num_points += rhs.num_points; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + } // namespace basalt #endif diff --git a/include/basalt/optimization/spline_optimize.h b/include/basalt/optimization/spline_optimize.h index 839991b..9ea2548 100644 --- a/include/basalt/optimization/spline_optimize.h +++ b/include/basalt/optimization/spline_optimize.h @@ -88,7 +88,12 @@ class SplineOptimization { typedef Se3Spline SplineT; SplineOptimization(int64_t dt_ns = 1e7) - : pose_var(1e-4), spline(dt_ns), dt_ns(dt_ns) { + : pose_var(1e-4), + lambda(1), + min_lambda(1e-6), + max_lambda(10), + spline(dt_ns), + dt_ns(dt_ns) { pose_var_inv = 1.0 / pose_var; // reasonable default values @@ -360,45 +365,101 @@ class SplineOptimization { // auto t1 = std::chrono::high_resolution_clock::now(); - if (use_poses) { - tbb::blocked_range pose_range(pose_measurements.begin(), - pose_measurements.end()); + tbb::blocked_range pose_range(pose_measurements.begin(), + pose_measurements.end()); + tbb::blocked_range april_range( + aprilgrid_corners_measurements.begin(), + aprilgrid_corners_measurements.end()); + tbb::blocked_range mocap_pose_range( + mocap_measurements.begin(), mocap_measurements.end()); + + tbb::blocked_range accel_range(accel_measurements.begin(), + accel_measurements.end()); + + tbb::blocked_range gyro_range(gyro_measurements.begin(), + gyro_measurements.end()); + + if (use_poses) { tbb::parallel_reduce(pose_range, lopt); // lopt(pose_range); } if (use_april_corners) { - tbb::blocked_range april_range( - aprilgrid_corners_measurements.begin(), - aprilgrid_corners_measurements.end()); tbb::parallel_reduce(april_range, lopt); // lopt(april_range); } if (use_mocap) { - tbb::blocked_range mocap_pose_range( - mocap_measurements.begin(), mocap_measurements.end()); tbb::parallel_reduce(mocap_pose_range, lopt); // lopt(mocap_pose_range); } - tbb::blocked_range accel_range(accel_measurements.begin(), - accel_measurements.end()); tbb::parallel_reduce(accel_range, lopt); - - tbb::blocked_range gyro_range(gyro_measurements.begin(), - gyro_measurements.end()); - tbb::parallel_reduce(gyro_range, lopt); - VectorX inc_full = -lopt.accum.solve(); - - applyInc(inc_full, offset_cam_intrinsics); - error = lopt.error; num_points = lopt.num_points; reprojection_error = lopt.reprojection_error; + + 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) { + VectorX inc_full = -lopt.accum.solve(lambda); + + Calibration calib_backup = *calib; + MocapCalibration mocap_calib_backup = *mocap_calib; + SplineT spline_backup = spline; + Vector3 g_backup = g; + + applyInc(inc_full, offset_cam_intrinsics); + + ComputeErrorSplineOpt eopt(opt_size, &spline, ccd); + if (use_poses) { + tbb::parallel_reduce(pose_range, eopt); + } + + if (use_april_corners) { + tbb::parallel_reduce(april_range, eopt); + } + + if (use_mocap) { + tbb::parallel_reduce(mocap_pose_range, eopt); + } + + tbb::parallel_reduce(accel_range, eopt); + tbb::parallel_reduce(gyro_range, eopt); + + if (eopt.error > lopt.error) { + std::cout << "\t[REJECTED] lambda:" << lambda + << " Error: " << eopt.error << " num points " + << eopt.num_points << std::endl; + lambda = std::min(max_lambda, 2 * lambda); + + spline = spline_backup; + *calib = calib_backup; + *mocap_calib = mocap_calib_backup; + g = g_backup; + + } else { + std::cout << "\t[ACCEPTED] lambda:" << lambda + << " Error: " << eopt.error << " num points " + << eopt.num_points << std::endl; + + lambda = std::max(min_lambda, lambda / 2); + + error = eopt.error; + num_points = eopt.num_points; + reprojection_error = eopt.reprojection_error; + + step = true; + } + max_iter--; + } } } @@ -461,6 +522,8 @@ class SplineOptimization { 1e9 * inc_full[mocap_block_offset + 2 * POSE_SIZE + 1]; } + Scalar lambda, min_lambda, max_lambda; + int64_t min_time_us, max_time_us; Eigen::deque pose_measurements;