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

Remove GenericPoseGraph. (#1540) 

Remove GenericPoseGraph implementation not to confuse the users. It will come back one day.
master
Alexander Belyaev 2019-03-14 08:12:04 +01:00 committed by Andre Gaschler
parent 99bc9e196d
commit ea2bf3b1b2
74 changed files with 0 additions and 4268 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
cartographer/pose_graph/constraint/acceleration_constraint_3d.cc
View File

@ -1,80 +0,0 @@
/*
* 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 "absl/memory/memory.h"
#include "cartographer/common/utils.h"
#include "cartographer/pose_graph/constraint/constraint_utils.h"
namespace cartographer {
namespace pose_graph {
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_(absl::make_unique<AutoDiffFunction>(cost_)) {}
void AccelerationConstraint3D::AddToSolver(Nodes* nodes,
ceres::Problem* problem) const {
FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes,
"First node was not found in pose_3d_nodes.");
FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes,
"Second node was not found in pose_3d_nodes.");
FIND_NODE_OR_RETURN(third_node, third_, nodes->pose_3d_nodes,
"Third node was not found in pose_3d_nodes.");
FIND_NODE_OR_RETURN(
imu_node, imu_, nodes->imu_calibration_nodes,
"IMU calibration node was not found in imu_calibration_nodes.");
if (first_node->constant() && second_node->constant() &&
third_node->constant() && imu_node->constant()) {
LOG(INFO) << "All nodes are constant, skipping the constraint.";
return;
}
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(),
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

61
cartographer/pose_graph/constraint/acceleration_constraint_3d.h
View File

@ -1,61 +0,0 @@
/*
* 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 AddToSolver(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_ACCELERATION_CONSTRAINT_3D_H_

57
cartographer/pose_graph/constraint/acceleration_constraint_3d_test.cc
View File

@ -1,57 +0,0 @@
/*
* 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

32
cartographer/pose_graph/constraint/constraint.cc
View File

@ -1,32 +0,0 @@
/*
* 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/constraint.h"
namespace cartographer {
namespace pose_graph {
// TODO(pifon): Add a test.
proto::Constraint Constraint::ToProto() const {
proto::Constraint constraint;
constraint.set_id(constraint_id_);
*constraint.mutable_cost_function() = ToCostFunctionProto();
*constraint.mutable_loss_function() = loss_function_.ToProto();
return constraint;
}
} // namespace pose_graph
} // namespace cartographer

63
cartographer/pose_graph/constraint/constraint.h
View File

@ -1,63 +0,0 @@
/*
* 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_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_H_
#include "cartographer/pose_graph/constraint/loss_function/loss_function.h"
#include "cartographer/pose_graph/node/nodes.h"
#include "cartographer/pose_graph/proto/constraint.pb.h"
#include "ceres/problem.h"
#include <string>
namespace cartographer {
namespace pose_graph {
using ConstraintId = std::string;
class Constraint {
public:
Constraint(const ConstraintId& id,
const proto::LossFunction& loss_function_proto)
: constraint_id_(id), loss_function_(loss_function_proto) {}
virtual ~Constraint() = default;
Constraint(const Constraint&) = delete;
Constraint& operator=(const Constraint&) = delete;
proto::Constraint ToProto() const;
const ConstraintId& constraint_id() const { return constraint_id_; }
virtual void AddToSolver(Nodes* nodes, ceres::Problem* problem) const = 0;
protected:
virtual proto::CostFunction ToCostFunctionProto() const = 0;
ceres::LossFunction* ceres_loss() const {
return loss_function_.ceres_loss();
}
private:
ConstraintId constraint_id_;
LossFunction loss_function_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_H_

59
cartographer/pose_graph/constraint/constraint_utils.cc
View File

@ -1,59 +0,0 @@
/*
* 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/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(),
pose->translation_parameterization());
problem->AddParameterBlock(rotation->data(), rotation->size(),
pose->rotation_parameterization());
if (pose->constant()) {
problem->SetParameterBlockConstant(transation->data());
problem->SetParameterBlockConstant(rotation->data());
}
}
void AddImuCalibration(ImuCalibration* imu_calibration,
ceres::Problem* problem) {
auto gravity = imu_calibration->mutable_gravity_constant();
auto orientation = imu_calibration->mutable_orientation();
problem->AddParameterBlock(gravity, 1);
problem->AddParameterBlock(orientation->data(), orientation->size(),
imu_calibration->orientation_parameterization());
if (imu_calibration->constant()) {
problem->SetParameterBlockConstant(gravity);
problem->SetParameterBlockConstant(orientation->data());
}
}
} // namespace pose_graph
} // namespace cartographer

53
cartographer/pose_graph/constraint/constraint_utils.h
View File

@ -1,53 +0,0 @@
/*
* 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/common/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);
template <typename MapType,
typename UniquePtrType = typename MapType::mapped_type,
typename ValueTyper = typename UniquePtrType::element_type>
ValueTyper* FindNodeOrNull(MapType& map, const NodeId& node_id) {
auto node = common::FindOrNull(map, node_id);
return node == nullptr ? nullptr : node->get();
}
#define FIND_NODE_OR_RETURN(node_name, node_id, map, log_message) \
auto node_name = FindNodeOrNull(map, node_id); \
if (node_name == nullptr) { \
LOG(INFO) << log_message; \
return; \
}
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_

47
cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.cc
View File

@ -1,47 +0,0 @@
/*
* 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

75
cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h
View File

@ -1,75 +0,0 @@
/*
* 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_

73
cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d_test.cc
View File

@ -1,73 +0,0 @@
/*
* 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 <array>
#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

48
cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.cc
View File

@ -1,48 +0,0 @@
/*
* 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/interpolated_relative_pose_cost_2d.h"
namespace cartographer {
namespace pose_graph {
InterpolatedRelativePoseCost2D::InterpolatedRelativePoseCost2D(
const proto::InterpolatedRelativePose2D::Parameters& parameters)
: translation_weight_(parameters.translation_weight()),
rotation_weight_(parameters.rotation_weight()),
interpolation_factor_(parameters.interpolation_factor()),
first_T_second_(transform::ToRigid3(parameters.first_t_second())),
gravity_alignment_first_start_(
transform::ToEigen(parameters.gravity_alignment_first_start())),
gravity_alignment_first_end_(
transform::ToEigen(parameters.gravity_alignment_first_end())) {}
proto::InterpolatedRelativePose2D::Parameters
InterpolatedRelativePoseCost2D::ToProto() const {
proto::InterpolatedRelativePose2D::Parameters parameters;
parameters.set_translation_weight(translation_weight_);
parameters.set_rotation_weight(rotation_weight_);
parameters.set_interpolation_factor(interpolation_factor_);
*parameters.mutable_first_t_second() = transform::ToProto(first_T_second_);
*parameters.mutable_gravity_alignment_first_start() =
transform::ToProto(gravity_alignment_first_start_);
*parameters.mutable_gravity_alignment_first_end() =
transform::ToProto(gravity_alignment_first_end_);
return parameters;
}
} // namespace pose_graph
} // namespace cartographer

66
cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h
View File

@ -1,66 +0,0 @@
/*
* 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_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H
#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
#include "cartographer/pose_graph/proto/cost_function.pb.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace pose_graph {
// Provides cost function for relative pose and de/serialization methods.
class InterpolatedRelativePoseCost2D {
public:
explicit InterpolatedRelativePoseCost2D(
const proto::InterpolatedRelativePose2D::Parameters& parameters);
proto::InterpolatedRelativePose2D::Parameters ToProto() const;
template <typename T>
bool operator()(const T* const first_start_pose,
const T* const first_end_pose,
const T* const second_translation,
const T* const second_rotation, T* const e) const {
const std::tuple<std::array<T, 4>, std::array<T, 3>>
interpolated_first_pose = mapping::optimization::InterpolateNodes2D(
first_start_pose, gravity_alignment_first_start_, first_end_pose,
gravity_alignment_first_end_, interpolation_factor_);
const std::array<T, 6> error = mapping::optimization::ScaleError(
mapping::optimization::ComputeUnscaledError(
first_T_second_, std::get<0>(interpolated_first_pose).data(),
std::get<1>(interpolated_first_pose).data(), second_rotation,
second_translation),
translation_weight_, rotation_weight_);
std::copy(std::begin(error), std::end(error), e);
return true;
}
private:
const double translation_weight_;
const double rotation_weight_;
const double interpolation_factor_;
const transform::Rigid3d first_T_second_;
const Eigen::Quaterniond gravity_alignment_first_start_;
const Eigen::Quaterniond gravity_alignment_first_end_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H

76
cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d_test.cc
View File

@ -1,76 +0,0 @@
/*
* 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/interpolated_relative_pose_cost_2d.h"
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
using ::testing::ElementsAre;
using testing::EqualsProto;
using testing::Near;
using testing::ParseProto;
using Position2DType = std::array<double, 3>;
using TranslationType = std::array<double, 3>;
using RotationType = std::array<double, 4>;
using ResidualType = std::array<double, 6>;
constexpr char kParameters[] = R"PROTO(
first_t_second {
translation: { x: 1 y: 2 z: 3 }
rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 }
}
translation_weight: 1
rotation_weight: 2
gravity_alignment_first_start { x: 0.4 y: 0.1 z: 0.3 w: 0.2 }
gravity_alignment_first_end { x: 0.3 y: 0.4 z: 0.2 w: 0.1 }
interpolation_factor: 0.3
)PROTO";
TEST(InterpolatedRelativePoseCost2DTest, SerializesCorrectly) {
const auto parameters =
ParseProto<proto::InterpolatedRelativePose2D::Parameters>(kParameters);
InterpolatedRelativePoseCost2D interpolated_relative_pose_cost_2d(parameters);
const auto actual_proto = interpolated_relative_pose_cost_2d.ToProto();
EXPECT_THAT(actual_proto, EqualsProto(kParameters));
}
TEST(InterpolatedRelativePoseCost2DTest, EvaluatesCorrectly) {
const auto parameters =
ParseProto<proto::InterpolatedRelativePose2D::Parameters>(kParameters);
InterpolatedRelativePoseCost2D interpolated_relative_pose_cost_2d(parameters);
const Position2DType kFirstStartPose{{1., 1., 1.}};
const Position2DType kFirstEndPose{{2., 3., 4.}};
const TranslationType kSecondTranslation{{0., -1., -2.}};
const RotationType kSecondRotation{{.4, .2, .3, .1}};
ResidualType residuals;
EXPECT_TRUE(interpolated_relative_pose_cost_2d(
kFirstStartPose.data(), kFirstEndPose.data(), kSecondTranslation.data(),
kSecondRotation.data(), residuals.data()));
EXPECT_THAT(residuals,
ElementsAre(Near(4.49044), Near(1.8527), Near(3.49511),
Near(-1.93746), Near(3.54854), Near(4.50243)));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

40
cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.cc
View File

@ -1,40 +0,0 @@
/*
* 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/interpolated_relative_pose_cost_3d.h"
namespace cartographer {
namespace pose_graph {
InterpolatedRelativePoseCost3D::InterpolatedRelativePoseCost3D(
const proto::InterpolatedRelativePose3D::Parameters& parameters)
: translation_weight_(parameters.translation_weight()),
rotation_weight_(parameters.rotation_weight()),
interpolation_factor_(parameters.interpolation_factor()),
first_T_second_(transform::ToRigid3(parameters.first_t_second())) {}
proto::InterpolatedRelativePose3D::Parameters
InterpolatedRelativePoseCost3D::ToProto() const {
proto::InterpolatedRelativePose3D::Parameters parameters;
parameters.set_translation_weight(translation_weight_);
parameters.set_rotation_weight(rotation_weight_);
parameters.set_interpolation_factor(interpolation_factor_);
*parameters.mutable_first_t_second() = transform::ToProto(first_T_second_);
return parameters;
}
} // namespace pose_graph
} // namespace cartographer

71
cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h
View File

@ -1,71 +0,0 @@
/*
* 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_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_3D_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_3D_H_
#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
#include "cartographer/pose_graph/proto/cost_function.pb.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace pose_graph {
// Provides cost function for interpolated relative pose in 3d and
// de/serialization methods; only the first pose is linearly interpolated
// between two nodes.
class InterpolatedRelativePoseCost3D {
public:
InterpolatedRelativePoseCost3D(
const proto::InterpolatedRelativePose3D::Parameters& parameters);
proto::InterpolatedRelativePose3D::Parameters ToProto() const;
template <typename T>
bool operator()(const T* const first_start_translation,
const T* const first_start_rotation,
const T* const first_end_translation,
const T* const first_end_rotation,
const T* const second_translation,
const T* const second_rotation, T* const error_out) const {
const std::tuple<std::array<T, 4>, std::array<T, 3>>
interpolated_rotation_and_translation =
mapping::optimization::InterpolateNodes3D(
first_start_rotation, first_start_translation,
first_end_rotation, first_end_translation,
interpolation_factor_);
const std::array<T, 6> error = mapping::optimization::ScaleError(
mapping::optimization::ComputeUnscaledError(
first_T_second_,
std::get<0>(interpolated_rotation_and_translation).data(),
std::get<1>(interpolated_rotation_and_translation).data(),
second_rotation, second_translation),
translation_weight_, rotation_weight_);
std::copy(std::begin(error), std::end(error), error_out);
return true;
}
private:
const double translation_weight_;
const double rotation_weight_;
const double interpolation_factor_;
const transform::Rigid3d first_T_second_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_3D_H_

76
cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d_test.cc
View File

@ -1,76 +0,0 @@
/*
* 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/interpolated_relative_pose_cost_3d.h"
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
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, 6>;
constexpr char kParameters[] = R"PROTO(
first_t_second {
translation: { x: 1 y: 2 z: 3 }
rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 }
}
translation_weight: 1
rotation_weight: 10
interpolation_factor: 0.3
)PROTO";
TEST(InterpolatedRelativePoseCost3DTest, SerializesCorrectly) {
const auto parameters =
ParseProto<proto::InterpolatedRelativePose3D::Parameters>(kParameters);
InterpolatedRelativePoseCost3D interpolated_relative_pose_cost_3d(parameters);
const auto actual_proto = interpolated_relative_pose_cost_3d.ToProto();
EXPECT_THAT(actual_proto, EqualsProto(kParameters));
}
TEST(InterpolatedRelativePoseCost3DTest, EvaluatesCorrectly) {
const auto parameters =
ParseProto<proto::InterpolatedRelativePose3D::Parameters>(kParameters);
InterpolatedRelativePoseCost3D interpolated_relative_pose_cost_3d(parameters);
const PositionType kFirstStartPosition{{1., 1., 1.}};
const RotationType kFirstStartRotation{{1., 1., 1., 1.}};
const PositionType kFirstEndPosition{{2., 3., 4.}};
const RotationType kFirstEndRotation{{1.1, 1.2, 1.3, 1.4}};
const PositionType kSecondPosition{{0., -1., -2.}};
const RotationType kSecondRotation{{.1, .2, .3, .4}};
ResidualType residuals;
EXPECT_TRUE(interpolated_relative_pose_cost_3d(
kFirstStartPosition.data(), kFirstStartRotation.data(),
kFirstEndPosition.data(), kFirstEndRotation.data(),
kSecondPosition.data(), kSecondRotation.data(), residuals.data()));
EXPECT_THAT(residuals,
ElementsAre(Near(8.4594), Near(10.27735), Near(-4.45472),
Near(0.968852), Near(11.96531), Near(3.34254)));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

94
cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.cc
View File

@ -1,94 +0,0 @@
/*
* 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/relative_pose_cost_2d.h"
namespace cartographer {
namespace pose_graph {
RelativePoseCost2D::RelativePoseCost2D(
const proto::RelativePose2D::Parameters& parameters)
: translation_weight_(parameters.translation_weight()),
rotation_weight_(parameters.rotation_weight()),
first_T_second_(transform::ToRigid2(parameters.first_t_second())) {}
proto::RelativePose2D::Parameters RelativePoseCost2D::ToProto() const {
proto::RelativePose2D::Parameters parameters;
parameters.set_translation_weight(translation_weight_);
parameters.set_rotation_weight(rotation_weight_);
*parameters.mutable_first_t_second() = transform::ToProto(first_T_second_);
return parameters;
}
bool RelativePoseCost2D::Evaluate(double const* const* parameters,
double* residuals, double** jacobians) const {
double const* start = parameters[0];
double const* end = parameters[1];
const double cos_start_rotation = cos(start[2]);
const double sin_start_rotation = sin(start[2]);
const double delta_x = end[0] - start[0];
const double delta_y = end[1] - start[1];
residuals[0] =
translation_weight_ *
(first_T_second_.translation().x() -
(cos_start_rotation * delta_x + sin_start_rotation * delta_y));
residuals[1] =
translation_weight_ *
(first_T_second_.translation().y() -
(-sin_start_rotation * delta_x + cos_start_rotation * delta_y));
residuals[2] = rotation_weight_ *
common::NormalizeAngleDifference(
first_T_second_.rotation().angle() - (end[2] - start[2]));
if (jacobians == nullptr) return true;
const double weighted_cos_start_rotation =
translation_weight_ * cos_start_rotation;
const double weighted_sin_start_rotation =
translation_weight_ * sin_start_rotation;
// Jacobians in Ceres are ordered by the parameter blocks:
// jacobian[i] = [(dr_0 / dx_i)^T, ..., (dr_n / dx_i)^T].
if (jacobians[0] != nullptr) {
jacobians[0][0] = weighted_cos_start_rotation;
jacobians[0][1] = weighted_sin_start_rotation;
jacobians[0][2] = weighted_sin_start_rotation * delta_x -
weighted_cos_start_rotation * delta_y;
jacobians[0][3] = -weighted_sin_start_rotation;
jacobians[0][4] = weighted_cos_start_rotation;
jacobians[0][5] = weighted_cos_start_rotation * delta_x +
weighted_sin_start_rotation * delta_y;
jacobians[0][6] = 0;
jacobians[0][7] = 0;
jacobians[0][8] = rotation_weight_;
}
if (jacobians[1] != nullptr) {
jacobians[1][0] = -weighted_cos_start_rotation;
jacobians[1][1] = -weighted_sin_start_rotation;
jacobians[1][2] = 0;
jacobians[1][3] = weighted_sin_start_rotation;
jacobians[1][4] = -weighted_cos_start_rotation;
jacobians[1][5] = 0;
jacobians[1][6] = 0;
jacobians[1][7] = 0;
jacobians[1][8] = -rotation_weight_;
}
return true;
}
} // namespace pose_graph
} // namespace cartographer

51
cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h
View File

@ -1,51 +0,0 @@
/*
* 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_COST_FUNCTION_RELATIVE_POSE_COST_2D_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_RELATIVE_POSE_COST_2D_H_
#include "cartographer/pose_graph/proto/cost_function.pb.h"
#include "cartographer/transform/transform.h"
#include "ceres/sized_cost_function.h"
namespace cartographer {
namespace pose_graph {
class RelativePoseCost2D
: public ceres::SizedCostFunction<3 /* number of residuals */,
3 /* size of first pose */,
3 /* size of second pose */> {
public:
explicit RelativePoseCost2D(
const proto::RelativePose2D::Parameters& parameters);
proto::RelativePose2D::Parameters ToProto() const;
// Parameters are packed as [first_pose_2d, second_pose_2d], where each 2D
// pose is [translation_x, translation_y, rotation].
bool Evaluate(double const* const* parameters, double* residuals,
double** jacobians) const final;
private:
const double translation_weight_;
const double rotation_weight_;
const transform::Rigid2d first_T_second_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_RELATIVE_POSE_COST_2D_H_

117
cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc
View File

@ -1,117 +0,0 @@
/*
* 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/relative_pose_cost_2d.h"
#include "absl/memory/memory.h"
#include "cartographer/testing/test_helpers.h"
#include "ceres/gradient_checker.h"
namespace cartographer {
namespace pose_graph {
namespace {
constexpr int kPoseDimension = 3;
constexpr int kResidualsCount = 3;
constexpr int kParameterBlocksCount = 2;
constexpr int kJacobianColDimension = kResidualsCount * kPoseDimension;
using ::testing::ElementsAre;
using testing::EqualsProto;
using testing::Near;
using testing::ParseProto;
using ResidualType = std::array<double, kResidualsCount>;
using JacobianType = std::array<std::array<double, kJacobianColDimension>,
kParameterBlocksCount>;
constexpr char kParameters[] = R"PROTO(
first_t_second {
translation: { x: 1 y: 1 }
rotation: -2.214297
}
translation_weight: 1
rotation_weight: 10
)PROTO";
class RelativePoseCost2DTest : public ::testing::Test {
public:
RelativePoseCost2DTest()
: relative_pose_cost_2d_(absl::make_unique<RelativePoseCost2D>(
ParseProto<proto::RelativePose2D::Parameters>(kParameters))) {
for (int i = 0; i < kParameterBlocksCount; ++i) {
jacobian_ptrs_[i] = jacobian_[i].data();
}
}
std::pair<const ResidualType&, const JacobianType&>
EvaluateRelativePoseCost2D(
const std::array<const double*, 2>& parameter_blocks) {
relative_pose_cost_2d_->Evaluate(parameter_blocks.data(), residuals_.data(),
jacobian_ptrs_.data());
return std::make_pair(std::cref(residuals_), std::cref(jacobian_));
}
protected:
ResidualType residuals_;
JacobianType jacobian_;
std::array<double*, kParameterBlocksCount> jacobian_ptrs_;
std::unique_ptr<RelativePoseCost2D> relative_pose_cost_2d_;
};
TEST_F(RelativePoseCost2DTest, SerializesCorrectly) {
EXPECT_THAT(relative_pose_cost_2d_->ToProto(), EqualsProto(kParameters));
}
TEST_F(RelativePoseCost2DTest, CheckGradient) {
std::array<double, kPoseDimension> start_pose{{1., 1., 1.}};
std::array<double, kPoseDimension> end_pose{{10., 1., 100.}};
std::array<const double*, kParameterBlocksCount> parameter_blocks{
{start_pose.data(), end_pose.data()}};
using ::ceres::GradientChecker;
GradientChecker gradient_checker(relative_pose_cost_2d_.get(),
{} /* local parameterizations */,
ceres::NumericDiffOptions{});
GradientChecker::ProbeResults probe_results;
gradient_checker.Probe(parameter_blocks.data(),
1e-08 /* relative precision */, &probe_results);
EXPECT_TRUE(probe_results.return_value);
}
TEST_F(RelativePoseCost2DTest, EvaluateRelativePoseCost2D) {
std::array<double, kPoseDimension> start_pose{{1., 1., 1.}};
std::array<double, kPoseDimension> end_pose{{10., 1., 100.}};
std::array<const double*, kParameterBlocksCount> parameter_blocks{
{start_pose.data(), end_pose.data()}};
auto residuals_and_jacobian = EvaluateRelativePoseCost2D(parameter_blocks);
EXPECT_THAT(residuals_and_jacobian.first,
ElementsAre(Near(-3.86272), Near(8.57324), Near(-6.83333)));
EXPECT_THAT(
residuals_and_jacobian.second,
ElementsAre(
ElementsAre(Near(0.540302), Near(0.841471), Near(7.57324),
Near(-0.841471), Near(0.540302), Near(4.86272), Near(0),
Near(0), Near(10)),
ElementsAre(Near(-0.540302), Near(-0.841471), Near(0), Near(0.841471),
Near(-0.540302), Near(0), Near(0), Near(0), Near(-10))));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

37
cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.cc
View File

@ -1,37 +0,0 @@
/*
* 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/relative_pose_cost_3d.h"
namespace cartographer {
namespace pose_graph {
RelativePoseCost3D::RelativePoseCost3D(
const proto::RelativePose3D::Parameters& parameters)
: translation_weight_(parameters.translation_weight()),
rotation_weight_(parameters.rotation_weight()),
first_T_second_(transform::ToRigid3(parameters.first_t_second())) {}
proto::RelativePose3D::Parameters RelativePoseCost3D::ToProto() const {
proto::RelativePose3D::Parameters parameters;
parameters.set_translation_weight(translation_weight_);
parameters.set_rotation_weight(rotation_weight_);
*parameters.mutable_first_t_second() = transform::ToProto(first_T_second_);
return parameters;
}
} // namespace pose_graph
} // namespace cartographer

57
cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h
View File

@ -1,57 +0,0 @@
/*
* 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_RELATIVE_POSE_COST_3D_H
#define CARTOGRAPHER_RELATIVE_POSE_COST_3D_H
#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
#include "cartographer/pose_graph/proto/cost_function.pb.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace pose_graph {
// Provides cost function for relative pose and de/serialization methods.
class RelativePoseCost3D {
public:
explicit RelativePoseCost3D(
const proto::RelativePose3D::Parameters& parameters);
proto::RelativePose3D::Parameters ToProto() const;
template <typename T>
bool operator()(const T* const c_i_translation, const T* const c_i_rotation,
const T* const c_j_translation, const T* const c_j_rotation,
T* const error_out) const {
const std::array<T, 6> error = mapping::optimization::ScaleError(
mapping::optimization::ComputeUnscaledError(
first_T_second_, c_i_rotation, c_i_translation, c_j_rotation,
c_j_translation),
translation_weight_, rotation_weight_);
std::copy(std::begin(error), std::end(error), error_out);
return true;
}
private:
const double translation_weight_;
const double rotation_weight_;
const transform::Rigid3d first_T_second_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_RELATIVE_POSE_COST_3D_H

76
cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d_test.cc
View File

@ -1,76 +0,0 @@
/*
* 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/relative_pose_cost_3d.h"
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
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, 6>;
constexpr char kParameters[] = R"PROTO(
first_t_second {
translation: { x: 1 y: 2 z: 3 }
rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 }
}
translation_weight: 1
rotation_weight: 10
)PROTO";
TEST(RelativePoseCost3DTest, SerializesCorrectly) {
auto relative_pose_parameters =
ParseProto<proto::RelativePose3D::Parameters>(kParameters);
RelativePoseCost3D relative_pose_cost_3d(relative_pose_parameters);
const auto actual_proto = relative_pose_cost_3d.ToProto();
EXPECT_THAT(actual_proto, EqualsProto(kParameters));
}
TEST(RelativePoseCost3DTest, EvaluatesCorrectly) {
auto relative_pose_parameters =
ParseProto<proto::RelativePose3D::Parameters>(kParameters);
RelativePoseCost3D relative_pose_cost_3d(relative_pose_parameters);
const PositionType kPosition1{{1., 1., 1.}};
const RotationType kRotation1{{1., 1., 1., 1.}};
ResidualType residuals;
EXPECT_TRUE(relative_pose_cost_3d(kPosition1.data(), kRotation1.data(),
kPosition1.data(), kRotation1.data(),
residuals.data()));
EXPECT_THAT(residuals, ElementsAre(Near(1), Near(2), Near(3), Near(0),
Near(19.1037), Near(6.3679)));
const PositionType kPosition2{{0., -1., -2.}};
const RotationType kRotation2{{.1, .2, .3, .4}};
EXPECT_TRUE(relative_pose_cost_3d(kPosition1.data(), kRotation1.data(),
kPosition2.data(), kRotation2.data(),
residuals.data()));
EXPECT_THAT(residuals, ElementsAre(Near(6), Near(8), Near(-2), Near(1.03544),
Near(11.38984), Near(3.10632)));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

38
cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.cc
View File

@ -1,38 +0,0 @@
/*
* 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/rotation_cost_3d.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace pose_graph {
RotationCost3D::RotationCost3D(const proto::Rotation3D::Parameters& parameters)
: scaling_factor_(parameters.scaling_factor()),
delta_rotation_imu_frame_(
transform::ToEigen(parameters.delta_rotation_imu_frame())) {}
proto::Rotation3D::Parameters RotationCost3D::ToProto() const {
proto::Rotation3D::Parameters parameters;
parameters.set_scaling_factor(scaling_factor_);
*parameters.mutable_delta_rotation_imu_frame() =
transform::ToProto(delta_rotation_imu_frame_);
return parameters;
}
} // namespace pose_graph
} // namespace cartographer

63
cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h
View File

@ -1,63 +0,0 @@
/*
* 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_ROTATION_COST_3D_H
#define CARTOGRAPHER_ROTATION_COST_3D_H
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/pose_graph/proto/cost_function.pb.h"
namespace cartographer {
namespace pose_graph {
// Provides cost function for 3D rotation and de/serialization methods. The cost
// function penalizes differences between IMU data and optimized orientations.
class RotationCost3D {
public:
explicit RotationCost3D(const proto::Rotation3D::Parameters& parameters);
proto::Rotation3D::Parameters ToProto() const;
// Cost function expecting three quaternions as input and 3D vector as output.
template <typename T>
bool operator()(const T* const start_rotation, const T* const end_rotation,
const T* const imu_calibration, T* residual) const {
const Eigen::Quaternion<T> start(start_rotation[0], start_rotation[1],
start_rotation[2], start_rotation[3]);
const Eigen::Quaternion<T> end(end_rotation[0], end_rotation[1],
end_rotation[2], end_rotation[3]);
const Eigen::Quaternion<T> eigen_imu_calibration(
imu_calibration[0], imu_calibration[1], imu_calibration[2],
imu_calibration[3]);
const Eigen::Quaternion<T> error =
end.conjugate() * start * eigen_imu_calibration *
delta_rotation_imu_frame_.cast<T>() * eigen_imu_calibration.conjugate();
residual[0] = scaling_factor_ * error.x();
residual[1] = scaling_factor_ * error.y();
residual[2] = scaling_factor_ * error.z();
return true;
}
private:
const double scaling_factor_;
const Eigen::Quaterniond delta_rotation_imu_frame_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_ROTATION_COST_3D_H

66
cartographer/pose_graph/constraint/cost_function/rotation_cost_3d_test.cc
View File

@ -1,66 +0,0 @@
/*
* 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/rotation_cost_3d.h"
#include <array>
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
using ::testing::ElementsAre;
using testing::EqualsProto;
using testing::Near;
using testing::ParseProto;
using RotationType = std::array<double, 4>;
using ResidualType = std::array<double, 3>;
constexpr char kParameters[] = R"PROTO(
delta_rotation_imu_frame { x: 0 y: 0.1 z: 0.2 w: 0.3 }
scaling_factor: 0.4
)PROTO";
TEST(RotationCost3DTest, SerializesCorrectly) {
const auto parameters =
ParseProto<proto::Rotation3D::Parameters>(kParameters);
RotationCost3D rotation_cost_3d(parameters);
const auto actual_proto = rotation_cost_3d.ToProto();
EXPECT_THAT(actual_proto, EqualsProto(kParameters));
}
TEST(RotationCost3DTest, EvaluatesCorrectly) {
const auto parameters =
ParseProto<proto::Rotation3D::Parameters>(kParameters);
RotationCost3D rotation_cost_3d(parameters);
const RotationType kStartRotation{{1., 1., 1., 1.}};
const RotationType kEndRotation{{1.1, 1.2, 1.3, 1.4}};
const RotationType kImuCalibration{{0.1, 0.1, 0.1, 0.1}};
ResidualType residuals;
rotation_cost_3d(kStartRotation.data(), kEndRotation.data(),
kImuCalibration.data(), residuals.data());
EXPECT_THAT(residuals,
ElementsAre(Near(0.01536), Near(-0.00256), Near(0.00832)));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

75
cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc
View File

@ -1,75 +0,0 @@
/*
* 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/interpolated_relative_pose_constraint_2d.h"
#include "absl/memory/memory.h"
#include "cartographer/common/utils.h"
#include "cartographer/pose_graph/constraint/constraint_utils.h"
namespace cartographer {
namespace pose_graph {
InterpolatedRelativePoseConstraint2D::InterpolatedRelativePoseConstraint2D(
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
const proto::InterpolatedRelativePose2D& proto)
: Constraint(id, loss_function_proto),
first_start_(proto.first_start()),
first_end_(proto.first_end()),
second_(proto.second()),
cost_(new InterpolatedRelativePoseCost2D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {}
void InterpolatedRelativePoseConstraint2D::AddToSolver(
Nodes* nodes, ceres::Problem* problem) const {
FIND_NODE_OR_RETURN(first_node_start, first_start_, nodes->pose_2d_nodes,
"First node (start) was not found in pose_2d_nodes.");
FIND_NODE_OR_RETURN(first_node_end, first_end_, nodes->pose_2d_nodes,
"First node (end) was not found in pose_2d_nodes.");
FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes,
"Second node was not found in pose_3d_nodes.");
if (first_node_start->constant() && first_node_end->constant() &&
second_node->constant()) {
LOG(INFO) << "All nodes are constant, skipping the constraint.";
return;
}
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(),
second_node->mutable_translation()->data(),
second_node->mutable_rotation()->data());
}
proto::CostFunction InterpolatedRelativePoseConstraint2D::ToCostFunctionProto()
const {
proto::CostFunction cost_function;
auto* interpolated_relative_pose_2d =
cost_function.mutable_interpolated_relative_pose_2d();
*interpolated_relative_pose_2d->mutable_first_start() =
first_start_.ToProto();
*interpolated_relative_pose_2d->mutable_first_end() = first_end_.ToProto();
*interpolated_relative_pose_2d->mutable_second() = second_.ToProto();
*interpolated_relative_pose_2d->mutable_parameters() = cost_->ToProto();
return cost_function;
}
} // namespace pose_graph
} // namespace cartographer

59
cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h
View File

@ -1,59 +0,0 @@
/*
* 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_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_
#include "cartographer/pose_graph/constraint/constraint.h"
#include "cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h"
namespace cartographer {
namespace pose_graph {
class InterpolatedRelativePoseConstraint2D : public Constraint {
public:
InterpolatedRelativePoseConstraint2D(
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
const proto::InterpolatedRelativePose2D& proto);
void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final;
protected:
proto::CostFunction ToCostFunctionProto() const final;
private:
// clang-format off
using AutoDiffFunction = ceres::AutoDiffCostFunction<
InterpolatedRelativePoseCost2D,
6 /* residuals */,
3 /* 2d pose variables of first start pose */,
3 /* 2d pose variables of first end pose */,
3 /* translation of second pose */,
4 /* rotation of second pose */>;
// clang-format on
NodeId first_start_;
NodeId first_end_;
NodeId second_;
// The cost function is owned by the ceres cost function.
InterpolatedRelativePoseCost2D* const cost_;
std::unique_ptr<AutoDiffFunction> ceres_cost_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_

62
cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d_test.cc
View File

@ -1,62 +0,0 @@
/*
* 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/interpolated_relative_pose_constraint_2d.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: "narf"
cost_function {
interpolated_relative_pose_2d {
first_start { object_id: "node0_start" }
first_end { object_id: "node0_end" }
second { object_id: "node1" }
parameters {
first_t_second {
translation: { x: 1 y: 2 z: 3 }
rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 }
}
translation_weight: 0.5
rotation_weight: 1.0
gravity_alignment_first_start { x: 0.4 y: 0.2 z: 0.3 w: 0.2 }
gravity_alignment_first_end { x: 0.3 y: 0.4 z: 0.2 w: 0.1 }
interpolation_factor: 0.3
}
}
}
loss_function { quadratic_loss {} }
)PROTO";
TEST(InterpolatedRelativePostConstraint2DTest, SerializesCorrectly) {
const auto proto = ParseProto<proto::Constraint>(kConstraint);
InterpolatedRelativePoseConstraint2D constraint(
proto.id(), proto.loss_function(),
proto.cost_function().interpolated_relative_pose_2d());
const auto actual_proto = constraint.ToProto();
EXPECT_THAT(actual_proto, EqualsProto(kConstraint));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

77
cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc
View File

@ -1,77 +0,0 @@
/*
* 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/interpolated_relative_pose_constraint_3d.h"
#include "absl/memory/memory.h"
#include "cartographer/common/utils.h"
#include "cartographer/pose_graph/constraint/constraint_utils.h"
namespace cartographer {
namespace pose_graph {
InterpolatedRelativePoseConstraint3D::InterpolatedRelativePoseConstraint3D(
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
const proto::InterpolatedRelativePose3D& proto)
: Constraint(id, loss_function_proto),
first_start_(proto.first_start()),
first_end_(proto.first_end()),
second_(proto.second()),
cost_(new InterpolatedRelativePoseCost3D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {}
void InterpolatedRelativePoseConstraint3D::AddToSolver(
Nodes* nodes, ceres::Problem* problem) const {
FIND_NODE_OR_RETURN(first_node_start, first_start_, nodes->pose_3d_nodes,
"First node (start) was not found in pose_3d_nodes.");
FIND_NODE_OR_RETURN(first_node_end, first_end_, nodes->pose_3d_nodes,
"First node (end) was not found in pose_3d_nodes.");
FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes,
"Second node was not found in pose_3d_nodes.");
if (first_node_start->constant() && first_node_end->constant() &&
second_node->constant()) {
LOG(INFO) << "All nodes are constant, skipping the constraint.";
return;
}
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(),
first_node_end->mutable_rotation()->data(),
second_node->mutable_translation()->data(),
second_node->mutable_rotation()->data());
}
proto::CostFunction InterpolatedRelativePoseConstraint3D::ToCostFunctionProto()
const {
proto::CostFunction cost_function;
auto* interpolated_relative_pose_3d =
cost_function.mutable_interpolated_relative_pose_3d();
*interpolated_relative_pose_3d->mutable_first_start() =
first_start_.ToProto();
*interpolated_relative_pose_3d->mutable_first_end() = first_end_.ToProto();
*interpolated_relative_pose_3d->mutable_second() = second_.ToProto();
*interpolated_relative_pose_3d->mutable_parameters() = cost_->ToProto();
return cost_function;
}
} // namespace pose_graph
} // namespace cartographer

61
cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h
View File

@ -1,61 +0,0 @@
/*
* 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_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_3D_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_3D_H_
#include "cartographer/pose_graph/constraint/constraint.h"
#include "cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h"
namespace cartographer {
namespace pose_graph {
class InterpolatedRelativePoseConstraint3D : public Constraint {
public:
InterpolatedRelativePoseConstraint3D(
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
const proto::InterpolatedRelativePose3D& proto);
void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final;
protected:
proto::CostFunction ToCostFunctionProto() const final;
private:
// clang-format off
using AutoDiffFunction = ceres::AutoDiffCostFunction<
InterpolatedRelativePoseCost3D,
6 /* number of residuals */,
3 /* translation of first start pose */,
4 /* rotation of first start pose */,
3 /* translation of first end pose */,
4 /* rotation of first end pose */,
3 /* translation of second pose */,
4 /* rotation of second pose */>;
// clang-format on
NodeId first_start_;
NodeId first_end_;
NodeId second_;
// The cost function is owned by the ceres cost function.
InterpolatedRelativePoseCost3D* const cost_;
std::unique_ptr<AutoDiffFunction> ceres_cost_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_3D_H_

67
cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d_test.cc
View File

@ -1,67 +0,0 @@
/*
* 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/interpolated_relative_pose_constraint_3d.h"
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
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 kConstraint[] = R"PROTO(
id: "narf"
cost_function {
interpolated_relative_pose_3d {
first_start { object_id: "node0_start" }
first_end { object_id: "node0_end" }
second { object_id: "node1" }
parameters {
first_t_second {
translation: { x: 1 y: 2 z: 3 }
rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 }
}
translation_weight: 0.5
rotation_weight: 1.0
interpolation_factor: 0.3
}
}
}
loss_function { quadratic_loss {} }
)PROTO";
TEST(InterpolatedRelativePostConstraint3DTest, SerializesCorrectly) {
const auto proto = ParseProto<proto::Constraint>(kConstraint);
InterpolatedRelativePoseConstraint3D constraint(
proto.id(), proto.loss_function(),
proto.cost_function().interpolated_relative_pose_3d());
const auto actual_proto = constraint.ToProto();
EXPECT_THAT(actual_proto, EqualsProto(kConstraint));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

44
cartographer/pose_graph/constraint/loss_function/loss_function.cc
View File

@ -1,44 +0,0 @@
/*
* 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/loss_function/loss_function.h"
#include "absl/memory/memory.h"
namespace cartographer {
namespace pose_graph {
namespace {
std::unique_ptr<ceres::LossFunction> CeresLossFromProto(
const proto::LossFunction& proto) {
switch (proto.Type_case()) {
case proto::LossFunction::kHuberLoss:
return absl::make_unique<ceres::HuberLoss>(proto.huber_loss().scale());
case proto::LossFunction::kQuadraticLoss:
return nullptr;
default:
LOG(FATAL) << "The loss function is not specified.";
return nullptr;
}
}
} // namespace
LossFunction::LossFunction(const proto::LossFunction& proto)
: proto_(proto), ceres_loss_(CeresLossFromProto(proto_)) {}
} // namespace pose_graph
} // namespace cartographer

44
cartographer/pose_graph/constraint/loss_function/loss_function.h
View File

@ -1,44 +0,0 @@
/*
* 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_LOSS_FUNCTION_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_LOSS_FUNCTION_H_
#include <memory>
#include "cartographer/pose_graph/proto/loss_function.pb.h"
#include "ceres/loss_function.h"
namespace cartographer {
namespace pose_graph {
class LossFunction {
public:
explicit LossFunction(const proto::LossFunction& proto);
const proto::LossFunction& ToProto() const { return proto_; }
ceres::LossFunction* ceres_loss() const { return ceres_loss_.get(); }
private:
const proto::LossFunction proto_;
const std::unique_ptr<ceres::LossFunction> ceres_loss_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_LOSS_FUNCTION_H_

45
cartographer/pose_graph/constraint/loss_function/loss_function_test.cc
View File

@ -1,45 +0,0 @@
/*
* 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/loss_function/loss_function.h"
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
using testing::ParseProto;
TEST(LossFunctionTest, ConstructQuadraticLoss) {
LossFunction quadratic_loss(
ParseProto<proto::LossFunction>(R"(quadratic_loss: {})"));
EXPECT_EQ(nullptr, quadratic_loss.ceres_loss());
}
TEST(LossFunctionTest, ConstructHuberLoss) {
LossFunction huber_loss(
ParseProto<proto::LossFunction>(R"(huber_loss: { scale: 0.5 })"));
EXPECT_NE(nullptr, dynamic_cast<ceres::HuberLoss*>(huber_loss.ceres_loss()));
}
TEST(LossFunctionDeathTest, FailToConstructUnspecifiedLoss) {
EXPECT_DEATH(LossFunction(proto::LossFunction{}), "");
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

72
cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc
View File

@ -1,72 +0,0 @@
/*
* 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/relative_pose_constraint_2d.h"
#include "absl/memory/memory.h"
#include "cartographer/common/utils.h"
#include "cartographer/pose_graph/constraint/constraint_utils.h"
namespace cartographer {
namespace pose_graph {
RelativePoseConstraint2D::RelativePoseConstraint2D(
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
const proto::RelativePose2D& proto)
: Constraint(id, loss_function_proto),
first_(proto.first()),
second_(proto.second()),
ceres_cost_(absl::make_unique<RelativePoseCost2D>(proto.parameters())) {}
// TODO(pifon): Add a test.
void RelativePoseConstraint2D::AddToSolver(Nodes* nodes,
ceres::Problem* problem) const {
FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_2d_nodes,
"First node was not found in pose_2d_nodes.");
FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_2d_nodes,
"Second node was not found in pose_2d_nodes.");
if (first_node->constant() && second_node->constant()) {
LOG(INFO) << "Both nodes are constant, skipping the constraint.";
return;
}
auto first_pose = first_node->mutable_pose_2d();
problem->AddParameterBlock(first_pose->data(), first_pose->size());
if (first_node->constant()) {
problem->SetParameterBlockConstant(first_pose->data());
}
auto second_pose = second_node->mutable_pose_2d();
problem->AddParameterBlock(second_pose->data(), second_pose->size());
if (second_node->constant()) {
problem->SetParameterBlockConstant(second_pose->data());
}
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), first_pose->data(),
second_pose->data());
}
proto::CostFunction RelativePoseConstraint2D::ToCostFunctionProto() const {
proto::CostFunction cost_function;
auto* relative_pose_2d = cost_function.mutable_relative_pose_2d();
*relative_pose_2d->mutable_first() = first_.ToProto();
*relative_pose_2d->mutable_second() = second_.ToProto();
*relative_pose_2d->mutable_parameters() = ceres_cost_->ToProto();
return cost_function;
}
} // namespace pose_graph
} // namespace cartographer

46
cartographer/pose_graph/constraint/relative_pose_constraint_2d.h
View File

@ -1,46 +0,0 @@
/*
* 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_RELATIVE_POSE_CONSTRAINT_2D_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_2D_H_
#include "cartographer/pose_graph/constraint/constraint.h"
#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h"
namespace cartographer {
namespace pose_graph {
class RelativePoseConstraint2D : public Constraint {
public:
RelativePoseConstraint2D(const ConstraintId& id,
const proto::LossFunction& loss_function_proto,
const proto::RelativePose2D& proto);
void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final;
protected:
proto::CostFunction ToCostFunctionProto() const final;
private:
NodeId first_;
NodeId second_;
std::unique_ptr<RelativePoseCost2D> ceres_cost_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_2D_H_

58
cartographer/pose_graph/constraint/relative_pose_constraint_2d_test.cc
View File

@ -1,58 +0,0 @@
/*
* 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/relative_pose_constraint_2d.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: "narf"
cost_function {
relative_pose_2d {
first { object_id: "node0" }
second { object_id: "node1" }
parameters {
first_t_second {
translation: { x: 1 y: 1 }
rotation: -2.214297
}
translation_weight: 1
rotation_weight: 10
}
}
}
loss_function { quadratic_loss {} }
)PROTO";
TEST(RelativePoseConstraint2DTest, SerializesCorrectly) {
const auto proto = ParseProto<proto::Constraint>(kConstraint);
RelativePoseConstraint2D constraint(proto.id(), proto.loss_function(),
proto.cost_function().relative_pose_2d());
const auto actual_proto = constraint.ToProto();
EXPECT_THAT(actual_proto, EqualsProto(kConstraint));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

66
cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc
View File

@ -1,66 +0,0 @@
/*
* 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/relative_pose_constraint_3d.h"
#include "absl/memory/memory.h"
#include "cartographer/common/utils.h"
#include "cartographer/pose_graph/constraint/constraint_utils.h"
namespace cartographer {
namespace pose_graph {
RelativePoseConstraint3D::RelativePoseConstraint3D(
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
const proto::RelativePose3D& proto)
: Constraint(id, loss_function_proto),
first_(proto.first()),
second_(proto.second()),
cost_(new RelativePoseCost3D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {}
void RelativePoseConstraint3D::AddToSolver(Nodes* nodes,
ceres::Problem* problem) const {
FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes,
"First node was not found in pose_3d_nodes.");
FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes,
"Second node was not found in pose_3d_nodes.");
if (first_node->constant() && second_node->constant()) {
LOG(INFO) << "Both nodes are constant, skipping the constraint.";
return;
}
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(),
second_node->mutable_rotation()->data());
}
proto::CostFunction RelativePoseConstraint3D::ToCostFunctionProto() const {
proto::CostFunction cost_function;
auto* relative_pose_3d = cost_function.mutable_relative_pose_3d();
*relative_pose_3d->mutable_first() = first_.ToProto();
*relative_pose_3d->mutable_second() = second_.ToProto();
*relative_pose_3d->mutable_parameters() = cost_->ToProto();
return cost_function;
}
} // namespace pose_graph
} // namespace cartographer

55
cartographer/pose_graph/constraint/relative_pose_constraint_3d.h
View File

@ -1,55 +0,0 @@
/*
* 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_RELATIVE_POSE_CONSTRAINT_3D_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_
#include "cartographer/pose_graph/constraint/constraint.h"
#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h"
namespace cartographer {
namespace pose_graph {
class RelativePoseConstraint3D : public Constraint {
public:
RelativePoseConstraint3D(const ConstraintId& id,
const proto::LossFunction& loss_function_proto,
const proto::RelativePose3D& proto);
void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final;
protected:
proto::CostFunction ToCostFunctionProto() const final;
private:
using AutoDiffFunction =
ceres::AutoDiffCostFunction<RelativePoseCost3D,
6 /* number of residuals */,
3 /* translation variables first pose */,
4 /* rotation variables first pose*/,
3 /* translation variables second pose */,
4 /* rotation variables second pose*/>;
NodeId first_;
NodeId second_;
// The cost function is owned by the ceres cost function.
RelativePoseCost3D* const cost_;
std::unique_ptr<AutoDiffFunction> ceres_cost_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_

58
cartographer/pose_graph/constraint/relative_pose_constraint_3d_test.cc
View File

@ -1,58 +0,0 @@
/*
* 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/relative_pose_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: "narf"
cost_function {
relative_pose_3d {
first { object_id: "node0" }
second { object_id: "node1" }
parameters {
first_t_second {
translation: { x: 1 y: 2 z: 3 }
rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 }
}
translation_weight: 1
rotation_weight: 10
}
}
}
loss_function { quadratic_loss {} }
)PROTO";
TEST(RelativePoseConstraint2DTest, SerializesCorrectly) {
const auto proto = ParseProto<proto::Constraint>(kConstraint);
RelativePoseConstraint3D constraint(proto.id(), proto.loss_function(),
proto.cost_function().relative_pose_3d());
const auto actual_proto = constraint.ToProto();
EXPECT_THAT(actual_proto, EqualsProto(kConstraint));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

91
cartographer/pose_graph/constraint/rotation_constraint_3d.cc
View File

@ -1,91 +0,0 @@
/*
* 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/rotation_constraint_3d.h"
#include "absl/memory/memory.h"
#include "cartographer/pose_graph/constraint/constraint_utils.h"
#include "cartographer/common/utils.h"
namespace cartographer {
namespace pose_graph {
namespace {
void AddRotation3D(Pose3D* pose, ceres::Problem* problem) {
auto rotation = pose->mutable_rotation();
problem->AddParameterBlock(rotation->data(), rotation->size());
if (pose->constant()) {
problem->SetParameterBlockConstant(rotation->data());
}
}
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(
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
const proto::Rotation3D& proto)
: Constraint(id, loss_function_proto),
first_(proto.first()),
second_(proto.second()),
imu_calibration_(proto.imu_calibration()),
cost_(new RotationCost3D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {}
void RotationContraint3D::AddToSolver(Nodes* nodes,
ceres::Problem* problem) const {
FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes,
"First node was not found in pose_3d_nodes.");
FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes,
"Second node was not found in pose_3d_nodes.");
FIND_NODE_OR_RETURN(imu_node, imu_calibration_, nodes->imu_calibration_nodes,
"Imu calibration node was not found.");
if (first_node->constant() && second_node->constant() &&
imu_node->constant()) {
LOG(INFO) << "Both nodes are constant, skipping the constraint.";
return;
}
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_node->mutable_orientation()->data());
}
proto::CostFunction RotationContraint3D::ToCostFunctionProto() const {
proto::CostFunction cost_function;
auto* rotation_3d = cost_function.mutable_rotation_3d();
*rotation_3d->mutable_first() = first_.ToProto();
*rotation_3d->mutable_second() = second_.ToProto();
*rotation_3d->mutable_imu_calibration() = imu_calibration_.ToProto();
*rotation_3d->mutable_parameters() = cost_->ToProto();
return cost_function;
}
} // namespace pose_graph
} // namespace cartographer

52
cartographer/pose_graph/constraint/rotation_constraint_3d.h
View File

@ -1,52 +0,0 @@
/*
* 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_ROTATION_CONSTRAINT_3D_H_
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ROTATION_CONSTRAINT_3D_H_
#include "cartographer/pose_graph/constraint/constraint.h"
#include "cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h"
namespace cartographer {
namespace pose_graph {
class RotationContraint3D : public Constraint {
public:
RotationContraint3D(const ConstraintId& id,
const proto::LossFunction& loss_function_proto,
const proto::Rotation3D& proto);
void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final;
protected:
proto::CostFunction ToCostFunctionProto() const final;
private:
using AutoDiffFunction = ceres::AutoDiffCostFunction<
RotationCost3D, 3 /* number of residuals */, 4 /* rotation first pose */,
4 /* rotation second pose */, 4 /* imu calibration */>;
NodeId first_;
NodeId second_;
NodeId imu_calibration_;
// The cost function is owned by the ceres cost function.
RotationCost3D* const cost_;
std::unique_ptr<AutoDiffFunction> ceres_cost_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ROTATION_CONSTRAINT_3D_H_

54
cartographer/pose_graph/constraint/rotation_constraint_3d_test.cc
View File

@ -1,54 +0,0 @@
/*
* 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/rotation_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: "narf"
cost_function {
rotation_3d {
first { object_id: "node0" }
second { object_id: "node1" }
imu_calibration { object_id: "imu_node" }
parameters {
delta_rotation_imu_frame { x: 0 y: 0.1 z: 0.2 w: 0.3 }
scaling_factor: 0.4
}
}
}
loss_function { quadratic_loss {} }
)PROTO";
TEST(RotationConstraint3DTest, SerializesCorrectly) {
const auto proto = ParseProto<proto::Constraint>(kConstraint);
RotationContraint3D constraint(proto.id(), proto.loss_function(),
proto.cost_function().rotation_3d());
const auto actual_proto = constraint.ToProto();
EXPECT_THAT(actual_proto, EqualsProto(kConstraint));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

50
cartographer/pose_graph/node/imu_calibration.cc
View File

@ -1,50 +0,0 @@
/*
* 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/node/imu_calibration.h"
namespace cartographer {
namespace pose_graph {
ImuCalibration::ImuCalibration(const NodeId& node_id, bool constant,
const proto::ImuCalibration& imu)
: Node(node_id, constant),
gravity_constant_(imu.gravity_constant()),
orientation_{{imu.orientation().x(), imu.orientation().y(),
imu.orientation().z(), imu.orientation().w()}},
orientation_parameterization_(imu.orientation_parameterization()) {}
proto::Parameters ImuCalibration::ToParametersProto() const {
proto::Parameters parameters;
auto* imu_calibration = parameters.mutable_imu_calibration();
imu_calibration->set_gravity_constant(gravity_constant_);
// TODO(pifon): Use a common method to convert from Eigen::Quaterniond to
// proto. Probably, the one defined in transform.h.
auto* orientation = imu_calibration->mutable_orientation();
orientation->set_x(orientation_[0]);
orientation->set_y(orientation_[1]);
orientation->set_z(orientation_[2]);
orientation->set_w(orientation_[3]);
imu_calibration->set_orientation_parameterization(
orientation_parameterization_.ToProto());
return parameters;
}
} // namespace pose_graph
} // namespace cartographer

57
cartographer/pose_graph/node/imu_calibration.h
View File

@ -1,57 +0,0 @@
/*
* 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_NODE_IMU_CALIBRATION_H_
#define CARTOGRAPHER_POSE_GRAPH_NODE_IMU_CALIBRATION_H_
#include "cartographer/pose_graph/node/node.h"
#include <array>
#include "cartographer/pose_graph/node/parameterization/parameterization.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace pose_graph {
class ImuCalibration : public Node {
public:
ImuCalibration(const NodeId& node_id, bool constant,
const proto::ImuCalibration& imu_calibration);
double* mutable_gravity_constant() { return &gravity_constant_; }
double gravity_constant() const { return gravity_constant_; }
std::array<double, 4>* mutable_orientation() { return &orientation_; }
const std::array<double, 4>& orientation() const { return orientation_; }
ceres::LocalParameterization* orientation_parameterization() const {
return orientation_parameterization_.ceres_parameterization();
}
protected:
proto::Parameters ToParametersProto() const final;
private:
double gravity_constant_;
std::array<double, 4> orientation_;
Parameterization orientation_parameterization_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_NODE_IMU_CALIBRATION_H_

51
cartographer/pose_graph/node/imu_calibration_test.cc
View File

@ -1,51 +0,0 @@
/*
* 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/node/imu_calibration.h"
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
using testing::ParseProto;
constexpr char kExpectedNode[] = R"PROTO(
id { object_id: "accelerometer" timestamp: 1 }
constant: true
parameters {
imu_calibration {
gravity_constant: 10
orientation: { w: 0 x: 1 y: 2 z: 3 }
orientation_parameterization: YAW_ONLY
}
}
)PROTO";
TEST(Pose3DTest, ToProto) {
ImuCalibration imu_calibration({"accelerometer", common::FromUniversal(1)},
true, ParseProto<proto::ImuCalibration>(R"(
gravity_constant: 10
orientation: { w: 0 x: 1 y: 2 z: 3 }
orientation_parameterization: YAW_ONLY
)"));
EXPECT_THAT(imu_calibration.ToProto(), testing::EqualsProto(kExpectedNode));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

31
cartographer/pose_graph/node/node.cc
View File

@ -1,31 +0,0 @@
/*
* 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/node/node.h"
namespace cartographer {
namespace pose_graph {
proto::Node Node::ToProto() const {
proto::Node node;
node.set_constant(constant_);
*node.mutable_id() = node_id_.ToProto();
*node.mutable_parameters() = ToParametersProto();
return node;
}
} // namespace pose_graph
} // namespace cartographer

53
cartographer/pose_graph/node/node.h
View File

@ -1,53 +0,0 @@
/*
* 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_NODE_NODE_H_
#define CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_
#include "cartographer/pose_graph/node/node_id.h"
#include "cartographer/pose_graph/proto/node.pb.h"
#include <vector>
namespace cartographer {
namespace pose_graph {
class Node {
public:
explicit Node(const NodeId& id, bool constant)
: node_id_(id), constant_(constant) {}
~Node() = default;
proto::Node ToProto() const;
const NodeId node_id() const { return node_id_; }
bool constant() const { return constant_; }
void set_constant(bool constant) { constant_ = constant; }
protected:
virtual proto::Parameters ToParametersProto() const = 0;
private:
NodeId node_id_;
bool constant_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_

42
cartographer/pose_graph/node/node_id.cc
View File

@ -1,42 +0,0 @@
/*
* 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/node/node_id.h"
namespace cartographer {
namespace pose_graph {
NodeId::NodeId(const std::string& object_id, common::Time time)
: object_id(object_id), time(time) {}
NodeId::NodeId(const std::string& object_id, const std::string& group_id,
common::Time time)
: object_id(object_id), group_id(group_id), time(time) {}
NodeId::NodeId(const proto::NodeId& node_id)
: NodeId(node_id.object_id(), node_id.group_id(),
common::FromUniversal(node_id.timestamp())) {}
proto::NodeId NodeId::ToProto() const {
proto::NodeId node_id;
node_id.set_object_id(object_id);
node_id.set_group_id(group_id);
node_id.set_timestamp(common::ToUniversal(time));
return node_id;
}
} // namespace pose_graph
} // namespace cartographer

63
cartographer/pose_graph/node/node_id.h
View File

@ -1,63 +0,0 @@
/*
* 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_NODE_NODE_ID_
#define CARTOGRAPHER_POSE_GRAPH_NODE_NODE_ID_
#include <ostream>
#include <string>
#include "cartographer/common/time.h"
#include "cartographer/pose_graph/proto/node.pb.h"
namespace cartographer {
namespace pose_graph {
struct NodeId {
NodeId(const std::string& object_id, common::Time time);
NodeId(const std::string& object_id, const std::string& group_id,
common::Time time);
explicit NodeId(const proto::NodeId& node_id);
// Object refers to dynamic/static objects, e.g. robot/landmark/submap poses,
// IMU calibration, etc.
std::string object_id;
// Id of the group to which the node belongs, e.g. "submap".
std::string group_id;
// Time associated with the object's pose.
common::Time time;
proto::NodeId ToProto() const;
};
inline bool operator<(const NodeId& lhs, const NodeId& rhs) {
return std::forward_as_tuple(lhs.object_id, lhs.group_id, lhs.time) <
std::forward_as_tuple(rhs.object_id, rhs.group_id, rhs.time);
}
inline std::ostream& operator<<(std::ostream& os, const NodeId& id) {
std::string group_message;
if (!id.group_id.empty()) {
group_message = ", group_id: " + id.group_id;
}
return os << "(object_id: " << id.object_id << group_message
<< ", time: " << id.time << ")";
}
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_NODE_NODE_ID_

62
cartographer/pose_graph/node/node_id_test.cc
View File

@ -1,62 +0,0 @@
/*
* 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/node/node_id.h"
#include <sstream>
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
TEST(NodeIdTest, FromProto) {
proto::NodeId proto;
proto.set_object_id("some_object");
proto.set_group_id("submap");
proto.set_timestamp(1);
NodeId node_id(proto);
EXPECT_EQ(node_id.object_id, "some_object");
EXPECT_EQ(node_id.group_id, "submap");
EXPECT_EQ(common::ToUniversal(node_id.time), 1);
}
TEST(NodeIdTest, ToProto) {
NodeId node_id{"some_object", "submap", common::FromUniversal(1)};
EXPECT_THAT(node_id.ToProto(),
testing::EqualsProto(
"object_id: 'some_object' group_id: 'submap' timestamp: 1"));
}
TEST(NodeIdTest, ToStringWithoutGroupId) {
std::stringstream ss;
ss << NodeId{"some_object", common::FromUniversal(1)};
EXPECT_EQ("(object_id: some_object, time: 1)", ss.str());
}
TEST(NodeIdTest, ToStringWithGroupId) {
std::stringstream ss;
ss << NodeId{"some_object", "submap", common::FromUniversal(1)};
EXPECT_EQ("(object_id: some_object, group_id: submap, time: 1)", ss.str());
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

39
cartographer/pose_graph/node/nodes.h
View File

@ -1,39 +0,0 @@
/*
* 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_NODE_NODES_H_
#define CARTOGRAPHER_POSE_GRAPH_NODE_NODES_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 <map>
namespace cartographer {
namespace pose_graph {
struct Nodes {
// TODO(pifon): Migrate to Swiss Tables when they are out.
std::map<NodeId, std::unique_ptr<Pose2D>> pose_2d_nodes;
std::map<NodeId, std::unique_ptr<Pose3D>> pose_3d_nodes;
std::map<NodeId, std::unique_ptr<ImuCalibration>> imu_calibration_nodes;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_NODE_NODES_H_

61
cartographer/pose_graph/node/parameterization/parameterization.cc
View File

@ -1,61 +0,0 @@
/*
* 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/node/parameterization/parameterization.h"
#include "absl/memory/memory.h"
#include "cartographer/common/math.h"
#include "cartographer/mapping/internal/3d/rotation_parameterization.h"
#include "ceres/autodiff_local_parameterization.h"
#include "ceres/rotation.h"
namespace cartographer {
namespace pose_graph {
namespace {
using absl::make_unique;
using ceres::AutoDiffLocalParameterization;
using ceres::LocalParameterization;
// TODO(pifon): Check if the functors are compatible with our quaternions. Test!
std::unique_ptr<LocalParameterization> CeresParameterizationFromProto(
const proto::Parameterization& parameterization) {
switch (parameterization) {
case (proto::Parameterization::NONE):
return nullptr;
case (proto::Parameterization::YAW_ONLY):
return make_unique<AutoDiffLocalParameterization<
mapping::YawOnlyQuaternionPlus, 4, 1>>();
case (proto::Parameterization::YAW_CONSTANT):
return make_unique<AutoDiffLocalParameterization<
mapping::ConstantYawQuaternionPlus, 4, 1>>();
case (proto::Parameterization::FIX_Z):
return make_unique<ceres::SubsetParameterization>(
3, /* constant parameters */ std::vector<int>{2});
default:
LOG(FATAL) << "Parameterization is not known.";
}
return nullptr;
}
} // namespace
Parameterization::Parameterization(const proto::Parameterization& proto)
: proto_(proto),
ceres_parameterization_(CeresParameterizationFromProto(proto_)) {}
} // namespace pose_graph
} // namespace cartographer

44
cartographer/pose_graph/node/parameterization/parameterization.h
View File

@ -1,44 +0,0 @@
/*
* 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_NODE_PARAMETERIZATION_PARAMETERIZATION_H_
#define CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_
#include "cartographer/pose_graph/proto/node.pb.h"
#include "ceres/local_parameterization.h"
namespace cartographer {
namespace pose_graph {
class Parameterization {
public:
explicit Parameterization(const proto::Parameterization& proto);
const proto::Parameterization& ToProto() const { return proto_; }
ceres::LocalParameterization* ceres_parameterization() const {
return ceres_parameterization_.get();
}
private:
const proto::Parameterization proto_;
const std::unique_ptr<ceres::LocalParameterization> ceres_parameterization_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_

47
cartographer/pose_graph/node/pose_2d.cc
View File

@ -1,47 +0,0 @@
/*
* 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/node/pose_2d.h"
namespace cartographer {
namespace pose_graph {
namespace {
constexpr size_t kXIndex = 0;
constexpr size_t kYIndex = 1;
constexpr size_t kRotationIndex = 2;
} // namespace
Pose2D::Pose2D(const NodeId& node_id, bool constant,
const proto::Pose2D& pose_2d)
: Node(node_id, constant),
pose_2d_{{pose_2d.translation().x(), pose_2d.translation().y(),
pose_2d.rotation()}} {}
proto::Parameters Pose2D::ToParametersProto() const {
proto::Parameters parameters;
auto* pose_2d = parameters.mutable_pose_2d();
pose_2d->set_rotation(pose_2d_[kRotationIndex]);
auto* translation = pose_2d->mutable_translation();
translation->set_x(pose_2d_[kXIndex]);
translation->set_y(pose_2d_[kYIndex]);
return parameters;
}
} // namespace pose_graph
} // namespace cartographer

46
cartographer/pose_graph/node/pose_2d.h
View File

@ -1,46 +0,0 @@
/*
* 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_NODE_POSE_2D_H_
#define CARTOGRAPHER_POSE_GRAPH_NODE_POSE_2D_H_
#include "cartographer/pose_graph/node/node.h"
#include <array>
#include "Eigen/Core"
namespace cartographer {
namespace pose_graph {
class Pose2D : public Node {
public:
Pose2D(const NodeId& node_id, bool constant, const proto::Pose2D& pose_2d);
std::array<double, 3>* mutable_pose_2d() { return &pose_2d_; }
const std::array<double, 3>& pose_2d() const { return pose_2d_; }
protected:
proto::Parameters ToParametersProto() const final;
private:
std::array<double, 3> pose_2d_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_NODE_POSE_2D_H_

49
cartographer/pose_graph/node/pose_2d_test.cc
View File

@ -1,49 +0,0 @@
/*
* 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/node/pose_2d.h"
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
using testing::ParseProto;
constexpr char kExpectedNode[] = R"PROTO(
id { object_id: "flat_world" timestamp: 1 }
constant: true
parameters {
pose_2d {
translation { x: 1 y: 2 }
rotation: 5
}
}
)PROTO";
TEST(Pose2DTest, ToProto) {
Pose2D pose_2d({"flat_world", common::FromUniversal(1)}, true,
ParseProto<proto::Pose2D>(R"(
translation { x: 1 y: 2 }
rotation: 5
)"));
EXPECT_THAT(pose_2d.ToProto(), testing::EqualsProto(kExpectedNode));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

54
cartographer/pose_graph/node/pose_3d.cc
View File

@ -1,54 +0,0 @@
/*
* 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/node/pose_3d.h"
namespace cartographer {
namespace pose_graph {
Pose3D::Pose3D(const NodeId& node_id, bool constant,
const proto::Pose3D& pose_3d)
: Node(node_id, constant),
translation_{{pose_3d.translation().x(), pose_3d.translation().y(),
pose_3d.translation().z()}},
translation_parameterization_(pose_3d.translation_parameterization()),
rotation_{{pose_3d.rotation().x(), pose_3d.rotation().y(),
pose_3d.rotation().z(), pose_3d.rotation().w()}},
rotation_parameterization_(pose_3d.rotation_parameterization()) {}
proto::Parameters Pose3D::ToParametersProto() const {
proto::Parameters parameters;
auto* pose_3d = parameters.mutable_pose_3d();
auto* translation = pose_3d->mutable_translation();
translation->set_x(translation_[0]);
translation->set_y(translation_[1]);
translation->set_z(translation_[2]);
pose_3d->set_translation_parameterization(
translation_parameterization_.ToProto());
auto* rotation = pose_3d->mutable_rotation();
rotation->set_x(rotation_[0]);
rotation->set_y(rotation_[1]);
rotation->set_z(rotation_[2]);
rotation->set_w(rotation_[3]);
pose_3d->set_rotation_parameterization(rotation_parameterization_.ToProto());
return parameters;
}
} // namespace pose_graph
} // namespace cartographer

60
cartographer/pose_graph/node/pose_3d.h
View File

@ -1,60 +0,0 @@
/*
* 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_NODE_POSE_3D_H_
#define CARTOGRAPHER_POSE_GRAPH_NODE_POSE_3D_H_
#include "cartographer/pose_graph/node/node.h"
#include <array>
#include "cartographer/pose_graph/node/parameterization/parameterization.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace pose_graph {
class Pose3D : public Node {
public:
Pose3D(const NodeId& node_id, bool constant, const proto::Pose3D& pose_3d);
std::array<double, 3>* mutable_translation() { return &translation_; }
const std::array<double, 3>& translation() const { return translation_; }
ceres::LocalParameterization* translation_parameterization() const {
return translation_parameterization_.ceres_parameterization();
}
std::array<double, 4>* mutable_rotation() { return &rotation_; }
const std::array<double, 4>& rotation() const { return rotation_; }
ceres::LocalParameterization* rotation_parameterization() const {
return rotation_parameterization_.ceres_parameterization();
}
protected:
proto::Parameters ToParametersProto() const final;
private:
std::array<double, 3> translation_;
Parameterization translation_parameterization_;
std::array<double, 4> rotation_;
Parameterization rotation_parameterization_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_NODE_POSE_3D_H_

52
cartographer/pose_graph/node/pose_3d_test.cc
View File

@ -1,52 +0,0 @@
/*
* 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/node/pose_3d.h"
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
using testing::ParseProto;
constexpr char kExpectedNode[] = R"PROTO(
id { object_id: "bumpy_world" timestamp: 1 }
constant: true
parameters {
pose_3d {
translation { x: 1 y: 2 z: 3 }
translation_parameterization: FIX_Z
rotation: { w: 0 x: 1 y: 2 z: 3 }
}
}
)PROTO";
TEST(Pose3DTest, ToProto) {
Pose3D pose_3d({"bumpy_world", common::FromUniversal(1)}, true,
ParseProto<proto::Pose3D>(R"(
translation { x: 1 y: 2 z: 3 }
translation_parameterization: FIX_Z
rotation: { w: 0 x: 1 y: 2 z: 3 }
rotation_parameterization: NONE
)"));
EXPECT_THAT(pose_3d.ToProto(), testing::EqualsProto(kExpectedNode));
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

48
cartographer/pose_graph/pose_graph_controller.cc
View File

@ -1,48 +0,0 @@
/*
* 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/pose_graph_controller.h"
namespace cartographer {
namespace pose_graph {
void PoseGraphController::AddData(const proto::PoseGraphData& data) {
absl::MutexLock locker(&mutex_);
for (const auto& node : data.nodes()) {
AddNodeToPoseGraphData(node, &data_);
}
for (const auto& constraint : data.constraints()) {
AddConstraintToPoseGraphData(constraint, &data_);
}
}
void PoseGraphController::AddNode(const proto::Node& node) {
absl::MutexLock locker(&mutex_);
AddNodeToPoseGraphData(node, &data_);
}
void PoseGraphController::AddConstraint(const proto::Constraint& constraint) {
absl::MutexLock locker(&mutex_);
AddConstraintToPoseGraphData(constraint, &data_);
}
Solver::SolverStatus PoseGraphController::Optimize() {
absl::MutexLock locker(&mutex_);
return solver_->Solve(&data_);
}
} // namespace pose_graph
} // namespace cartographer

53
cartographer/pose_graph/pose_graph_controller.h
View File

@ -1,53 +0,0 @@
/*
* 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_POSE_GRAPH_CONTROLLER_H_
#define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_
#include "absl/synchronization/mutex.h"
#include "cartographer/pose_graph/pose_graph_data.h"
#include "cartographer/pose_graph/proto/pose_graph_data.pb.h"
#include "cartographer/pose_graph/solver/solver.h"
namespace cartographer {
namespace pose_graph {
class PoseGraphController {
public:
PoseGraphController(std::unique_ptr<Solver> optimizer)
: solver_(std::move(optimizer)) {}
PoseGraphController(const PoseGraphController&) = delete;
PoseGraphController& operator=(const PoseGraphController&) = delete;
void AddData(const proto::PoseGraphData& data) LOCKS_EXCLUDED(mutex_);
void AddNode(const proto::Node& node) LOCKS_EXCLUDED(mutex_);
void AddConstraint(const proto::Constraint& constraint)
LOCKS_EXCLUDED(mutex_);
Solver::SolverStatus Optimize() LOCKS_EXCLUDED(mutex_);
private:
std::unique_ptr<Solver> solver_;
mutable absl::Mutex mutex_;
PoseGraphData data_ GUARDED_BY(mutex_);
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_

100
cartographer/pose_graph/pose_graph_data.cc
View File

@ -1,100 +0,0 @@
/*
* 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/pose_graph_data.h"
#include "absl/memory/memory.h"
#include "cartographer/pose_graph/constraint/acceleration_constraint_3d.h"
#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h"
#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h"
#include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h"
#include "cartographer/pose_graph/constraint/relative_pose_constraint_3d.h"
#include "cartographer/pose_graph/constraint/rotation_constraint_3d.h"
namespace cartographer {
namespace pose_graph {
namespace {
using absl::make_unique;
std::unique_ptr<Constraint> CreateConstraint(
const proto::Constraint& constraint) {
const auto& id = constraint.id();
const auto& loss = constraint.loss_function();
const auto& cost = constraint.cost_function();
switch (cost.type_case()) {
case (proto::CostFunction::kRelativePose2D):
return make_unique<RelativePoseConstraint2D>(id, loss,
cost.relative_pose_2d());
case (proto::CostFunction::kRelativePose3D):
return make_unique<RelativePoseConstraint3D>(id, loss,
cost.relative_pose_3d());
case (proto::CostFunction::kAcceleration3D):
return make_unique<AccelerationConstraint3D>(id, loss,
cost.acceleration_3d());
case (proto::CostFunction::kRotation3D):
return make_unique<RotationContraint3D>(id, loss, cost.rotation_3d());
case (proto::CostFunction::kInterpolatedRelativePose2D):
return make_unique<InterpolatedRelativePoseConstraint2D>(
id, loss, cost.interpolated_relative_pose_2d());
case (proto::CostFunction::kInterpolatedRelativePose3D):
return make_unique<InterpolatedRelativePoseConstraint3D>(
id, loss, cost.interpolated_relative_pose_3d());
case (proto::CostFunction::TYPE_NOT_SET):
LOG(FATAL) << "Constraint cost function type is not set.";
}
return nullptr;
}
} // namespace
void AddNodeToPoseGraphData(const proto::Node& node, PoseGraphData* data) {
NodeId node_id(node.id());
switch (node.parameters().type_case()) {
case (proto::Parameters::kPose2D): {
data->nodes.pose_2d_nodes.emplace(
node_id, make_unique<Pose2D>(node_id, node.constant(),
node.parameters().pose_2d()));
return;
}
case (proto::Parameters::kPose3D): {
data->nodes.pose_3d_nodes.emplace(
node_id, make_unique<Pose3D>(node_id, node.constant(),
node.parameters().pose_3d()));
return;
}
case (proto::Parameters::kImuCalibration): {
data->nodes.imu_calibration_nodes.emplace(
node_id,
make_unique<ImuCalibration>(node_id, node.constant(),
node.parameters().imu_calibration()));
return;
}
case (proto::Parameters::TYPE_NOT_SET): {
LOG(FATAL) << "Node parameter type is not set.";
return;
}
}
}
void AddConstraintToPoseGraphData(const proto::Constraint& constraint,
PoseGraphData* data) {
data->constraints.emplace_back(CreateConstraint(constraint));
}
} // namespace pose_graph
} // namespace cartographer

38
cartographer/pose_graph/pose_graph_data.h
View File

@ -1,38 +0,0 @@
/*
* 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_POSE_GRAPH_DATA_H_
#define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_DATA_H_
#include "cartographer/pose_graph/constraint/constraint.h"
#include "cartographer/pose_graph/node/nodes.h"
namespace cartographer {
namespace pose_graph {
struct PoseGraphData {
Nodes nodes;
std::vector<std::unique_ptr<Constraint>> constraints;
};
void AddNodeToPoseGraphData(const proto::Node& node, PoseGraphData* data);
void AddConstraintToPoseGraphData(const proto::Constraint& constraint,
PoseGraphData* data);
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_DATA_H_

26
cartographer/pose_graph/proto/constraint.proto
View File

@ -1,26 +0,0 @@
// 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.
syntax = "proto3";
import "cartographer/pose_graph/proto/loss_function.proto";
import "cartographer/pose_graph/proto/cost_function.proto";
package cartographer.pose_graph.proto;
message Constraint {
string id = 1;
CostFunction cost_function = 2;
LossFunction loss_function = 3;
}

114
cartographer/pose_graph/proto/cost_function.proto
View File

@ -1,114 +0,0 @@
// 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.
syntax = "proto3";
import "cartographer/pose_graph/proto/node.proto";
import "cartographer/transform/proto/transform.proto";
package cartographer.pose_graph.proto;
message RelativePose2D {
NodeId first = 1;
NodeId second = 2;
message Parameters {
transform.proto.Rigid2d first_t_second = 1;
double translation_weight = 2;
double rotation_weight = 3;
}
Parameters parameters = 3;
}
message RelativePose3D {
NodeId first = 1;
NodeId second = 2;
message Parameters {
transform.proto.Rigid3d first_t_second = 1;
double translation_weight = 2;
double rotation_weight = 3;
}
Parameters parameters = 3;
}
message Acceleration3D {
NodeId first = 1;
NodeId second = 2;
NodeId third = 3;
NodeId imu_calibration = 4;
message Parameters {
transform.proto.Vector3d delta_velocity_imu_frame = 1;
double first_to_second_delta_time_seconds = 2;
double second_to_third_delta_time_seconds = 3;
double scaling_factor = 4;
}
Parameters parameters = 5;
}
message Rotation3D {
NodeId first = 1;
NodeId second = 2;
NodeId imu_calibration = 3;
message Parameters {
transform.proto.Quaterniond delta_rotation_imu_frame = 1;
double scaling_factor = 2;
}
Parameters parameters = 4;
}
message InterpolatedRelativePose2D {
NodeId first_start = 1;
NodeId first_end = 2;
NodeId second = 3;
message Parameters {
transform.proto.Rigid3d first_t_second = 1;
double translation_weight = 2;
double rotation_weight = 3;
transform.proto.Quaterniond gravity_alignment_first_start = 4;
transform.proto.Quaterniond gravity_alignment_first_end = 5;
// Interpolates between first_start and first_end.
double interpolation_factor = 6;
}
Parameters parameters = 4;
}
message InterpolatedRelativePose3D {
NodeId first_start = 1;
NodeId first_end = 2;
NodeId second = 3;
message Parameters {
transform.proto.Rigid3d first_t_second = 1;
double translation_weight = 2;
double rotation_weight = 3;
// Interpolates between first_start and first_end.
double interpolation_factor = 4;
}
Parameters parameters = 4;
}
message CostFunction {
oneof type {
RelativePose2D relative_pose_2d = 1;
RelativePose3D relative_pose_3d = 2;
Acceleration3D acceleration_3d = 3;
Rotation3D rotation_3d = 4;
InterpolatedRelativePose2D interpolated_relative_pose_2d = 5;
InterpolatedRelativePose3D interpolated_relative_pose_3d = 6;
}
}

30
cartographer/pose_graph/proto/loss_function.proto
View File

@ -1,30 +0,0 @@
// 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.
syntax = "proto3";
package cartographer.pose_graph.proto;
message QuadraticLoss {}
message HuberLoss {
double scale = 1;
}
message LossFunction {
oneof Type {
QuadraticLoss quadratic_loss = 1;
HuberLoss huber_loss = 2;
}
}

65
cartographer/pose_graph/proto/node.proto
View File

@ -1,65 +0,0 @@
// 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.
syntax = "proto3";
package cartographer.pose_graph.proto;
import "cartographer/transform/proto/transform.proto";
enum Parameterization {
NONE = 0;
FIX_Z = 1;
YAW_ONLY = 2;
YAW_CONSTANT = 3;
}
message Pose2D {
transform.proto.Vector2d translation = 1;
double rotation = 2;
}
message Pose3D {
transform.proto.Vector3d translation = 1;
transform.proto.Quaterniond rotation = 2;
Parameterization translation_parameterization = 3;
Parameterization rotation_parameterization = 4;
}
message ImuCalibration {
double gravity_constant = 1;
transform.proto.Quaterniond orientation = 2;
Parameterization orientation_parameterization = 3;
}
message NodeId {
string object_id = 1;
string group_id = 2;
int64 timestamp = 3;
}
message Parameters {
oneof type {
Pose2D pose_2d = 1;
Pose3D pose_3d = 2;
ImuCalibration imu_calibration = 3;
}
}
message Node {
NodeId id = 1;
bool constant = 2;
Parameters parameters = 3;
}

25
cartographer/pose_graph/proto/pose_graph_data.proto
View File

@ -1,25 +0,0 @@
// 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.
syntax = "proto3";
import "cartographer/pose_graph/proto/node.proto";
import "cartographer/pose_graph/proto/constraint.proto";
package cartographer.pose_graph.proto;
message PoseGraphData {
repeated Node nodes = 1;
repeated Constraint constraints = 2;
}

14
cartographer/pose_graph/proto/solver_config.proto
View File

@ -1,14 +0,0 @@
syntax = "proto3";
import "cartographer/common/proto/ceres_solver_options.proto";
package cartographer.pose_graph.proto;
// Configuration for the non-linear least squares solver.
message SolverConfig {
// If true, logs a summary of the optimization in every iteration.
bool log_solver_summary = 1;
// Options specific for the Ceres solver.
cartographer.common.proto.CeresSolverOptions ceres_options = 2;
}

64
cartographer/pose_graph/solver/ceres_solver.cc
View File

@ -1,64 +0,0 @@
/*
* 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/solver/ceres_solver.h"
namespace cartographer {
namespace pose_graph {
namespace {
ceres::Problem::Options CreateCeresProblemOptions() {
ceres::Problem::Options problem_options;
problem_options.cost_function_ownership =
ceres::Ownership::DO_NOT_TAKE_OWNERSHIP;
problem_options.loss_function_ownership =
ceres::Ownership::DO_NOT_TAKE_OWNERSHIP;
problem_options.local_parameterization_ownership =
ceres::Ownership::DO_NOT_TAKE_OWNERSHIP;
return problem_options;
}
Solver::SolverStatus ToSolverStatus(
const ceres::TerminationType& termination_type) {
switch (termination_type) {
case (ceres::TerminationType::CONVERGENCE):
return Solver::SolverStatus::CONVERGENCE;
case (ceres::TerminationType::NO_CONVERGENCE):
return Solver::SolverStatus::NO_CONVERGENCE;
default:
return Solver::SolverStatus::FAILURE;
}
}
} // namespace
CeresSolver::CeresSolver(const ceres::Solver::Options& options)
: problem_options_(CreateCeresProblemOptions()), solver_options_(options) {}
Solver::SolverStatus CeresSolver::Solve(PoseGraphData* data) const {
ceres::Problem problem(problem_options_);
for (const auto& constraint : data->constraints) {
constraint->AddToSolver(&data->nodes, &problem);
}
ceres::Solver::Summary summary;
ceres::Solve(solver_options_, &problem, &summary);
return ToSolverStatus(summary.termination_type);
}
} // namespace pose_graph
} // namespace cartographer

41
cartographer/pose_graph/solver/ceres_solver.h
View File

@ -1,41 +0,0 @@
/*
* 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_OPTIMIZER_CERES_SOLVER_H_
#define CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_SOLVER_H_
#include "cartographer/pose_graph/solver/solver.h"
#include "ceres/problem.h"
#include "ceres/solver.h"
namespace cartographer {
namespace pose_graph {
class CeresSolver : public Solver {
public:
explicit CeresSolver(const ceres::Solver::Options& options);
SolverStatus Solve(PoseGraphData* data) const final;
private:
const ceres::Problem::Options problem_options_;
const ceres::Solver::Options solver_options_;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_SOLVER_H_

82
cartographer/pose_graph/solver/ceres_solver_test.cc
View File

@ -1,82 +0,0 @@
/*
* 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/solver/ceres_solver.h"
#include "absl/memory/memory.h"
#include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h"
#include "cartographer/testing/test_helpers.h"
namespace cartographer {
namespace pose_graph {
namespace {
using testing::ParseProto;
constexpr char kStartNode[] = R"PROTO(
id { object_id: "start_node" timestamp: 1 }
constant: false
parameters {
pose_2d {
translation { x: 1 y: 2 }
rotation: 5
}
}
)PROTO";
constexpr char kEndNode[] = R"PROTO(
id { object_id: "end_node" timestamp: 1 }
constant: false
parameters {
pose_2d {
translation { x: 1 y: 2 }
rotation: 5
}
}
)PROTO";
constexpr char kRelativePose2D[] = R"PROTO(
id: "constraint_1"
loss_function { quadratic_loss: {} }
cost_function {
relative_pose_2d {
first { object_id: "start_node" timestamp: 1 }
second { object_id: "end_node" timestamp: 1 }
parameters {
first_t_second {
translation { x: 1 y: 1 }
rotation: 0
}
translation_weight: 1
rotation_weight: 1
}
}
}
)PROTO";
TEST(CeresSolverTest, SmokeTest) {
PoseGraphData data;
AddNodeToPoseGraphData(ParseProto<proto::Node>(kStartNode), &data);
AddNodeToPoseGraphData(ParseProto<proto::Node>(kEndNode), &data);
AddConstraintToPoseGraphData(ParseProto<proto::Constraint>(kRelativePose2D),
&data);
CeresSolver optimizer(ceres::Solver::Options{});
EXPECT_EQ(optimizer.Solve(&data), Solver::SolverStatus::CONVERGENCE);
}
} // namespace
} // namespace pose_graph
} // namespace cartographer

45
cartographer/pose_graph/solver/solver.h
View File

@ -1,45 +0,0 @@
/*
* 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_SOLVER_SOLVER_H_
#define CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_
#include "cartographer/pose_graph/pose_graph_data.h"
namespace cartographer {
namespace pose_graph {
class Solver {
public:
enum class SolverStatus {
CONVERGENCE,
NO_CONVERGENCE,
FAILURE,
};
Solver() = default;
virtual ~Solver() = default;
Solver(const Solver&) = delete;
Solver& operator=(const Solver&) = delete;
virtual SolverStatus Solve(PoseGraphData* data) const = 0;
};
} // namespace pose_graph
} // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_