Support fixing z during 3D sparse pose adjustment. (#161)

This is in preparation of using the 3D sparse pose graph
optimization for 2D SLAM.
master
Wolfgang Hess 2016-12-14 15:32:23 +01:00 committed by GitHub
parent ae490c540d
commit 0937f4f515
9 changed files with 31 additions and 13 deletions

View File

@ -12,7 +12,7 @@
# See the License for the specific language governing permissions and # See the License for the specific language governing permissions and
# limitations under the License. # limitations under the License.
cmake_minimum_required (VERSION 2.8.7) cmake_minimum_required (VERSION 2.8.12)
project(cartographer) project(cartographer)
set(CARTOGRAPHER_MAJOR_VERSION 1) set(CARTOGRAPHER_MAJOR_VERSION 1)

View File

@ -21,13 +21,15 @@ namespace mapping_3d {
CeresPose::CeresPose( CeresPose::CeresPose(
const transform::Rigid3d& rigid, const transform::Rigid3d& rigid,
std::unique_ptr<ceres::LocalParameterization> translation_parametrization,
std::unique_ptr<ceres::LocalParameterization> rotation_parametrization, std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
ceres::Problem* problem) ceres::Problem* problem)
: translation_({{rigid.translation().x(), rigid.translation().y(), : translation_({{rigid.translation().x(), rigid.translation().y(),
rigid.translation().z()}}), rigid.translation().z()}}),
rotation_({{rigid.rotation().w(), rigid.rotation().x(), rotation_({{rigid.rotation().w(), rigid.rotation().x(),
rigid.rotation().y(), rigid.rotation().z()}}) { rigid.rotation().y(), rigid.rotation().z()}}) {
problem->AddParameterBlock(translation_.data(), 3); problem->AddParameterBlock(translation_.data(), 3,
translation_parametrization.release());
problem->AddParameterBlock(rotation_.data(), 4, problem->AddParameterBlock(rotation_.data(), 4,
rotation_parametrization.release()); rotation_parametrization.release());
} }

View File

@ -31,6 +31,7 @@ class CeresPose {
public: public:
CeresPose( CeresPose(
const transform::Rigid3d& rigid, const transform::Rigid3d& rigid,
std::unique_ptr<ceres::LocalParameterization> translation_parametrization,
std::unique_ptr<ceres::LocalParameterization> rotation_parametrization, std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
ceres::Problem* problem); ceres::Problem* problem);

View File

@ -94,7 +94,7 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose,
ceres::Solver::Summary* const summary) { ceres::Solver::Summary* const summary) {
ceres::Problem problem; ceres::Problem problem;
CeresPose ceres_pose( CeresPose ceres_pose(
initial_pose_estimate, initial_pose_estimate, nullptr /* translation_parameterization */,
options_.only_optimize_yaw() options_.only_optimize_yaw()
? std::unique_ptr<ceres::LocalParameterization>( ? std::unique_ptr<ceres::LocalParameterization>(
common::make_unique<ceres::AutoDiffLocalParameterization< common::make_unique<ceres::AutoDiffLocalParameterization<

View File

@ -44,7 +44,8 @@ SparsePoseGraph::SparsePoseGraph(
common::ThreadPool* thread_pool, common::ThreadPool* thread_pool,
std::deque<mapping::TrajectoryNode::ConstantData>* constant_node_data) std::deque<mapping::TrajectoryNode::ConstantData>* constant_node_data)
: options_(options), : options_(options),
optimization_problem_(options_.optimization_problem_options()), optimization_problem_(options_.optimization_problem_options(),
sparse_pose_graph::OptimizationProblem::FixZ::kNo),
constraint_builder_(options_.constraint_builder_options(), thread_pool), constraint_builder_(options_.constraint_builder_options(), thread_pool),
constant_node_data_(constant_node_data) {} constant_node_data_(constant_node_data) {}

View File

@ -72,8 +72,9 @@ struct ConstantYawQuaternionPlus {
OptimizationProblem::OptimizationProblem( OptimizationProblem::OptimizationProblem(
const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&
options) options,
: options_(options) {} FixZ fix_z)
: options_(options), fix_z_(fix_z) {}
OptimizationProblem::~OptimizationProblem() {} OptimizationProblem::~OptimizationProblem() {}
@ -114,13 +115,21 @@ void OptimizationProblem::Solve(
ceres::Problem::Options problem_options; ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options); ceres::Problem problem(problem_options);
const auto translation_parameterization =
[this]() -> std::unique_ptr<ceres::LocalParameterization> {
return fix_z_ == FixZ::kYes
? common::make_unique<ceres::SubsetParameterization>(
3, std::vector<int>{2})
: nullptr;
};
// Set the starting point. // Set the starting point.
std::deque<CeresPose> C_submaps; std::deque<CeresPose> C_submaps;
std::deque<CeresPose> C_point_clouds; std::deque<CeresPose> C_point_clouds;
// Tie the first submap to the origin. // Tie the first submap to the origin.
CHECK(!submap_transforms->empty()); CHECK(!submap_transforms->empty());
C_submaps.emplace_back( C_submaps.emplace_back(
transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), translation_parameterization(),
common::make_unique<ceres::AutoDiffLocalParameterization< common::make_unique<ceres::AutoDiffLocalParameterization<
ConstantYawQuaternionPlus, 4, 2>>(), ConstantYawQuaternionPlus, 4, 2>>(),
&problem); &problem);
@ -128,12 +137,12 @@ void OptimizationProblem::Solve(
for (size_t i = 1; i != submap_transforms->size(); ++i) { for (size_t i = 1; i != submap_transforms->size(); ++i) {
C_submaps.emplace_back( C_submaps.emplace_back(
(*submap_transforms)[i], (*submap_transforms)[i], translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), &problem); common::make_unique<ceres::QuaternionParameterization>(), &problem);
} }
for (size_t j = 0; j != node_data_.size(); ++j) { for (size_t j = 0; j != node_data_.size(); ++j) {
C_point_clouds.emplace_back( C_point_clouds.emplace_back(
node_data_[j].point_cloud_pose, node_data_[j].point_cloud_pose, translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), &problem); common::make_unique<ceres::QuaternionParameterization>(), &problem);
} }

View File

@ -47,9 +47,12 @@ class OptimizationProblem {
public: public:
using Constraint = mapping::SparsePoseGraph::Constraint; using Constraint = mapping::SparsePoseGraph::Constraint;
explicit OptimizationProblem( enum class FixZ { kYes, kNo };
OptimizationProblem(
const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&
options); options,
FixZ fix_z);
~OptimizationProblem(); ~OptimizationProblem();
OptimizationProblem(const OptimizationProblem&) = delete; OptimizationProblem(const OptimizationProblem&) = delete;
@ -72,6 +75,7 @@ class OptimizationProblem {
private: private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
FixZ fix_z_;
std::map<const mapping::Submaps*, std::deque<ImuData>> imu_data_; std::map<const mapping::Submaps*, std::deque<ImuData>> imu_data_;
std::vector<NodeData> node_data_; std::vector<NodeData> node_data_;
double gravity_constant_ = 9.8; double gravity_constant_ = 9.8;

View File

@ -34,7 +34,8 @@ namespace {
class OptimizationProblemTest : public ::testing::Test { class OptimizationProblemTest : public ::testing::Test {
protected: protected:
OptimizationProblemTest() OptimizationProblemTest()
: optimization_problem_(CreateOptions()), rng_(45387) {} : optimization_problem_(CreateOptions(), OptimizationProblem::FixZ::kNo),
rng_(45387) {}
mapping::sparse_pose_graph::proto::OptimizationProblemOptions mapping::sparse_pose_graph::proto::OptimizationProblemOptions
CreateOptions() { CreateOptions() {

View File

@ -48,7 +48,7 @@ OrderedMultiQueue::~OrderedMultiQueue() {
void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) { void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
CHECK_EQ(queues_.count(queue_key), 0); CHECK_EQ(queues_.count(queue_key), 0);
queues_[queue_key].callback = callback; queues_[queue_key].callback = std::move(callback);
} }
void OrderedMultiQueue::MarkQueueAsFinished(const QueueKey& queue_key) { void OrderedMultiQueue::MarkQueueAsFinished(const QueueKey& queue_key) {