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

[GenericPoseGraph] Add a macro to find nodes. (#1382)

master
Alexander Belyaev 2018-08-10 18:55:43 +02:00 committed by GitHub
parent 3877f97b23
commit b6b41e9b17
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 46 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -36,29 +36,15 @@ AccelerationConstraint3D::AccelerationConstraint3D(
void AccelerationConstraint3D::AddToOptimizer(Nodes* nodes,
ceres::Problem* problem) const {
auto first_node = common::FindOrNull(nodes->pose_3d_nodes, first_);
if (first_node == nullptr) {
LOG(INFO) << "First node was not found in pose_3d_nodes.";
return;
}
auto second_node = common::FindOrNull(nodes->pose_3d_nodes, second_);
if (second_node == nullptr) {
LOG(INFO) << "Second node was not found in pose_3d_nodes.";
return;
}
auto third_node = common::FindOrNull(nodes->pose_3d_nodes, third_);
if (third_node == nullptr) {
LOG(INFO) << "Third node was not found in pose_3d_nodes.";
return;
}
auto imu_node = common::FindOrNull(nodes->imu_calibration_nodes, imu_);
if (imu_node == nullptr) {
LOG(INFO) << "IMU calibration node was not found in imu_calibration_nodes.";
return;
}
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()) {

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

@ -31,6 +31,13 @@ void AddPose3D(Pose3D* pose, ceres::Problem* problem);
void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem);
#define FIND_NODE_OR_RETURN(node_name, node_id, map, log_message) \
auto node_name = common::FindOrNull(map, node_id); \
if (node_name == nullptr) { \
LOG(INFO) << log_message; \
return; \
}
} // namespace pose_graph
} // namespace cartographer

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

@ -35,24 +35,12 @@ InterpolatedRelativePoseConstraint2D::InterpolatedRelativePoseConstraint2D(
void InterpolatedRelativePoseConstraint2D::AddToOptimizer(
Nodes* nodes, ceres::Problem* problem) const {
auto first_node_start =
common::FindOrNull(nodes->pose_2d_nodes, first_start_);
if (first_node_start == nullptr) {
LOG(INFO) << "First node (start) was not found in pose_2d_nodes.";
return;
}
auto first_node_end = common::FindOrNull(nodes->pose_2d_nodes, first_end_);
if (first_node_end == nullptr) {
LOG(INFO) << "First node (end) was not found in pose_2d_nodes.";
return;
}
auto second_node = common::FindOrNull(nodes->pose_3d_nodes, second_);
if (second_node == nullptr) {
LOG(INFO) << "Second node was not found in pose_3d_nodes.";
return;
}
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()) {

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

@ -35,24 +35,12 @@ InterpolatedRelativePoseConstraint3D::InterpolatedRelativePoseConstraint3D(
void InterpolatedRelativePoseConstraint3D::AddToOptimizer(
Nodes* nodes, ceres::Problem* problem) const {
auto first_node_start =
common::FindOrNull(nodes->pose_3d_nodes, first_start_);
if (first_node_start == nullptr) {
LOG(INFO) << "First node (start) was not found in pose_3d_nodes.";
return;
}
auto first_node_end = common::FindOrNull(nodes->pose_3d_nodes, first_end_);
if (first_node_end == nullptr) {
LOG(INFO) << "First node (end) was not found in pose_3d_nodes.";
return;
}
auto second_node = common::FindOrNull(nodes->pose_3d_nodes, second_);
if (second_node == nullptr) {
LOG(INFO) << "Second node was not found in pose_3d_nodes.";
return;
}
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()) {

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

@ -34,17 +34,10 @@ RelativePoseConstraint2D::RelativePoseConstraint2D(
// TODO(pifon): Add a test.
void RelativePoseConstraint2D::AddToOptimizer(Nodes* nodes,
ceres::Problem* problem) const {
auto first_node = common::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 = common::FindOrNull(nodes->pose_2d_nodes, second_);
if (second_node == nullptr) {
LOG(INFO) << "Second node was not found in pose_2d_nodes.";
return;
}
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.";

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

@ -34,17 +34,10 @@ RelativePoseConstraint3D::RelativePoseConstraint3D(
void RelativePoseConstraint3D::AddToOptimizer(Nodes* nodes,
ceres::Problem* problem) const {
auto first_node = common::FindOrNull(nodes->pose_3d_nodes, first_);
if (first_node == nullptr) {
LOG(INFO) << "First node was not found in pose_3d_nodes.";
return;
}
auto second_node = common::FindOrNull(nodes->pose_3d_nodes, second_);
if (second_node == nullptr) {
LOG(INFO) << "Second node was not found in pose_3d_nodes.";
return;
}
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.";

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

@ -17,6 +17,8 @@
#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 {
@ -53,30 +55,19 @@ RotationContraint3D::RotationContraint3D(
void RotationContraint3D::AddToOptimizer(Nodes* nodes,
ceres::Problem* problem) const {
auto first_node = common::FindOrNull(nodes->pose_3d_nodes, first_);
if (first_node == nullptr) {
LOG(INFO) << "First node was not found in pose_3d_nodes.";
return;
}
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.");
auto second_node = common::FindOrNull(nodes->pose_3d_nodes, second_);
if (second_node == nullptr) {
LOG(INFO) << "Second node was not found in pose_3d_nodes.";
return;
}
if (first_node->constant() && second_node->constant()) {
if (first_node->constant() && second_node->constant() &&
imu_node->constant()) {
LOG(INFO) << "Both nodes are constant, skipping the constraint.";
return;
}
auto imu_node =
common::FindOrNull(nodes->imu_calibration_nodes, imu_calibration_);
if (imu_node == nullptr) {
LOG(INFO) << "Imu calibration node was not found.";
return;
}
AddRotation3D(first_node, problem);
AddRotation3D(second_node, problem);
AddImuOrientation(imu_node, problem);