What is the graph optimization for in SubMapping::insert_frame()'s enable_imu branch?

[VRichardJP]

I fail to understand what the graph optimization is doing in this branch:

glim/src/glim/mapping/sub_mapping.cpp

Lines 117 to 161 in 42bf677

	if (params.enable_imu) {
	logger->debug("smoothing trajectory");
	// Smoothing IMU-based pose estimation
	gtsam::NavState nav_world_imu(gtsam::Pose3(odom_frame->T_world_imu.matrix()), odom_frame->v_world_imu);
	gtsam::imuBias::ConstantBias imu_bias(odom_frame->imu_bias);
	std::vector<double> imu_stamps;
	std::vector<Eigen::Isometry3d> imu_poses;
	imu_integration->integrate_imu(odom_frame->stamp, next_frame->stamp, nav_world_imu, imu_bias, imu_stamps, imu_poses);
	gtsam::Values values;
	for (int i = 0; i < imu_stamps.size(); i++) {
	values.insert(X(i), gtsam::Pose3(imu_poses[i].matrix()));
	}
	gtsam::NonlinearFactorGraph graph;
	graph.emplace_shared<gtsam::PriorFactor<gtsam::Pose3>>(X(0), gtsam::Pose3(odom_frame->T_world_imu.matrix()), gtsam::noiseModel::Isotropic::Sigma(6, 1e-5));
	graph.emplace_shared<gtsam::PriorFactor<gtsam::Pose3>>(X(imu_stamps.size() - 1), gtsam::Pose3(next_frame->T_world_imu.matrix()), gtsam::noiseModel::Isotropic::Sigma(6, 1e-5));
	for (int i = 1; i < imu_stamps.size(); i++) {
	const double dt = (imu_stamps[i] - imu_stamps[i - 1]) / (next_frame->stamp - odom_frame->stamp);
	const Eigen::Isometry3d T_last_current = imu_poses[i - 1].inverse() * imu_poses[i];
	graph.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(X(i - 1), X(i), gtsam::Pose3(T_last_current.matrix()), gtsam::noiseModel::Isotropic::Sigma(6, dt + 1e-2));
	}
	gtsam::LevenbergMarquardtParams lm_params;
	lm_params.setAbsoluteErrorTol(1e-6);
	lm_params.setRelativeErrorTol(1e-6);
	lm_params.setMaxIterations(5);
	#ifdef GTSAM_USE_TBB
	auto arena = static_cast<tbb::task_arena*>(this->tbb_task_arena.get());
	arena->execute([&] {
	#endif
	values = gtsam::LevenbergMarquardtOptimizer(graph, values, lm_params).optimize();
	#ifdef GTSAM_USE_TBB
	});
	#endif
	odom_frame->imu_rate_trajectory.resize(8, imu_stamps.size());
	for (int i = 0; i < imu_stamps.size(); i++) {
	const Eigen::Vector3d trans(imu_poses[i].translation());
	const Eigen::Quaterniond quat(imu_poses[i].linear());
	odom_frame->imu_rate_trajectory.col(i) << imu_stamps[i], trans, quat.x(), quat.y(), quat.z(), quat.w();
	}
	}

graph and values are both local variables, and optimization output (values) does not seem to be used before the if branch is left. In other words, the lines 127 to 153 are self-contained and seem to have no effect.

Do I miss something?

[VRichardJP]

Basically, it feels like these lines should use the smoothened values values->at<gtsam::Pose3>(X(i)) instead of the preintegrated IMU poses imu_poses[i]:

glim/src/glim/mapping/sub_mapping.cpp

Lines 156 to 160 in 42bf677

	for (int i = 0; i < imu_stamps.size(); i++) {
	const Eigen::Vector3d trans(imu_poses[i].translation());
	const Eigen::Quaterniond quat(imu_poses[i].linear());
	odom_frame->imu_rate_trajectory.col(i) << imu_stamps[i], trans, quat.x(), quat.y(), quat.z(), quat.w();
	}

[koide3]

Ah, you are correct. It intended to smooth the trajectory with the estimation results of odometry estimation (i.e., fusing IMU forward prediction and point cloud backward correction in a loose way), but it wrongly used imu_poses instead of values as you pointed out... I'll fix it soon.