[GenericPoseGraph] Add acceleration constraint. (#1319)

master
Alexander Belyaev 2018-07-24 06:36:24 +02:00 committed by GitHub
parent 8250264441
commit d4193d4a2a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 431 additions and 2 deletions

View File

@ -0,0 +1,118 @@
/*
* 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.
*/
#include "cartographer/pose_graph/constraint/acceleration_constraint_3d.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/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,
const proto::Acceleration3D& proto)
: Constraint(id, loss_function_proto),
first_(proto.first()),
second_(proto.second()),
third_(proto.third()),
imu_(proto.imu_calibration()),
cost_(new AccelerationCost3D(proto.parameters())),
ceres_cost_(common::make_unique<AutoDiffFunction>(cost_)) {}
void AccelerationConstraint3D::AddToOptimizer(Nodes* nodes,
ceres::Problem* problem) const {
auto first_node = common::FindOrNull(nodes->pose_3d_nodes, first_);
if (first_node == nullptr) {
LOG(INFO) << "First node was not found in pose_3d_nodes.";
return;
}
auto second_node = common::FindOrNull(nodes->pose_3d_nodes, second_);
if (second_node == nullptr) {
LOG(INFO) << "Second node was not found in pose_3d_nodes.";
return;
}
auto third_node = common::FindOrNull(nodes->pose_3d_nodes, third_);
if (third_node == nullptr) {
LOG(INFO) << "Third node was not found in pose_3d_nodes.";
return;
}
auto imu_node = common::FindOrNull(nodes->imu_calibration_nodes, imu_);
if (imu_node == nullptr) {
LOG(INFO) << "IMU calibration node was not found in imu_calibration_nodes.";
return;
}
if (first_node->constant() && second_node->constant() &&
third_node->constant() && imu_node->constant()) {
LOG(INFO) << "All nodes are constant, skipping the constraint.";
return;
}
AddPoseParameters(first_node, problem);
AddPoseParameters(second_node, problem);
AddPoseParameters(third_node, problem);
AddImuParameters(imu_node, problem);
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
second_node->mutable_rotation()->data(),
second_node->mutable_translation()->data(),
first_node->mutable_translation()->data(),
third_node->mutable_translation()->data(),
imu_node->mutable_gravity_constant(),
imu_node->mutable_orientation()->data());
}
proto::CostFunction AccelerationConstraint3D::ToCostFunctionProto() const {
proto::CostFunction cost_function;
auto* acceleration_3d = cost_function.mutable_acceleration_3d();
*acceleration_3d->mutable_first() = first_.ToProto();
*acceleration_3d->mutable_second() = second_.ToProto();
*acceleration_3d->mutable_third() = third_.ToProto();
*acceleration_3d->mutable_imu_calibration() = imu_.ToProto();
*acceleration_3d->mutable_parameters() = cost_->ToProto();
return cost_function;
}
} // namespace pose_graph
} // namespace cartographer

View File

@ -0,0 +1,61 @@
/*
* 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_ACCELERATION_CONSTRAINT_3D_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ACCELERATION_CONSTRAINT_3D_H_
#include "cartographer/pose_graph/constraint/constraint.h"
#include "cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h"
namespace cartographer {
namespace pose_graph {
class AccelerationConstraint3D : public Constraint {
public:
AccelerationConstraint3D(const ConstraintId& id,
const proto::LossFunction& loss_function_proto,
const proto::Acceleration3D& proto);
void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final;
protected:
proto::CostFunction ToCostFunctionProto() const final;
private:
// clang-format off
using AutoDiffFunction =
ceres::AutoDiffCostFunction<AccelerationCost3D,
3 /* residuals */,
4 /* rotation variables second pose*/,
3 /* position variables second pose*/,
3 /* position variables first pose */,
3 /* position variables second pose */,
1 /* gravity variable */,
4 /* imu orientation */>;
// clang-format on
NodeId first_;
NodeId second_;
NodeId third_;
NodeId imu_;
// The cost function is owned by the ceres cost function.
AccelerationCost3D* const cost_;
std::unique_ptr<AutoDiffFunction> ceres_cost_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_

View File

@ -0,0 +1,57 @@
/*
* 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.
*/
#include "cartographer/pose_graph/constraint/acceleration_constraint_3d.h"
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
using testing::EqualsProto;
using testing::ParseProto;
constexpr char kConstraint[] = R"PROTO(
id: "hal_acceleration"
cost_function {
acceleration_3d {
first { object_id: "hal9000" timestamp: 100 }
second { object_id: "hal9000" timestamp: 200 }
third { object_id: "hal9000" timestamp: 300 }
imu_calibration { object_id: "hal_imu" }
parameters {
delta_velocity_imu_frame { x: 1 y: 1 z: 1 }
first_to_second_delta_time_seconds: 10.0
second_to_third_delta_time_seconds: 20.0
scaling_factor: 2.0
}
}
}
loss_function { quadratic_loss {} }
)PROTO";
TEST(AccelerationConstraint3DTest, SerializesCorrectly) {
const auto proto = ParseProto<proto::Constraint>(kConstraint);
AccelerationConstraint3D constraint(proto.id(), proto.loss_function(),
proto.cost_function().acceleration_3d());
const auto actual_proto = constraint.ToProto();
EXPECT_THAT(actual_proto, EqualsProto(kConstraint));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

View File

@ -0,0 +1,47 @@
/*
* 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.
*/
#include "cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace pose_graph {
AccelerationCost3D::AccelerationCost3D(
const proto::Acceleration3D::Parameters& parameters)
: delta_velocity_imu_frame_(
transform::ToEigen(parameters.delta_velocity_imu_frame())),
first_to_second_delta_time_seconds_(
parameters.first_to_second_delta_time_seconds()),
second_to_third_delta_time_seconds_(
parameters.second_to_third_delta_time_seconds()),
scaling_factor_(parameters.scaling_factor()) {}
proto::Acceleration3D::Parameters AccelerationCost3D::ToProto() const {
proto::Acceleration3D::Parameters parameters;
*parameters.mutable_delta_velocity_imu_frame() =
transform::ToProto(delta_velocity_imu_frame_);
parameters.set_first_to_second_delta_time_seconds(
first_to_second_delta_time_seconds_);
parameters.set_second_to_third_delta_time_seconds(
second_to_third_delta_time_seconds_);
parameters.set_scaling_factor(scaling_factor_);
return parameters;
}
} // namespace pose_graph
} // namespace cartographer

View File

@ -0,0 +1,75 @@
/*
* 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_ACCELERATION_COST_3D_H_
#define CARTOGRAPHER_ACCELERATION_COST_3D_H_
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/pose_graph/proto/cost_function.pb.h"
namespace cartographer {
namespace pose_graph {
// Penalizes differences between IMU data and optimized accelerations.
class AccelerationCost3D {
public:
explicit AccelerationCost3D(
const proto::Acceleration3D::Parameters& parameters);
proto::Acceleration3D::Parameters ToProto() const;
template <typename T>
bool operator()(const T* const middle_rotation, const T* const start_position,
const T* const middle_position, const T* const end_position,
const T* const gravity_constant,
const T* const imu_calibration, T* residual) const {
using Vector3T = Eigen::Matrix<T, 3, 1>;
using TranslationMap = Eigen::Map<const Vector3T>;
using RotationMap = Eigen::Map<const Eigen::Quaternion<T>>;
const Vector3T imu_delta_velocity =
RotationMap(middle_rotation) * RotationMap(imu_calibration) *
delta_velocity_imu_frame_.cast<T>() -
(*gravity_constant * 0.5 *
(first_to_second_delta_time_seconds_ +
second_to_third_delta_time_seconds_)) *
Eigen::Vector3d::UnitZ().cast<T>();
const Vector3T start_velocity =
(TranslationMap(middle_position) - TranslationMap(start_position)) /
T(first_to_second_delta_time_seconds_);
const Vector3T end_velocity =
(TranslationMap(end_position) - TranslationMap(middle_position)) /
T(second_to_third_delta_time_seconds_);
const Vector3T delta_velocity = end_velocity - start_velocity;
(Eigen::Map<Vector3T>(residual) =
T(scaling_factor_) * (imu_delta_velocity - delta_velocity));
return true;
}
private:
const Eigen::Vector3d delta_velocity_imu_frame_;
const double first_to_second_delta_time_seconds_;
const double second_to_third_delta_time_seconds_;
const double scaling_factor_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_ACCELERATION_COST_3D_H_

View File

@ -0,0 +1,71 @@
/*
* 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.
*/
#include "cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h"
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
using ::google::protobuf::TextFormat;
using ::testing::ElementsAre;
using testing::EqualsProto;
using testing::Near;
using testing::ParseProto;
using PositionType = std::array<double, 3>;
using RotationType = std::array<double, 4>;
using ResidualType = std::array<double, 3>;
constexpr char kParameters[] = R"PROTO(
delta_velocity_imu_frame { x: 1 y: 1 z: 1 }
first_to_second_delta_time_seconds: 10.0
second_to_third_delta_time_seconds: 20.0
scaling_factor: 2.0
)PROTO";
TEST(AccelerationCost3DTest, SerializesCorrectly) {
AccelerationCost3D acceleration_cost_3d(
ParseProto<proto::Acceleration3D::Parameters>(kParameters));
EXPECT_THAT(acceleration_cost_3d.ToProto(), EqualsProto(kParameters));
}
TEST(AccelerationCost3DTest, EvaluatesCorrectly) {
AccelerationCost3D acceleration_cost_3d(
ParseProto<proto::Acceleration3D::Parameters>(kParameters));
const PositionType kStartPosition{{1., 1., 1.}};
const PositionType kMiddlePosition{{2., 2., 2.}};
const RotationType kMiddleRotation{{0., 0., 0., 1.}};
const PositionType kEndPosition{{3., 3., 3.}};
const double kGravityConstant{10.};
const RotationType kImuCalibration{{0., 0., 0., 1.0}};
ResidualType residuals;
EXPECT_TRUE(acceleration_cost_3d(
kMiddleRotation.data(), kStartPosition.data(), kMiddlePosition.data(),
kEndPosition.data(), &kGravityConstant, kImuCalibration.data(),
residuals.data()));
EXPECT_THAT(residuals, ElementsAre(Near(2.1), Near(2.1), Near(-297.9)));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

View File

@ -51,8 +51,8 @@ message Acceleration3D {
message Parameters { message Parameters {
transform.proto.Vector3d delta_velocity_imu_frame = 1; transform.proto.Vector3d delta_velocity_imu_frame = 1;
int64 first_to_second_time_delta = 2; double first_to_second_delta_time_seconds = 2;
int64 second_to_third_time_delta = 3; double second_to_third_delta_time_seconds = 3;
double scaling_factor = 4; double scaling_factor = 4;
} }
Parameters parameters = 5; Parameters parameters = 5;