Add solver for april tag mapping
Takes in robot locations (at control loop frequency), and less frequent
localizations off of april tags. Builds a graph of constraints between
subsequent target detections and solves for the optimal target locations
on the field.
Change-Id: I1256f5914b7656d9e14735f9d994572688c3aaa8
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
diff --git a/frc971/vision/ceres/BUILD b/frc971/vision/ceres/BUILD
new file mode 100644
index 0000000..5622b55
--- /dev/null
+++ b/frc971/vision/ceres/BUILD
@@ -0,0 +1,24 @@
+# Copy of ceres example code from their repo
+cc_library(
+ name = "pose_graph_2d_lib",
+ hdrs = [
+ "angle_local_parameterization.h",
+ "normalize_angle.h",
+ "pose_graph_2d_error_term.h",
+ "read_g2o.h",
+ "types.h",
+ ],
+ copts = [
+ # Needed to silence GFlags complaints.
+ "-Wno-sign-compare",
+ "-Wno-unused-parameter",
+ "-Wno-format-nonliteral",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "@com_github_gflags_gflags//:gflags",
+ "@com_google_ceres_solver//:ceres",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
diff --git a/frc971/vision/ceres/angle_local_parameterization.h b/frc971/vision/ceres/angle_local_parameterization.h
new file mode 100644
index 0000000..a81637c
--- /dev/null
+++ b/frc971/vision/ceres/angle_local_parameterization.h
@@ -0,0 +1,64 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+
+#include "ceres/local_parameterization.h"
+#include "normalize_angle.h"
+
+namespace ceres {
+namespace examples {
+
+// Defines a local parameterization for updating the angle to be constrained in
+// [-pi to pi).
+class AngleLocalParameterization {
+ public:
+ template <typename T>
+ bool operator()(const T* theta_radians,
+ const T* delta_theta_radians,
+ T* theta_radians_plus_delta) const {
+ *theta_radians_plus_delta =
+ NormalizeAngle(*theta_radians + *delta_theta_radians);
+
+ return true;
+ }
+
+ static ceres::LocalParameterization* Create() {
+ return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
+ 1,
+ 1>);
+ }
+};
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
diff --git a/frc971/vision/ceres/normalize_angle.h b/frc971/vision/ceres/normalize_angle.h
new file mode 100644
index 0000000..c215671
--- /dev/null
+++ b/frc971/vision/ceres/normalize_angle.h
@@ -0,0 +1,53 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
+
+#include <cmath>
+
+#include "ceres/ceres.h"
+
+namespace ceres {
+namespace examples {
+
+// Normalizes the angle in radians between [-pi and pi).
+template <typename T>
+inline T NormalizeAngle(const T& angle_radians) {
+ // Use ceres::floor because it is specialized for double and Jet types.
+ T two_pi(2.0 * M_PI);
+ return angle_radians -
+ two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
+}
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
diff --git a/frc971/vision/ceres/pose_graph_2d_error_term.h b/frc971/vision/ceres/pose_graph_2d_error_term.h
new file mode 100644
index 0000000..2df31f6
--- /dev/null
+++ b/frc971/vision/ceres/pose_graph_2d_error_term.h
@@ -0,0 +1,119 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Cost function for a 2D pose graph formulation.
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
+
+#include "Eigen/Core"
+
+namespace ceres {
+namespace examples {
+
+template <typename T>
+Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians) {
+ const T cos_yaw = ceres::cos(yaw_radians);
+ const T sin_yaw = ceres::sin(yaw_radians);
+
+ Eigen::Matrix<T, 2, 2> rotation;
+ rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
+ return rotation;
+}
+
+// Computes the error term for two poses that have a relative pose measurement
+// between them. Let the hat variables be the measurement.
+//
+// residual = information^{1/2} * [ r_a^T * (p_b - p_a) - \hat{p_ab} ]
+// [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ]
+//
+// where r_a is the rotation matrix that rotates a vector represented in frame A
+// into the global frame, and Normalize(*) ensures the angles are in the range
+// [-pi, pi).
+class PoseGraph2dErrorTerm {
+ public:
+ PoseGraph2dErrorTerm(double x_ab,
+ double y_ab,
+ double yaw_ab_radians,
+ const Eigen::Matrix3d& sqrt_information)
+ : p_ab_(x_ab, y_ab),
+ yaw_ab_radians_(yaw_ab_radians),
+ sqrt_information_(sqrt_information) {}
+
+ template <typename T>
+ bool operator()(const T* const x_a,
+ const T* const y_a,
+ const T* const yaw_a,
+ const T* const x_b,
+ const T* const y_b,
+ const T* const yaw_b,
+ T* residuals_ptr) const {
+ const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
+ const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
+
+ Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);
+
+ residuals_map.template head<2>() =
+ RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
+ residuals_map(2) = ceres::examples::NormalizeAngle(
+ (*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
+
+ // Scale the residuals by the square root information matrix to account for
+ // the measurement uncertainty.
+ residuals_map = sqrt_information_.template cast<T>() * residuals_map;
+
+ return true;
+ }
+
+ static ceres::CostFunction* Create(double x_ab,
+ double y_ab,
+ double yaw_ab_radians,
+ const Eigen::Matrix3d& sqrt_information) {
+ return (new ceres::
+ AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
+ new PoseGraph2dErrorTerm(
+ x_ab, y_ab, yaw_ab_radians, sqrt_information)));
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+ // The position of B relative to A in the A frame.
+ const Eigen::Vector2d p_ab_;
+ // The orientation of frame B relative to frame A.
+ const double yaw_ab_radians_;
+ // The inverse square root of the measurement covariance matrix.
+ const Eigen::Matrix3d sqrt_information_;
+};
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
diff --git a/frc971/vision/ceres/read_g2o.h b/frc971/vision/ceres/read_g2o.h
new file mode 100644
index 0000000..fea32e9
--- /dev/null
+++ b/frc971/vision/ceres/read_g2o.h
@@ -0,0 +1,143 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Reads a file in the g2o filename format that describes a pose graph problem.
+
+#ifndef EXAMPLES_CERES_READ_G2O_H_
+#define EXAMPLES_CERES_READ_G2O_H_
+
+#include <fstream>
+#include <string>
+
+#include "glog/logging.h"
+
+namespace ceres {
+namespace examples {
+
+// Reads a single pose from the input and inserts it into the map. Returns false
+// if there is a duplicate entry.
+template <typename Pose, typename Allocator>
+bool ReadVertex(std::ifstream* infile,
+ std::map<int, Pose, std::less<int>, Allocator>* poses) {
+ int id;
+ Pose pose;
+ *infile >> id >> pose;
+
+ // Ensure we don't have duplicate poses.
+ if (poses->find(id) != poses->end()) {
+ LOG(ERROR) << "Duplicate vertex with ID: " << id;
+ return false;
+ }
+ (*poses)[id] = pose;
+
+ return true;
+}
+
+// Reads the contraints between two vertices in the pose graph
+template <typename Constraint, typename Allocator>
+void ReadConstraint(std::ifstream* infile,
+ std::vector<Constraint, Allocator>* constraints) {
+ Constraint constraint;
+ *infile >> constraint;
+
+ constraints->push_back(constraint);
+}
+
+// Reads a file in the g2o filename format that describes a pose graph
+// problem. The g2o format consists of two entries, vertices and constraints.
+//
+// In 2D, a vertex is defined as follows:
+//
+// VERTEX_SE2 ID x_meters y_meters yaw_radians
+//
+// A constraint is defined as follows:
+//
+// EDGE_SE2 ID_A ID_B A_x_B A_y_B A_yaw_B I_11 I_12 I_13 I_22 I_23 I_33
+//
+// where I_ij is the (i, j)-th entry of the information matrix for the
+// measurement.
+//
+//
+// In 3D, a vertex is defined as follows:
+//
+// VERTEX_SE3:QUAT ID x y z q_x q_y q_z q_w
+//
+// where the quaternion is in Hamilton form.
+// A constraint is defined as follows:
+//
+// EDGE_SE3:QUAT ID_a ID_b x_ab y_ab z_ab q_x_ab q_y_ab q_z_ab q_w_ab I_11 I_12 I_13 ... I_16 I_22 I_23 ... I_26 ... I_66 // NOLINT
+//
+// where I_ij is the (i, j)-th entry of the information matrix for the
+// measurement. Only the upper-triangular part is stored. The measurement order
+// is the delta position followed by the delta orientation.
+template <typename Pose,
+ typename Constraint,
+ typename MapAllocator,
+ typename VectorAllocator>
+bool ReadG2oFile(const std::string& filename,
+ std::map<int, Pose, std::less<int>, MapAllocator>* poses,
+ std::vector<Constraint, VectorAllocator>* constraints) {
+ CHECK(poses != NULL);
+ CHECK(constraints != NULL);
+
+ poses->clear();
+ constraints->clear();
+
+ std::ifstream infile(filename.c_str());
+ if (!infile) {
+ return false;
+ }
+
+ std::string data_type;
+ while (infile.good()) {
+ // Read whether the type is a node or a constraint.
+ infile >> data_type;
+ if (data_type == Pose::name()) {
+ if (!ReadVertex(&infile, poses)) {
+ return false;
+ }
+ } else if (data_type == Constraint::name()) {
+ ReadConstraint(&infile, constraints);
+ } else {
+ LOG(ERROR) << "Unknown data type: " << data_type;
+ return false;
+ }
+
+ // Clear any trailing whitespace from the line.
+ infile >> std::ws;
+ }
+
+ return true;
+}
+
+} // namespace examples
+} // namespace ceres
+
+#endif // EXAMPLES_CERES_READ_G2O_H_
diff --git a/frc971/vision/ceres/types.h b/frc971/vision/ceres/types.h
new file mode 100644
index 0000000..3c13824
--- /dev/null
+++ b/frc971/vision/ceres/types.h
@@ -0,0 +1,101 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Defines the types used in the 2D pose graph SLAM formulation. Each vertex of
+// the graph has a unique integer ID with a position and orientation. There are
+// delta transformation constraints between two vertices.
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
+
+#include <fstream>
+
+#include "Eigen/Core"
+#include "normalize_angle.h"
+
+namespace ceres {
+namespace examples {
+
+// The state for each vertex in the pose graph.
+struct Pose2d {
+ double x;
+ double y;
+ double yaw_radians;
+
+ // The name of the data type in the g2o file format.
+ static std::string name() { return "VERTEX_SE2"; }
+};
+
+inline std::istream& operator>>(std::istream& input, Pose2d& pose) {
+ input >> pose.x >> pose.y >> pose.yaw_radians;
+ // Normalize the angle between -pi to pi.
+ pose.yaw_radians = NormalizeAngle(pose.yaw_radians);
+ return input;
+}
+
+// The constraint between two vertices in the pose graph. The constraint is the
+// transformation from vertex id_begin to vertex id_end.
+struct Constraint2d {
+ int id_begin;
+ int id_end;
+
+ double x;
+ double y;
+ double yaw_radians;
+
+ // The inverse of the covariance matrix for the measurement. The order of the
+ // entries are x, y, and yaw.
+ Eigen::Matrix3d information;
+
+ // The name of the data type in the g2o file format.
+ static std::string name() { return "EDGE_SE2"; }
+};
+
+inline std::istream& operator>>(std::istream& input, Constraint2d& constraint) {
+ input >> constraint.id_begin >> constraint.id_end >> constraint.x >>
+ constraint.y >> constraint.yaw_radians >> constraint.information(0, 0) >>
+ constraint.information(0, 1) >> constraint.information(0, 2) >>
+ constraint.information(1, 1) >> constraint.information(1, 2) >>
+ constraint.information(2, 2);
+
+ // Set the lower triangular part of the information matrix.
+ constraint.information(1, 0) = constraint.information(0, 1);
+ constraint.information(2, 0) = constraint.information(0, 2);
+ constraint.information(2, 1) = constraint.information(1, 2);
+
+ // Normalize the angle between -pi to pi.
+ constraint.yaw_radians = NormalizeAngle(constraint.yaw_radians);
+ return input;
+}
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_