-
Notifications
You must be signed in to change notification settings - Fork 2.2k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[GenericPoseGraph] Add parameterization. (#1385)
- Loading branch information
1 parent
05f2c6c
commit 5261c90
Showing
15 changed files
with
212 additions
and
38 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
61 changes: 61 additions & 0 deletions
61
cartographer/pose_graph/node/parameterization/parameterization.cc
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,61 @@ | ||
/* | ||
* Copyright 2018 The Cartographer Authors | ||
* | ||
* Licensed under the Apache License, Version 2.0 (the "License"); | ||
* you may not use this file except in compliance with the License. | ||
* You may obtain a copy of the License at | ||
* | ||
* http://www.apache.org/licenses/LICENSE-2.0 | ||
* | ||
* Unless required by applicable law or agreed to in writing, software | ||
* distributed under the License is distributed on an "AS IS" BASIS, | ||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
* See the License for the specific language governing permissions and | ||
* limitations under the License. | ||
*/ | ||
|
||
#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 changes: 44 additions & 0 deletions
44
cartographer/pose_graph/node/parameterization/parameterization.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,44 @@ | ||
/* | ||
* 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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.