Skip to content

Commit aeb815c

Browse files
ojuraojura
authored
Merge branch 'master' into introduce-rangefinderpoint
2 parents 75c38b0 + d5840e9 commit aeb815c

12 files changed

+125
-134
lines changed

‎cartographer/BUILD.bazel

+3-2
Original file line numberDiff line numberDiff line change
@@ -96,8 +96,9 @@ cc_library(
9696
":cc_protos",
9797
"@boost//:iostreams",
9898
"@com_google_absl//absl/base",
99-
"@com_google_absl//absl/types:optional",
99+
"@com_google_absl//absl/strings",
100100
"@com_google_absl//absl/synchronization",
101+
"@com_google_absl//absl/types:optional",
101102
"@com_google_glog//:glog",
102103
"@org_cairographics_cairo//:cairo",
103104
"@org_ceres_solver_ceres_solver//:ceres",
@@ -113,8 +114,8 @@ cc_library(
113114
deps = [
114115
":cartographer",
115116
":cartographer_test_library",
116-
"@com_google_googletest//:gtest_main",
117117
"@com_google_absl//absl/memory",
118+
"@com_google_googletest//:gtest_main",
118119
],
119120
) for src in glob(
120121
["**/*_test.cc"],

‎cartographer/pose_graph/constraint/acceleration_constraint_3d.cc

+5-29
Original file line numberDiff line numberDiff line change
@@ -18,34 +18,10 @@
1818

1919
#include "absl/memory/memory.h"
2020
#include "cartographer/common/utils.h"
21+
#include "cartographer/pose_graph/constraint/constraint_utils.h"
2122

2223
namespace cartographer {
2324
namespace pose_graph {
24-
namespace {
25-
26-
void AddPoseParameters(Pose3D* pose, ceres::Problem* problem) {
27-
auto transation = pose->mutable_translation();
28-
auto rotation = pose->mutable_rotation();
29-
problem->AddParameterBlock(transation->data(), transation->size());
30-
problem->AddParameterBlock(rotation->data(), rotation->size());
31-
if (pose->constant()) {
32-
problem->SetParameterBlockConstant(transation->data());
33-
problem->SetParameterBlockConstant(rotation->data());
34-
}
35-
}
36-
37-
void AddImuParameters(ImuCalibration* pose, ceres::Problem* problem) {
38-
auto gravity = pose->mutable_gravity_constant();
39-
auto orientation = pose->mutable_orientation();
40-
problem->AddParameterBlock(gravity, 1);
41-
problem->AddParameterBlock(orientation->data(), orientation->size());
42-
if (pose->constant()) {
43-
problem->SetParameterBlockConstant(gravity);
44-
problem->SetParameterBlockConstant(orientation->data());
45-
}
46-
}
47-
48-
} // namespace
4925

5026
AccelerationConstraint3D::AccelerationConstraint3D(
5127
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
@@ -90,10 +66,10 @@ void AccelerationConstraint3D::AddToOptimizer(Nodes* nodes,
9066
return;
9167
}
9268

93-
AddPoseParameters(first_node, problem);
94-
AddPoseParameters(second_node, problem);
95-
AddPoseParameters(third_node, problem);
96-
AddImuParameters(imu_node, problem);
69+
AddPose3D(first_node, problem);
70+
AddPose3D(second_node, problem);
71+
AddPose3D(third_node, problem);
72+
AddImuCalibration(imu_node, problem);
9773
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
9874
second_node->mutable_rotation()->data(),
9975
second_node->mutable_translation()->data(),

‎cartographer/pose_graph/constraint/acceleration_constraint_3d.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -58,4 +58,4 @@ class AccelerationConstraint3D : public Constraint {
5858
} // namespace pose_graph
5959
} // namespace cartographer
6060

61-
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_
61+
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ACCELERATION_CONSTRAINT_3D_H_

‎cartographer/pose_graph/constraint/constraint_utils.cc

+37
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
#include "cartographer/pose_graph/constraint/constraint_utils.h"
2+
3+
namespace cartographer {
4+
namespace pose_graph {
5+
6+
void AddPose2D(Pose2D* pose, ceres::Problem* problem) {
7+
auto pose_2d = pose->mutable_pose_2d();
8+
problem->AddParameterBlock(pose_2d->data(), pose_2d->size());
9+
if (pose->constant()) {
10+
problem->SetParameterBlockConstant(pose_2d->data());
11+
}
12+
}
13+
14+
void AddPose3D(Pose3D* pose, ceres::Problem* problem) {
15+
auto transation = pose->mutable_translation();
16+
auto rotation = pose->mutable_rotation();
17+
problem->AddParameterBlock(transation->data(), transation->size());
18+
problem->AddParameterBlock(rotation->data(), rotation->size());
19+
if (pose->constant()) {
20+
problem->SetParameterBlockConstant(transation->data());
21+
problem->SetParameterBlockConstant(rotation->data());
22+
}
23+
}
24+
25+
void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem) {
26+
auto gravity = pose->mutable_gravity_constant();
27+
auto orientation = pose->mutable_orientation();
28+
problem->AddParameterBlock(gravity, 1);
29+
problem->AddParameterBlock(orientation->data(), orientation->size());
30+
if (pose->constant()) {
31+
problem->SetParameterBlockConstant(gravity);
32+
problem->SetParameterBlockConstant(orientation->data());
33+
}
34+
}
35+
36+
} // namespace pose_graph
37+
} // namespace cartographer

‎cartographer/pose_graph/constraint/constraint_utils.h

+37
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
/*
2+
* Copyright 2018 The Cartographer Authors
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*/
16+
17+
#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_
18+
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_
19+
20+
#include "cartographer/pose_graph/node/imu_calibration.h"
21+
#include "cartographer/pose_graph/node/pose_2d.h"
22+
#include "cartographer/pose_graph/node/pose_3d.h"
23+
#include "ceres/problem.h"
24+
25+
namespace cartographer {
26+
namespace pose_graph {
27+
28+
void AddPose2D(Pose2D* pose, ceres::Problem* problem);
29+
30+
void AddPose3D(Pose3D* pose, ceres::Problem* problem);
31+
32+
void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem);
33+
34+
} // namespace pose_graph
35+
} // namespace cartographer
36+
37+
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_

‎cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc

+4-25
Original file line numberDiff line numberDiff line change
@@ -18,31 +18,10 @@
1818

1919
#include "absl/memory/memory.h"
2020
#include "cartographer/common/utils.h"
21+
#include "cartographer/pose_graph/constraint/constraint_utils.h"
2122

2223
namespace cartographer {
2324
namespace pose_graph {
24-
namespace {
25-
26-
void AddPose3DParameters(Pose3D* pose, ceres::Problem* problem) {
27-
auto transation = pose->mutable_translation();
28-
auto rotation = pose->mutable_rotation();
29-
problem->AddParameterBlock(transation->data(), transation->size());
30-
problem->AddParameterBlock(rotation->data(), rotation->size());
31-
if (pose->constant()) {
32-
problem->SetParameterBlockConstant(transation->data());
33-
problem->SetParameterBlockConstant(rotation->data());
34-
}
35-
}
36-
37-
void AddPose2DParameters(Pose2D* pose, ceres::Problem* problem) {
38-
auto pose_2d = pose->mutable_pose_2d();
39-
problem->AddParameterBlock(pose_2d->data(), pose_2d->size());
40-
if (pose->constant()) {
41-
problem->SetParameterBlockConstant(pose_2d->data());
42-
}
43-
}
44-
45-
} // namespace
4625

4726
InterpolatedRelativePoseConstraint2D::InterpolatedRelativePoseConstraint2D(
4827
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
@@ -81,9 +60,9 @@ void InterpolatedRelativePoseConstraint2D::AddToOptimizer(
8160
return;
8261
}
8362

84-
AddPose2DParameters(first_node_start, problem);
85-
AddPose2DParameters(first_node_end, problem);
86-
AddPose3DParameters(second_node, problem);
63+
AddPose2D(first_node_start, problem);
64+
AddPose2D(first_node_end, problem);
65+
AddPose3D(second_node, problem);
8766
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
8867
first_node_start->mutable_pose_2d()->data(),
8968
first_node_end->mutable_pose_2d()->data(),

‎cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc

+5-18
Original file line numberDiff line numberDiff line change
@@ -18,23 +18,10 @@
1818

1919
#include "absl/memory/memory.h"
2020
#include "cartographer/common/utils.h"
21+
#include "cartographer/pose_graph/constraint/constraint_utils.h"
2122

2223
namespace cartographer {
2324
namespace pose_graph {
24-
namespace {
25-
26-
void AddPoseParameters(Pose3D* pose, ceres::Problem* problem) {
27-
auto transation = pose->mutable_translation();
28-
auto rotation = pose->mutable_rotation();
29-
problem->AddParameterBlock(transation->data(), transation->size());
30-
problem->AddParameterBlock(rotation->data(), rotation->size());
31-
if (pose->constant()) {
32-
problem->SetParameterBlockConstant(transation->data());
33-
problem->SetParameterBlockConstant(rotation->data());
34-
}
35-
}
36-
37-
} // namespace
3825

3926
InterpolatedRelativePoseConstraint3D::InterpolatedRelativePoseConstraint3D(
4027
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
@@ -73,10 +60,10 @@ void InterpolatedRelativePoseConstraint3D::AddToOptimizer(
7360
return;
7461
}
7562

76-
AddPoseParameters(first_node_start, problem);
77-
AddPoseParameters(first_node_end, problem);
78-
AddPoseParameters(second_node, problem);
79-
problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */,
63+
AddPose3D(first_node_start, problem);
64+
AddPose3D(first_node_end, problem);
65+
AddPose3D(second_node, problem);
66+
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
8067
first_node_start->mutable_translation()->data(),
8168
first_node_start->mutable_rotation()->data(),
8269
first_node_end->mutable_translation()->data(),

‎cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc

+1
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818

1919
#include "absl/memory/memory.h"
2020
#include "cartographer/common/utils.h"
21+
#include "cartographer/pose_graph/constraint/constraint_utils.h"
2122

2223
namespace cartographer {
2324
namespace pose_graph {

‎cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc

+4-17
Original file line numberDiff line numberDiff line change
@@ -18,23 +18,10 @@
1818

1919
#include "absl/memory/memory.h"
2020
#include "cartographer/common/utils.h"
21+
#include "cartographer/pose_graph/constraint/constraint_utils.h"
2122

2223
namespace cartographer {
2324
namespace pose_graph {
24-
namespace {
25-
26-
void AddPoseParameters(Pose3D* pose, ceres::Problem* problem) {
27-
auto transation = pose->mutable_translation();
28-
auto rotation = pose->mutable_rotation();
29-
problem->AddParameterBlock(transation->data(), transation->size());
30-
problem->AddParameterBlock(rotation->data(), rotation->size());
31-
if (pose->constant()) {
32-
problem->SetParameterBlockConstant(transation->data());
33-
problem->SetParameterBlockConstant(rotation->data());
34-
}
35-
}
36-
37-
} // namespace
3825

3926
RelativePoseConstraint3D::RelativePoseConstraint3D(
4027
const ConstraintId& id, const proto::LossFunction& loss_function_proto,
@@ -64,9 +51,9 @@ void RelativePoseConstraint3D::AddToOptimizer(Nodes* nodes,
6451
return;
6552
}
6653

67-
AddPoseParameters(first_node, problem);
68-
AddPoseParameters(second_node, problem);
69-
problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */,
54+
AddPose3D(first_node, problem);
55+
AddPose3D(second_node, problem);
56+
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
7057
first_node->mutable_translation()->data(),
7158
first_node->mutable_rotation()->data(),
7259
second_node->mutable_translation()->data(),

‎cartographer/pose_graph/constraint/rotation_constraint_3d.cc

+16-13
Original file line numberDiff line numberDiff line change
@@ -23,14 +23,22 @@ namespace cartographer {
2323
namespace pose_graph {
2424
namespace {
2525

26-
void AddRotationParameters(Pose3D* pose, ceres::Problem* problem) {
26+
void AddRotation3D(Pose3D* pose, ceres::Problem* problem) {
2727
auto rotation = pose->mutable_rotation();
2828
problem->AddParameterBlock(rotation->data(), rotation->size());
2929
if (pose->constant()) {
3030
problem->SetParameterBlockConstant(rotation->data());
3131
}
3232
}
3333

34+
void AddImuOrientation(ImuCalibration* imu_node, ceres::Problem* problem) {
35+
auto imu_orientation = imu_node->mutable_orientation();
36+
problem->AddParameterBlock(imu_orientation->data(), imu_orientation->size());
37+
if (imu_node->constant()) {
38+
problem->SetParameterBlockConstant(imu_orientation->data());
39+
}
40+
}
41+
3442
} // namespace
3543

3644
RotationContraint3D::RotationContraint3D(
@@ -62,25 +70,20 @@ void RotationContraint3D::AddToOptimizer(Nodes* nodes,
6270
return;
6371
}
6472

65-
auto imu_calibration_node =
73+
auto imu_node =
6674
common::FindOrNull(nodes->imu_calibration_nodes, imu_calibration_);
67-
if (imu_calibration_node == nullptr) {
75+
if (imu_node == nullptr) {
6876
LOG(INFO) << "Imu calibration node was not found.";
6977
return;
7078
}
7179

72-
AddRotationParameters(first_node, problem);
73-
AddRotationParameters(second_node, problem);
74-
auto imu_orientation = imu_calibration_node->mutable_orientation();
75-
problem->AddParameterBlock(imu_orientation->data(), imu_orientation->size());
76-
if (imu_calibration_node->constant()) {
77-
problem->SetParameterBlockConstant(imu_orientation->data());
78-
}
79-
80-
problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */,
80+
AddRotation3D(first_node, problem);
81+
AddRotation3D(second_node, problem);
82+
AddImuOrientation(imu_node, problem);
83+
problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(),
8184
first_node->mutable_rotation()->data(),
8285
second_node->mutable_rotation()->data(),
83-
imu_orientation->data());
86+
imu_node->mutable_orientation()->data());
8487
}
8588

8689
proto::CostFunction RotationContraint3D::ToCostFunctionProto() const {

‎cartographer/transform/rigid_transform.h

+7-26
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323

2424
#include "Eigen/Core"
2525
#include "Eigen/Geometry"
26+
#include "absl/strings/substitute.h"
2627
#include "cartographer/common/lua_parameter_dictionary.h"
2728
#include "cartographer/common/math.h"
2829
#include "cartographer/common/port.h"
@@ -77,15 +78,8 @@ class Rigid2 {
7778
}
7879

7980
std::string DebugString() const {
80-
std::string out;
81-
out.append("{ t: [");
82-
out.append(std::to_string(translation().x()));
83-
out.append(", ");
84-
out.append(std::to_string(translation().y()));
85-
out.append("], r: [");
86-
out.append(std::to_string(rotation().angle()));
87-
out.append("] }");
88-
return out;
81+
return absl::Substitute("{ t: [$0, $1], r: [$2] }", translation().x(),
82+
translation().y(), rotation().angle());
8983
}
9084

9185
private:
@@ -169,23 +163,10 @@ class Rigid3 {
169163
}
170164

171165
std::string DebugString() const {
172-
std::string out;
173-
out.append("{ t: [");
174-
out.append(std::to_string(translation().x()));
175-
out.append(", ");
176-
out.append(std::to_string(translation().y()));
177-
out.append(", ");
178-
out.append(std::to_string(translation().z()));
179-
out.append("], q: [");
180-
out.append(std::to_string(rotation().w()));
181-
out.append(", ");
182-
out.append(std::to_string(rotation().x()));
183-
out.append(", ");
184-
out.append(std::to_string(rotation().y()));
185-
out.append(", ");
186-
out.append(std::to_string(rotation().z()));
187-
out.append("] }");
188-
return out;
166+
return absl::Substitute("{ t: [$0, $1, $2], q: [$3, $4, $5, $6] }",
167+
translation().x(), translation().y(),
168+
translation().z(), rotation().w(), rotation().x(),
169+
rotation().y(), rotation().z());
189170
}
190171

191172
bool IsValid() const {

0 commit comments

Comments
 (0)