[GenericPoseGraph] Add RelativePoseConstraint2D. (#1297)
Alexander Belyaev
GitHub
parent
ce7f33f97a
commit
df337194e2
|
|
@ -0,0 +1,30 @@
|
|||
/*
|
||||
* 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 {
|
||||
|
||||
proto::Constraint Constraint::ToProto() const {
|
||||
proto::Constraint constraint;
|
||||
constraint.set_id(constraint_id_);
|
||||
*constraint.mutable_cost_function() = ToCostFunctionProto();
|
||||
return constraint;
|
||||
}
|
||||
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
/*
|
||||
* 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/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:
|
||||
explicit Constraint(const ConstraintId& id) : constraint_id_(id) {}
|
||||
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 AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const = 0;
|
||||
|
||||
protected:
|
||||
virtual proto::CostFunction ToCostFunctionProto() const = 0;
|
||||
|
||||
private:
|
||||
ConstraintId constraint_id_;
|
||||
};
|
||||
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_H_
|
||||
|
|
@ -0,0 +1,88 @@
|
|||
/*
|
||||
* 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/common/make_unique.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace pose_graph {
|
||||
namespace {
|
||||
|
||||
// TODO(pifon): Move to common/utils.h.
|
||||
template <typename MapType, typename KeyType = typename MapType::key_type,
|
||||
typename ValueType = typename MapType::mapped_type>
|
||||
ValueType* FindOrNull(MapType& map, const KeyType& key) {
|
||||
auto it = map.find(key);
|
||||
if (it == map.end()) return nullptr;
|
||||
return &(it->second);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
RelativePoseConstraint2D::RelativePoseConstraint2D(
|
||||
const ConstraintId& id, const proto::RelativePose2D& proto)
|
||||
: Constraint(id),
|
||||
first_(proto.first()),
|
||||
second_(proto.second()),
|
||||
ceres_cost_(common::make_unique<RelativePoseCost2D>(proto.parameters())) {
|
||||
}
|
||||
|
||||
void RelativePoseConstraint2D::AddToOptimizer(Nodes* nodes,
|
||||
ceres::Problem* problem) const {
|
||||
auto first_node = FindOrNull(nodes->pose_2d_nodes, first_);
|
||||
if (first_node == nullptr) {
|
||||
LOG(INFO) << "First node was not found in pose_2d_nodes.";
|
||||
return;
|
||||
}
|
||||
|
||||
auto second_node = FindOrNull(nodes->pose_2d_nodes, second_);
|
||||
if (second_node == nullptr) {
|
||||
LOG(INFO) << "Second node was not found in pose_2d_nodes.";
|
||||
return;
|
||||
}
|
||||
|
||||
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(), nullptr /* loss function */,
|
||||
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
|
||||
|
|
@ -0,0 +1,45 @@
|
|||
/*
|
||||
* 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::RelativePose2D& proto);
|
||||
|
||||
void AddToOptimizer(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_
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
/*
|
||||
* 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/nodes/nodes.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace pose_graph {
|
||||
|
||||
struct PoseGraphData {
|
||||
Nodes nodes;
|
||||
std::vector<std::unique_ptr<Constraint>> constraints;
|
||||
};
|
||||
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_DATA_H_
|
||||
Loading…
Reference in New Issue