Small improvements to 2D fixed frame pose support. (#1714)
This is a follow up to #1580. Signed-off-by: Wolfgang Hess <whess@lyft.com>master
parent
5899bff425
commit
595ffba0ce
|
@ -209,6 +209,7 @@ void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id,
|
||||||
void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) {
|
void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) {
|
||||||
imu_data_.Trim(node_data_, node_id);
|
imu_data_.Trim(node_data_, node_id);
|
||||||
odometry_data_.Trim(node_data_, node_id);
|
odometry_data_.Trim(node_data_, node_id);
|
||||||
|
fixed_frame_pose_data_.Trim(node_data_, node_id);
|
||||||
node_data_.Trim(node_id);
|
node_data_.Trim(node_id);
|
||||||
if (node_data_.SizeOfTrajectoryOrZero(node_id.trajectory_id) == 0) {
|
if (node_data_.SizeOfTrajectoryOrZero(node_id.trajectory_id) == 0) {
|
||||||
trajectory_data_.erase(node_id.trajectory_id);
|
trajectory_data_.erase(node_id.trajectory_id);
|
||||||
|
@ -377,19 +378,24 @@ void OptimizationProblem2D::Solve(
|
||||||
fixed_frame_pose_in_map = transform::Project2D(
|
fixed_frame_pose_in_map = transform::Project2D(
|
||||||
trajectory_data.fixed_frame_origin_in_map.value());
|
trajectory_data.fixed_frame_origin_in_map.value());
|
||||||
} else {
|
} else {
|
||||||
fixed_frame_pose_in_map = node_data.global_pose_2d;
|
fixed_frame_pose_in_map =
|
||||||
|
node_data.global_pose_2d *
|
||||||
|
transform::Project2D(constraint_pose.zbar_ij).inverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
C_fixed_frames.emplace(
|
C_fixed_frames.emplace(trajectory_id,
|
||||||
std::piecewise_construct, std::forward_as_tuple(trajectory_id),
|
FromPose(fixed_frame_pose_in_map));
|
||||||
std::forward_as_tuple(FromPose(fixed_frame_pose_in_map)));
|
|
||||||
fixed_frame_pose_initialized = true;
|
fixed_frame_pose_initialized = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(constraint_pose),
|
problem.AddResidualBlock(
|
||||||
nullptr /* loss function */,
|
CreateAutoDiffSpaCostFunction(constraint_pose),
|
||||||
C_fixed_frames.at(trajectory_id).data(),
|
options_.fixed_frame_pose_use_tolerant_loss()
|
||||||
C_nodes.at(node_id).data());
|
? new ceres::TolerantLoss(
|
||||||
|
options_.fixed_frame_pose_tolerant_loss_param_a(),
|
||||||
|
options_.fixed_frame_pose_tolerant_loss_param_b())
|
||||||
|
: nullptr,
|
||||||
|
C_fixed_frames.at(trajectory_id).data(), C_nodes.at(node_id).data());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue