12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Use Huber loss for landmark cost functions. (#1377)

master
Michael Grupp 2018-08-09 10:30:41 +02:00 committed by Alexander Belyaev
parent d5840e960a
commit 4d6120d2cb
2 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cartographer/mapping/internal/optimization/optimization_problem_2d.cc
View File

@ -81,7 +81,8 @@ void AddLandmarkCostFunctions(
const std::map<std::string, LandmarkNode>& landmark_nodes, const std::map<std::string, LandmarkNode>& landmark_nodes,
bool freeze_landmarks, const MapById<NodeId, NodeSpec2D>& node_data, bool freeze_landmarks, const MapById<NodeId, NodeSpec2D>& node_data,
MapById<NodeId, std::array<double, 3>>* C_nodes, MapById<NodeId, std::array<double, 3>>* C_nodes,
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) { std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem,
double huber_scale) {
for (const auto& landmark_node : landmark_nodes) { for (const auto& landmark_node : landmark_nodes) {
// Do not use landmarks that were not optimized for localization. // Do not use landmarks that were not optimized for localization.
if (!landmark_node.second.global_landmark_pose.has_value() && if (!landmark_node.second.global_landmark_pose.has_value() &&
@ -132,7 +133,7 @@ void AddLandmarkCostFunctions(
problem->AddResidualBlock( problem->AddResidualBlock(
LandmarkCostFunction2D::CreateAutoDiffCostFunction( LandmarkCostFunction2D::CreateAutoDiffCostFunction(
observation, prev->data, next->data), observation, prev->data, next->data),
nullptr /* loss function */, prev_node_pose->data(), new ceres::HuberLoss(huber_scale), prev_node_pose->data(),
next_node_pose->data(), C_landmarks->at(landmark_id).rotation(), next_node_pose->data(), C_landmarks->at(landmark_id).rotation(),
C_landmarks->at(landmark_id).translation()); C_landmarks->at(landmark_id).translation());
} }
@ -246,7 +247,7 @@ void OptimizationProblem2D::Solve(
for (const Constraint& constraint : constraints) { for (const Constraint& constraint : constraints) {
problem.AddResidualBlock( problem.AddResidualBlock(
CreateAutoDiffSpaCostFunction(constraint.pose), CreateAutoDiffSpaCostFunction(constraint.pose),
// Only loop closure constraints should have a loss function. // Loop closure constraints should have a loss function.
constraint.tag == Constraint::INTER_SUBMAP constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale()) ? new ceres::HuberLoss(options_.huber_scale())
: nullptr, : nullptr,
@ -255,7 +256,7 @@ void OptimizationProblem2D::Solve(
} }
// Add cost functions for landmarks. // Add cost functions for landmarks.
AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_, AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,
&C_nodes, &C_landmarks, &problem); &C_nodes, &C_landmarks, &problem, options_.huber_scale());
// Add penalties for violating odometry or changes between consecutive nodes // Add penalties for violating odometry or changes between consecutive nodes
// if odometry is not available. // if odometry is not available.
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {

9
cartographer/mapping/internal/optimization/optimization_problem_3d.cc
View File

@ -125,7 +125,8 @@ void AddLandmarkCostFunctions(
const std::map<std::string, LandmarkNode>& landmark_nodes, const std::map<std::string, LandmarkNode>& landmark_nodes,
bool freeze_landmarks, const MapById<NodeId, NodeSpec3D>& node_data, bool freeze_landmarks, const MapById<NodeId, NodeSpec3D>& node_data,
MapById<NodeId, CeresPose>* C_nodes, MapById<NodeId, CeresPose>* C_nodes,
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) { std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem,
double huber_scale) {
for (const auto& landmark_node : landmark_nodes) { for (const auto& landmark_node : landmark_nodes) {
// Do not use landmarks that were not optimized for localization. // Do not use landmarks that were not optimized for localization.
if (!landmark_node.second.global_landmark_pose.has_value() && if (!landmark_node.second.global_landmark_pose.has_value() &&
@ -176,7 +177,7 @@ void AddLandmarkCostFunctions(
problem->AddResidualBlock( problem->AddResidualBlock(
LandmarkCostFunction3D::CreateAutoDiffCostFunction( LandmarkCostFunction3D::CreateAutoDiffCostFunction(
observation, prev->data, next->data), observation, prev->data, next->data),
nullptr /* loss function */, prev_node_pose->rotation(), new ceres::HuberLoss(huber_scale), prev_node_pose->rotation(),
prev_node_pose->translation(), next_node_pose->rotation(), prev_node_pose->translation(), next_node_pose->rotation(),
next_node_pose->translation(), next_node_pose->translation(),
C_landmarks->at(landmark_id).rotation(), C_landmarks->at(landmark_id).rotation(),
@ -340,7 +341,7 @@ void OptimizationProblem3D::Solve(
for (const Constraint& constraint : constraints) { for (const Constraint& constraint : constraints) {
problem.AddResidualBlock( problem.AddResidualBlock(
SpaCostFunction3D::CreateAutoDiffCostFunction(constraint.pose), SpaCostFunction3D::CreateAutoDiffCostFunction(constraint.pose),
// Only loop closure constraints should have a loss function. // Loop closure constraints should have a loss function.
constraint.tag == Constraint::INTER_SUBMAP constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale()) ? new ceres::HuberLoss(options_.huber_scale())
: nullptr /* loss function */, : nullptr /* loss function */,
@ -351,7 +352,7 @@ void OptimizationProblem3D::Solve(
} }
// Add cost functions for landmarks. // Add cost functions for landmarks.
AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_, AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,
&C_nodes, &C_landmarks, &problem); &C_nodes, &C_landmarks, &problem, options_.huber_scale());
// Add constraints based on IMU observations of angular velocities and // Add constraints based on IMU observations of angular velocities and
// linear acceleration. // linear acceleration.
if (!options_.fix_z_in_3d()) { if (!options_.fix_z_in_3d()) {