Skip to content

Commit

Permalink
[GenericPoseGraph] Add a macro to find nodes. (#1382)
Browse files Browse the repository at this point in the history
  • Loading branch information
@pifon2a
pifon2a committed Aug 10, 2018
1 parent 3877f97 commit b6b41e9
Show file tree
Hide file tree
Showing 7 changed files with 46 additions and 100 deletions.
32 changes: 9 additions & 23 deletions cartographer/pose_graph/constraint/acceleration_constraint_3d.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Expand Down
7 changes: 7 additions & 0 deletions cartographer/pose_graph/constraint/constraint_utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
24 changes: 6 additions & 18 deletions cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Expand Down
24 changes: 6 additions & 18 deletions cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Expand Down
15 changes: 4 additions & 11 deletions cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.";
Expand Down
15 changes: 4 additions & 11 deletions cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.";
Expand Down
29 changes: 10 additions & 19 deletions cartographer/pose_graph/constraint/rotation_constraint_3d.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit b6b41e9

Please sign in to comment.