diff --git a/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc b/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc
index b7fbdbd..b0b36ab 100644
--- a/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc
+++ b/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc
@@ -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()) {
diff --git a/cartographer/pose_graph/constraint/constraint_utils.h b/cartographer/pose_graph/constraint/constraint_utils.h
index f667fcf..b46f4df 100644
--- a/cartographer/pose_graph/constraint/constraint_utils.h
+++ b/cartographer/pose_graph/constraint/constraint_utils.h
@@ -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
 
diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc
index e164b82..0f30b15 100644
--- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc
+++ b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc
@@ -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()) {
diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc
index 059bc65..dc3f63e 100644
--- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc
+++ b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc
@@ -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()) {
diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc
index 7fa34e9..9b7faa6 100644
--- a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc
+++ b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc
@@ -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.";
diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc
index fe4a717..dc19b8b 100644
--- a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc
+++ b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc
@@ -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.";
diff --git a/cartographer/pose_graph/constraint/rotation_constraint_3d.cc b/cartographer/pose_graph/constraint/rotation_constraint_3d.cc
index 2455f2d..f1ed3db 100644
--- a/cartographer/pose_graph/constraint/rotation_constraint_3d.cc
+++ b/cartographer/pose_graph/constraint/rotation_constraint_3d.cc
@@ -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);