[GenericPoseGraph] Move functions that add param blocks to constraint_utils. (#1374)

Also call `ceres_loss()` instead of passing `nullptr`.
master
Alexander Belyaev 2018-08-07 14:48:39 +02:00 committed by Wally B. Feed
parent 6274fc1558
commit d5840e960a
9 changed files with 110 additions and 103 deletions

View File

@ -18,34 +18,10 @@
#include "absl/memory/memory.h"
#include "cartographer/common/utils.h"
#include "cartographer/pose_graph/constraint/constraint_utils.h"
namespace cartographer {
namespace pose_graph {
namespace {
void AddPoseParameters(Pose3D* pose, ceres::Problem* problem) {
auto transation = pose->mutable_translation();
auto rotation = pose->mutable_rotation();
problem->AddParameterBlock(transation->data(), transation->size());
problem->AddParameterBlock(rotation->data(), rotation->size());
if (pose->constant()) {
problem->SetParameterBlockConstant(transation->data());
problem->SetParameterBlockConstant(rotation->data());
}
}
void AddImuParameters(ImuCalibration* pose, ceres::Problem* problem) {
auto gravity = pose->mutable_gravity_constant();
auto orientation = pose->mutable_orientation();
problem->AddParameterBlock(gravity, 1);
problem->AddParameterBlock(orientation->data(), orientation->size());
if (pose->constant()) {
problem->SetParameterBlockConstant(gravity);
problem->SetParameterBlockConstant(orientation->data());
}
}
} // namespace
AccelerationConstraint3D::AccelerationConstraint3D(
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
@ -90,10 +66,10 @@ void AccelerationConstraint3D::AddToOptimizer(Nodes* nodes,
return;
}
AddPoseParameters(first_node, problem);
AddPoseParameters(second_node, problem);
AddPoseParameters(third_node, problem);
AddImuParameters(imu_node, problem);
AddPose3D(first_node, problem);
AddPose3D(second_node, problem);
AddPose3D(third_node, problem);
AddImuCalibration(imu_node, problem);
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
second_node->mutable_rotation()->data(),
second_node->mutable_translation()->data(),

View File

@ -58,4 +58,4 @@ class AccelerationConstraint3D : public Constraint {
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ACCELERATION_CONSTRAINT_3D_H_

View File

@ -0,0 +1,37 @@
#include "cartographer/pose_graph/constraint/constraint_utils.h"
namespace cartographer {
namespace pose_graph {
void AddPose2D(Pose2D* pose, ceres::Problem* problem) {
auto pose_2d = pose->mutable_pose_2d();
problem->AddParameterBlock(pose_2d->data(), pose_2d->size());
if (pose->constant()) {
problem->SetParameterBlockConstant(pose_2d->data());
}
}
void AddPose3D(Pose3D* pose, ceres::Problem* problem) {
auto transation = pose->mutable_translation();
auto rotation = pose->mutable_rotation();
problem->AddParameterBlock(transation->data(), transation->size());
problem->AddParameterBlock(rotation->data(), rotation->size());
if (pose->constant()) {
problem->SetParameterBlockConstant(transation->data());
problem->SetParameterBlockConstant(rotation->data());
}
}
void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem) {
auto gravity = pose->mutable_gravity_constant();
auto orientation = pose->mutable_orientation();
problem->AddParameterBlock(gravity, 1);
problem->AddParameterBlock(orientation->data(), orientation->size());
if (pose->constant()) {
problem->SetParameterBlockConstant(gravity);
problem->SetParameterBlockConstant(orientation->data());
}
}
} // namespace pose_graph
} // namespace cartographer

View File

@ -0,0 +1,37 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_
#include "cartographer/pose_graph/node/imu_calibration.h"
#include "cartographer/pose_graph/node/pose_2d.h"
#include "cartographer/pose_graph/node/pose_3d.h"
#include "ceres/problem.h"
namespace cartographer {
namespace pose_graph {
void AddPose2D(Pose2D* pose, ceres::Problem* problem);
void AddPose3D(Pose3D* pose, ceres::Problem* problem);
void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem);
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_

View File

@ -18,31 +18,10 @@
#include "absl/memory/memory.h"
#include "cartographer/common/utils.h"
#include "cartographer/pose_graph/constraint/constraint_utils.h"
namespace cartographer {
namespace pose_graph {
namespace {
void AddPose3DParameters(Pose3D* pose, ceres::Problem* problem) {
auto transation = pose->mutable_translation();
auto rotation = pose->mutable_rotation();
problem->AddParameterBlock(transation->data(), transation->size());
problem->AddParameterBlock(rotation->data(), rotation->size());
if (pose->constant()) {
problem->SetParameterBlockConstant(transation->data());
problem->SetParameterBlockConstant(rotation->data());
}
}
void AddPose2DParameters(Pose2D* pose, ceres::Problem* problem) {
auto pose_2d = pose->mutable_pose_2d();
problem->AddParameterBlock(pose_2d->data(), pose_2d->size());
if (pose->constant()) {
problem->SetParameterBlockConstant(pose_2d->data());
}
}
} // namespace
InterpolatedRelativePoseConstraint2D::InterpolatedRelativePoseConstraint2D(
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
@ -81,9 +60,9 @@ void InterpolatedRelativePoseConstraint2D::AddToOptimizer(
return;
}
AddPose2DParameters(first_node_start, problem);
AddPose2DParameters(first_node_end, problem);
AddPose3DParameters(second_node, problem);
AddPose2D(first_node_start, problem);
AddPose2D(first_node_end, problem);
AddPose3D(second_node, problem);
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
first_node_start->mutable_pose_2d()->data(),
first_node_end->mutable_pose_2d()->data(),

View File

@ -18,23 +18,10 @@
#include "absl/memory/memory.h"
#include "cartographer/common/utils.h"
#include "cartographer/pose_graph/constraint/constraint_utils.h"
namespace cartographer {
namespace pose_graph {
namespace {
void AddPoseParameters(Pose3D* pose, ceres::Problem* problem) {
auto transation = pose->mutable_translation();
auto rotation = pose->mutable_rotation();
problem->AddParameterBlock(transation->data(), transation->size());
problem->AddParameterBlock(rotation->data(), rotation->size());
if (pose->constant()) {
problem->SetParameterBlockConstant(transation->data());
problem->SetParameterBlockConstant(rotation->data());
}
}
} // namespace
InterpolatedRelativePoseConstraint3D::InterpolatedRelativePoseConstraint3D(
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
@ -73,10 +60,10 @@ void InterpolatedRelativePoseConstraint3D::AddToOptimizer(
return;
}
AddPoseParameters(first_node_start, problem);
AddPoseParameters(first_node_end, problem);
AddPoseParameters(second_node, problem);
problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */,
AddPose3D(first_node_start, problem);
AddPose3D(first_node_end, problem);
AddPose3D(second_node, problem);
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
first_node_start->mutable_translation()->data(),
first_node_start->mutable_rotation()->data(),
first_node_end->mutable_translation()->data(),

View File

@ -18,6 +18,7 @@
#include "absl/memory/memory.h"
#include "cartographer/common/utils.h"
#include "cartographer/pose_graph/constraint/constraint_utils.h"
namespace cartographer {
namespace pose_graph {

View File

@ -18,23 +18,10 @@
#include "absl/memory/memory.h"
#include "cartographer/common/utils.h"
#include "cartographer/pose_graph/constraint/constraint_utils.h"
namespace cartographer {
namespace pose_graph {
namespace {
void AddPoseParameters(Pose3D* pose, ceres::Problem* problem) {
auto transation = pose->mutable_translation();
auto rotation = pose->mutable_rotation();
problem->AddParameterBlock(transation->data(), transation->size());
problem->AddParameterBlock(rotation->data(), rotation->size());
if (pose->constant()) {
problem->SetParameterBlockConstant(transation->data());
problem->SetParameterBlockConstant(rotation->data());
}
}
} // namespace
RelativePoseConstraint3D::RelativePoseConstraint3D(
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
@ -64,9 +51,9 @@ void RelativePoseConstraint3D::AddToOptimizer(Nodes* nodes,
return;
}
AddPoseParameters(first_node, problem);
AddPoseParameters(second_node, problem);
problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */,
AddPose3D(first_node, problem);
AddPose3D(second_node, problem);
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
first_node->mutable_translation()->data(),
first_node->mutable_rotation()->data(),
second_node->mutable_translation()->data(),

View File

@ -23,7 +23,7 @@ namespace cartographer {
namespace pose_graph {
namespace {
void AddRotationParameters(Pose3D* pose, ceres::Problem* problem) {
void AddRotation3D(Pose3D* pose, ceres::Problem* problem) {
auto rotation = pose->mutable_rotation();
problem->AddParameterBlock(rotation->data(), rotation->size());
if (pose->constant()) {
@ -31,6 +31,14 @@ void AddRotationParameters(Pose3D* pose, ceres::Problem* problem) {
}
}
void AddImuOrientation(ImuCalibration* imu_node, ceres::Problem* problem) {
auto imu_orientation = imu_node->mutable_orientation();
problem->AddParameterBlock(imu_orientation->data(), imu_orientation->size());
if (imu_node->constant()) {
problem->SetParameterBlockConstant(imu_orientation->data());
}
}
} // namespace
RotationContraint3D::RotationContraint3D(
@ -62,25 +70,20 @@ void RotationContraint3D::AddToOptimizer(Nodes* nodes,
return;
}
auto imu_calibration_node =
auto imu_node =
common::FindOrNull(nodes->imu_calibration_nodes, imu_calibration_);
if (imu_calibration_node == nullptr) {
if (imu_node == nullptr) {
LOG(INFO) << "Imu calibration node was not found.";
return;
}
AddRotationParameters(first_node, problem);
AddRotationParameters(second_node, problem);
auto imu_orientation = imu_calibration_node->mutable_orientation();
problem->AddParameterBlock(imu_orientation->data(), imu_orientation->size());
if (imu_calibration_node->constant()) {
problem->SetParameterBlockConstant(imu_orientation->data());
}
problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */,
AddRotation3D(first_node, problem);
AddRotation3D(second_node, problem);
AddImuOrientation(imu_node, problem);
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
first_node->mutable_rotation()->data(),
second_node->mutable_rotation()->data(),
imu_orientation->data());
imu_node->mutable_orientation()->data());
}
proto::CostFunction RotationContraint3D::ToCostFunctionProto() const {