LM for cam-imu optimization
This commit is contained in:
Vladyslav Usenko
parent
14dd14fb02
commit
57bc27c713
|
|
@ -155,9 +155,7 @@ class PosesOptimization {
|
|||
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;
|
||||
Calibration<Scalar> calib_backup = *calib;
|
||||
|
||||
Eigen::VectorXd inc = -lopt.accum.solve(lambda);
|
||||
|
||||
|
|
@ -179,22 +177,26 @@ class PosesOptimization {
|
|||
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 {
|
||||
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--;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -631,6 +631,206 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
|
|||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
template <int N, typename Scalar>
|
||||
struct ComputeErrorSplineOpt : public LinearizeBase<Scalar> {
|
||||
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, 2, 4> Matrix24;
|
||||
|
||||
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
|
||||
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
|
||||
|
||||
typedef Se3Spline<N, Scalar> SplineT;
|
||||
|
||||
typedef typename Eigen::deque<PoseData>::const_iterator PoseDataIter;
|
||||
typedef typename Eigen::deque<GyroData>::const_iterator GyroDataIter;
|
||||
typedef typename Eigen::deque<AccelData>::const_iterator AccelDataIter;
|
||||
typedef typename Eigen::deque<AprilgridCornersData>::const_iterator
|
||||
AprilgridCornersDataIter;
|
||||
typedef
|
||||
typename Eigen::deque<MocapPoseData>::const_iterator MocapPoseDataIter;
|
||||
|
||||
// typedef typename LinearizeBase<Scalar>::PoseCalibH PoseCalibH;
|
||||
typedef typename LinearizeBase<Scalar>::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<PoseDataIter>& 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<AccelDataIter>& 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<GyroDataIter>& 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<AprilgridCornersDataIter>& 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<MocapPoseDataIter>& 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
|
||||
|
|
|
|||
|
|
@ -88,7 +88,12 @@ class SplineOptimization {
|
|||
typedef Se3Spline<N, Scalar> 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<PoseDataIter> pose_range(pose_measurements.begin(),
|
||||
pose_measurements.end());
|
||||
tbb::blocked_range<AprilgridCornersDataIter> april_range(
|
||||
aprilgrid_corners_measurements.begin(),
|
||||
aprilgrid_corners_measurements.end());
|
||||
|
||||
tbb::blocked_range<MocapPoseDataIter> mocap_pose_range(
|
||||
mocap_measurements.begin(), mocap_measurements.end());
|
||||
|
||||
tbb::blocked_range<AccelDataIter> accel_range(accel_measurements.begin(),
|
||||
accel_measurements.end());
|
||||
|
||||
tbb::blocked_range<GyroDataIter> 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<AprilgridCornersDataIter> 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<MocapPoseDataIter> mocap_pose_range(
|
||||
mocap_measurements.begin(), mocap_measurements.end());
|
||||
tbb::parallel_reduce(mocap_pose_range, lopt);
|
||||
// lopt(mocap_pose_range);
|
||||
}
|
||||
|
||||
tbb::blocked_range<AccelDataIter> accel_range(accel_measurements.begin(),
|
||||
accel_measurements.end());
|
||||
tbb::parallel_reduce(accel_range, lopt);
|
||||
|
||||
tbb::blocked_range<GyroDataIter> 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<Scalar> calib_backup = *calib;
|
||||
MocapCalibration<Scalar> 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<PoseData> pose_measurements;
|
||||
|
|
|
|||
Loading…
Reference in New Issue