LM for cam-imu optimization

This commit is contained in:
Vladyslav Usenko 2019-06-27 14:45:25 +02:00
parent 14dd14fb02
commit 57bc27c713
3 changed files with 297 additions and 32 deletions

View File

@ -155,9 +155,7 @@ class PosesOptimization {
while (!step && max_iter > 0) { while (!step && max_iter > 0) {
Eigen::unordered_map<int64_t, Sophus::SE3d> timestam_to_pose_backup = Eigen::unordered_map<int64_t, Sophus::SE3d> timestam_to_pose_backup =
timestam_to_pose; timestam_to_pose;
Eigen::vector<SE3> T_i_c_backup = calib->T_i_c; Calibration<Scalar> calib_backup = *calib;
Eigen::vector<GenericCamera<Scalar>> intrinsics_backup =
calib->intrinsics;
Eigen::VectorXd inc = -lopt.accum.solve(lambda); Eigen::VectorXd inc = -lopt.accum.solve(lambda);
@ -179,22 +177,26 @@ class PosesOptimization {
ComputeErrorPosesOpt<double> eopt(problem_size, timestam_to_pose, ccd); ComputeErrorPosesOpt<double> eopt(problem_size, timestam_to_pose, ccd);
tbb::parallel_reduce(april_range, eopt); tbb::parallel_reduce(april_range, eopt);
if (eopt.error <= lopt.error) { 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 std::cout << "\t[REJECTED] lambda:" << lambda
<< " Error: " << eopt.error << " num points " << " Error: " << eopt.error << " num points "
<< eopt.num_points << std::endl; << eopt.num_points << std::endl;
lambda = std::min(max_lambda, 2 * lambda); lambda = std::min(max_lambda, 2 * lambda);
timestam_to_pose = timestam_to_pose_backup; timestam_to_pose = timestam_to_pose_backup;
calib->T_i_c = T_i_c_backup; *calib = calib_backup;
calib->intrinsics = intrinsics_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--; max_iter--;
} }

View File

@ -631,6 +631,206 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 } // namespace basalt
#endif #endif

View File

@ -88,7 +88,12 @@ class SplineOptimization {
typedef Se3Spline<N, Scalar> SplineT; typedef Se3Spline<N, Scalar> SplineT;
SplineOptimization(int64_t dt_ns = 1e7) 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; pose_var_inv = 1.0 / pose_var;
// reasonable default values // reasonable default values
@ -360,45 +365,101 @@ class SplineOptimization {
// auto t1 = std::chrono::high_resolution_clock::now(); // auto t1 = std::chrono::high_resolution_clock::now();
if (use_poses) { tbb::blocked_range<PoseDataIter> pose_range(pose_measurements.begin(),
tbb::blocked_range<PoseDataIter> pose_range(pose_measurements.begin(), pose_measurements.end());
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); tbb::parallel_reduce(pose_range, lopt);
// lopt(pose_range); // lopt(pose_range);
} }
if (use_april_corners) { if (use_april_corners) {
tbb::blocked_range<AprilgridCornersDataIter> april_range(
aprilgrid_corners_measurements.begin(),
aprilgrid_corners_measurements.end());
tbb::parallel_reduce(april_range, lopt); tbb::parallel_reduce(april_range, lopt);
// lopt(april_range); // lopt(april_range);
} }
if (use_mocap) { if (use_mocap) {
tbb::blocked_range<MocapPoseDataIter> mocap_pose_range(
mocap_measurements.begin(), mocap_measurements.end());
tbb::parallel_reduce(mocap_pose_range, lopt); tbb::parallel_reduce(mocap_pose_range, lopt);
// lopt(mocap_pose_range); // lopt(mocap_pose_range);
} }
tbb::blocked_range<AccelDataIter> accel_range(accel_measurements.begin(),
accel_measurements.end());
tbb::parallel_reduce(accel_range, lopt); tbb::parallel_reduce(accel_range, lopt);
tbb::blocked_range<GyroDataIter> gyro_range(gyro_measurements.begin(),
gyro_measurements.end());
tbb::parallel_reduce(gyro_range, lopt); tbb::parallel_reduce(gyro_range, lopt);
VectorX inc_full = -lopt.accum.solve();
applyInc(inc_full, offset_cam_intrinsics);
error = lopt.error; error = lopt.error;
num_points = lopt.num_points; num_points = lopt.num_points;
reprojection_error = lopt.reprojection_error; 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]; 1e9 * inc_full[mocap_block_offset + 2 * POSE_SIZE + 1];
} }
Scalar lambda, min_lambda, max_lambda;
int64_t min_time_us, max_time_us; int64_t min_time_us, max_time_us;
Eigen::deque<PoseData> pose_measurements; Eigen::deque<PoseData> pose_measurements;