├── CMakeLists.txt
├── README.md
├── include
    └── localization.h
├── package.xml
└── src
    └── localization.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
 2 | project(slam_backend)
 3 | 
 4 | find_package(catkin REQUIRED COMPONENTS
 5 |   roscpp
 6 | )
 7 | 
 8 | find_package(Boost REQUIRED COMPONENTS system)
 9 | find_package(cmake_modules REQUIRED)
10 | find_package(Eigen REQUIRED)
11 | find_package(GTSAM REQUIRED)
12 | include_directories(include ${GTSAM_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR})
13 | add_definitions(-std=c++11)
14 | 
15 | ###################################
16 | ## catkin specific configuration ##
17 | ###################################
18 | catkin_package(
19 |  INCLUDE_DIRS include
20 |  LIBRARIES localization
21 |  CATKIN_DEPENDS roscpp 
22 |  DEPENDS gtsam
23 | )
24 | 
25 | add_library (localization src/localization.cpp)
26 | add_dependencies(localization
27 |   ${catkin_EXPORTED_TARGETS}
28 | )
29 | target_link_libraries (localization gtsam ${catkin_LIBRARIES} ${Boost_LIBRARIES})
30 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # slam_backend README
 2 | This is the SLAVE README. Read it after reading the README in the `slam` repo
 3 | 
 4 | ## iSAM2-based backend interface for 2D Pose Graph SLAM
 5 | 
 6 | Build by cloning this into the src directory of your catkin workspace and then doing catkin_make.
 7 | 
 8 | ### Requires GTSAM.
 9 | To install GTSAM, follow the instructions (https://bitbucket.org/gtborg/gtsam), cloning into a folder outside your catkin workspace.
10 | 
11 | $ mkdir build
12 | 
13 | $ cd build
14 | 
15 | $ cmake ..
16 | 
17 | $ ccmake .. (set GTSAM_WITH_EIGEN_MKL and GTSAM_WITH_EIGEN_MKL_OPENMP to OFF)
18 | 
19 | $ make check (optional, runs unit tests)
20 | 
21 | $ make install
22 | 
23 | ## To Install SLAM Backend
24 | 
25 | Clone into your catkin workspace source folder and then catkin_make.
26 | 
27 | # Debugging
28 | Installing gtsam on Ubuntu 16.04:
29 | In file: /home/student/Downloads/gtsam-3.2.1/gtsam/base/FastSet.h
30 | add the line `#include <boost/serialization/serialization.hpp>`
31 | before the line `#include <boost/serialization/set.hpp>`
32 | 
33 | based on https://svn.boost.org/trac/boost/ticket/12126


--------------------------------------------------------------------------------
/include/localization.h:
--------------------------------------------------------------------------------
  1 | #ifndef LOCALIZATION_H
  2 | #define LOCALIZATION_H
  3 | 
  4 | #include <iostream>
  5 | #include <sstream>
  6 | #include <vector>
  7 | #include <unordered_map>
  8 | 
  9 | // GTSAM Stuff
 10 | #include <gtsam/geometry/Pose2.h>
 11 | 
 12 | // Each variable in the system (poses and landmarks) must be identified with a unique key.
 13 | // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
 14 | // Here we will use Symbols
 15 | #include <gtsam/inference/Symbol.h>
 16 | 
 17 | // We want to use iSAM2 to solve the structure-from-motion problem incrementally, so
 18 | // include iSAM2 here
 19 | #include <gtsam/nonlinear/ISAM2.h>
 20 | 
 21 | // iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
 22 | // and initial guesses for any new variables used in the added factors
 23 | #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 24 | #include <gtsam/nonlinear/Values.h>
 25 | 
 26 | // Once the optimized values have been calculated, we can also calculate the marginal covariance
 27 | // of desired variables
 28 | #include <gtsam/nonlinear/Marginals.h>
 29 | 
 30 | // In GTSAM, measurement functions are represented as 'factors'. Several common factors
 31 | // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
 32 | // Also, we will initialize the robot at some location using a Prior factor.
 33 | #include <gtsam/slam/PriorFactor.h>
 34 | #include <gtsam/slam/BetweenFactor.h>
 35 | #include <gtsam/slam/BearingRangeFactor.h>
 36 | 
 37 | namespace slam {
 38 | 
 39 | class Localization
 40 | {
 41 | 	public:
 42 |     // Constructors
 43 |     Localization();
 44 |     Localization(double isam2_relinearize_thresh, double isam2_relinearize_skip, double prior_trans_stddev, 
 45 |       double prior_rot_stddev, double odom_trans_stddev, double odom_rot_stddev, double land_obs_trans_stddev, 
 46 |       double land_obs_rot_stddev);
 47 |     // Destructor
 48 |     ~Localization();
 49 | 
 50 |     // 2D Pose Struct
 51 |     struct Pose2D {
 52 |       double x;
 53 |       double y;
 54 |       double theta;
 55 |     };
 56 | 
 57 |     // Landmark Info for HashMap
 58 |     struct LandmarkInfo {
 59 |       // Symbol of the landmark
 60 |       gtsam::Symbol land_sym;
 61 |       // Number of times the landmark is observed
 62 |       int num_obs;
 63 |       // Symbol of the robot pose at obs time
 64 |       gtsam::Symbol robot_pose_sym;
 65 |       // Measured transform from the robot (at obs time) to landmark
 66 |       gtsam::Pose2 robot_T_land;
 67 |       // Estimated pose of landmark in world
 68 |       gtsam::Pose2 world_T_land;
 69 |     };
 70 | 
 71 |     // Initializes localization
 72 |     void init_localization(double isam2_relinearize_thresh, double isam2_relinearize_skip, 
 73 |       double prior_trans_stddev, double prior_rot_stddev, double odom_trans_stddev, double odom_rot_stddev,
 74 |       double land_obs_trans_stddev, double land_obs_rot_stddev);
 75 | 
 76 |     // // Adds an odometry measurement to iSAM2 and returns the current estimated state
 77 |     // Localization::Pose2D add_odom_measurement(double x, double y, double theta);
 78 | 
 79 |     // Adds an odometry measurement to iSAM2 and returns the current estimated state
 80 |     Localization::Pose2D add_odom_measurement(double odom_x, double odom_y, double odom_theta, 
 81 |         double global_x, double global_y, double global_theta);
 82 | 
 83 |     // Adds/stores a landmark measurement to iSAM2 and returns whether the factor graph can be optimized
 84 |     bool add_landmark_measurement(int landmark_id, double land_rel_x, double land_rel_y, double land_rel_theta, 
 85 |       double land_glob_x, double land_glob_y, double land_glob_theta);
 86 | 
 87 |     // Optimizes the factor graph
 88 |     void optimize_factor_graph();
 89 | 
 90 |     // Returns the estimated robot pose
 91 |     Localization::Pose2D get_est_robot_pose();
 92 | 
 93 | 	private:
 94 |     // Initialize iSAM2 with parameters
 95 |     void initialize_isam2();
 96 | 
 97 |     // Initialize the factor graph
 98 |     void initialize_factor_graph();
 99 | 
100 |     // Initialize the noise models
101 |     void initialize_noise_models();
102 | 
103 |     // Map of the landmark ids to LandmarkInfo
104 |     std::unordered_map<int, Localization::LandmarkInfo> landmark_id_map_;
105 | 
106 |     std::string landmark_name_;
107 |     std::string robot_name_;
108 |     std::string est_robot_name_;
109 |     std::string est_landmark_name_;
110 | 
111 |     gtsam::Symbol current_robot_sym_;
112 | 
113 |     // Counters for the robot pose and landmarks
114 |     int robot_pose_counter_;
115 |     int landmark_obs_counter_;
116 | 
117 |     // Only relinearize variables whose linear delta magnitude is greater than this threshold (default: 0.1). 
118 |     double isam2_relinearize_thresh_;
119 |     // Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10) 
120 |     int isam2_relinearize_skip_;
121 | 
122 |     // Standard deviation of the translation and rotation components of the robot pose prior
123 |     double prior_trans_stddev_;
124 |     double prior_rot_stddev_;
125 |     // Standard deviation of the translation and rotation components of the odometry measurements
126 |     double odom_trans_stddev_;
127 |     double odom_rot_stddev_;
128 |     // Standard deviation of the translation and rotation components of the landmark observations
129 |     double land_obs_trans_stddev_;
130 |     double land_obs_rot_stddev_;
131 | 
132 |     // isam2 solver
133 |     gtsam::ISAM2 isam2_;
134 | 
135 |     // The iSAM2 estimated state
136 |     gtsam::Values est_state_;
137 |     // The estimated pose of the robot
138 |     gtsam::Pose2 est_robot_pose_;
139 |     // The estimated pose of the landmark
140 |     gtsam::Pose2 est_landmark_pose_;
141 | 
142 |     // The pose of the robot at the last time an optimization was done
143 |     gtsam::Pose2 opt_robot_pose_;
144 | 
145 |     // Transformation from the last optimized pose to the current optimized pose
146 |     gtsam::Pose2 last_opt_robot_T_robot_now_;
147 | 
148 |     // Create a Factor Graph and Values to hold the new data
149 |     gtsam::NonlinearFactorGraph factor_graph_;
150 |     gtsam::Values init_est_;
151 | 
152 |     // Noise model of the prior pose noise
153 |     gtsam::noiseModel::Diagonal::shared_ptr prior_pose_noise_;
154 |     // Noise model of the robot odometry
155 |     gtsam::noiseModel::Diagonal::shared_ptr odom_noise_;
156 |     // Noise model of the landmark observations
157 |     gtsam::noiseModel::Diagonal::shared_ptr land_obs_noise_;
158 | };
159 | 
160 | }  // namespace slam
161 | 
162 | #endif // LOCALIZATION_H


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>slam_backend</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The slam_backend package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="vkee@todo.todo">vkee</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/live_icp</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, mutiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintianers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *_depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use build_depend for packages you need at compile time: -->
35 |   <!--   <build_depend>message_generation</build_depend> -->
36 |   <!-- Use buildtool_depend for build tool packages: -->
37 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
38 |   <!-- Use run_depend for packages you need at runtime: -->
39 |   <!--   <run_depend>message_runtime</run_depend> -->
40 |   <!-- Use test_depend for packages you need only for testing: -->
41 |   <!--   <test_depend>gtest</test_depend> -->
42 |   <buildtool_depend>catkin</buildtool_depend>
43 |   <build_depend>roscpp</build_depend>
44 |   <build_depend>gtsam</build_depend>
45 |   <run_depend>gtsam</run_depend>
46 |   <run_depend>roscpp</run_depend>
47 | 
48 |   <!-- The export tag contains other, unspecified, tags -->
49 |   <export>
50 |     <!-- Other tools can request additional information be placed here -->
51 | 
52 |   </export>
53 | </package>


--------------------------------------------------------------------------------
/src/localization.cpp:
--------------------------------------------------------------------------------
  1 | #include "localization.h"
  2 | 
  3 | namespace slam {
  4 | 
  5 | Localization::Localization():robot_pose_counter_ (0), landmark_obs_counter_(0)
  6 | {
  7 | }
  8 | 
  9 | Localization::Localization(double isam2_relinearize_thresh, double isam2_relinearize_skip, 
 10 |     double prior_trans_stddev, double prior_rot_stddev, double odom_trans_stddev, double odom_rot_stddev,
 11 |     double land_obs_trans_stddev, double land_obs_rot_stddev): 
 12 |       isam2_relinearize_thresh_(isam2_relinearize_thresh), isam2_relinearize_skip_(isam2_relinearize_skip), 
 13 |       prior_trans_stddev_(prior_trans_stddev), prior_rot_stddev_(prior_rot_stddev), 
 14 |       odom_trans_stddev_(odom_trans_stddev), odom_rot_stddev_(odom_rot_stddev), 
 15 |       land_obs_trans_stddev_(land_obs_trans_stddev), land_obs_rot_stddev_(land_obs_rot_stddev),
 16 |       robot_pose_counter_ (0), landmark_obs_counter_(0)
 17 | {
 18 |   // Initialize iSAM2
 19 |   initialize_isam2();
 20 | 
 21 |   // Initialize the noise models
 22 |   initialize_noise_models();
 23 | 
 24 |   // Initialize the factor graph
 25 |   initialize_factor_graph();
 26 | }
 27 | 
 28 | Localization::~Localization()
 29 | {
 30 | }
 31 | 
 32 | // Initializes localization
 33 | void Localization::init_localization(double isam2_relinearize_thresh, double isam2_relinearize_skip, 
 34 |     double prior_trans_stddev, double prior_rot_stddev, double odom_trans_stddev, double odom_rot_stddev,
 35 |     double land_obs_trans_stddev, double land_obs_rot_stddev)
 36 | {
 37 |   isam2_relinearize_thresh_ = isam2_relinearize_thresh;
 38 |   isam2_relinearize_skip_ = isam2_relinearize_skip;
 39 |   prior_trans_stddev_ = prior_trans_stddev;
 40 |   prior_rot_stddev_ = prior_rot_stddev;
 41 |   odom_trans_stddev_ = odom_trans_stddev;
 42 |   odom_rot_stddev_ = odom_rot_stddev_;
 43 |   land_obs_trans_stddev_ = land_obs_trans_stddev;
 44 |   land_obs_rot_stddev_ = land_obs_rot_stddev;      
 45 | 
 46 |   // Initialize iSAM2
 47 |   initialize_isam2();
 48 | 
 49 |   // Initialize the noise models
 50 |   initialize_noise_models();
 51 | 
 52 |   // Initialize the factor graph
 53 |   initialize_factor_graph();
 54 | }
 55 | 
 56 | // Initialize iSAM2 with parameters
 57 | void Localization::initialize_isam2()
 58 | {
 59 |   gtsam::ISAM2Params parameters;
 60 |   parameters.relinearizeThreshold = isam2_relinearize_thresh_;
 61 |   parameters.relinearizeSkip = isam2_relinearize_skip_;
 62 |   isam2_ = gtsam::ISAM2(parameters);
 63 | }
 64 | 
 65 | // Initialize the factor graph
 66 | void Localization::initialize_factor_graph()
 67 | {
 68 |   // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
 69 |   gtsam::Pose2 prior_pose; // prior mean is at origin
 70 | 
 71 |   current_robot_sym_ = gtsam::Symbol('x', robot_pose_counter_);
 72 | 
 73 |   factor_graph_.add(gtsam::PriorFactor<gtsam::Pose2>(current_robot_sym_, gtsam::Pose2(), prior_pose_noise_)); // add directly to graph
 74 |   init_est_.insert(current_robot_sym_, gtsam::Pose2());
 75 | }
 76 | 
 77 | // Initialize the noise models
 78 | void Localization::initialize_noise_models()
 79 | {
 80 |   gtsam::Vector prior_sigmas(3);
 81 |   prior_sigmas << prior_trans_stddev_, prior_trans_stddev_, prior_rot_stddev_;
 82 |   prior_pose_noise_ = gtsam::noiseModel::Diagonal::Sigmas(prior_sigmas);
 83 | 
 84 |   gtsam::Vector odom_sigmas(3);
 85 |   odom_sigmas << odom_trans_stddev_, odom_trans_stddev_, odom_rot_stddev_;
 86 |   odom_noise_ = gtsam::noiseModel::Diagonal::Sigmas(odom_sigmas);
 87 | 
 88 |   // gtsam::Vector obs_sigmas(3);
 89 |   // obs_sigmas << land_obs_trans_stddev_, land_obs_trans_stddev_, land_obs_rot_stddev_;
 90 |   // land_obs_noise_ = gtsam::noiseModel::Diagonal::Sigmas(obs_sigmas);
 91 | 
 92 |   // Because now doing range bearing measurements
 93 |   gtsam::Vector obs_sigmas(2);
 94 |   obs_sigmas << land_obs_rot_stddev_, land_obs_trans_stddev_;
 95 |   land_obs_noise_ = gtsam::noiseModel::Diagonal::Sigmas(obs_sigmas);
 96 | }
 97 | 
 98 | // Adds an odometry measurement to iSAM2 and returns the current estimated state
 99 | Localization::Pose2D Localization::add_odom_measurement(double odom_x, double odom_y, double odom_theta,
100 |   double global_x, double global_y, double global_theta)
101 | {
102 |   robot_pose_counter_ += 1;
103 |   gtsam::Symbol next_robot_sym = gtsam::Symbol('x', robot_pose_counter_);
104 | 
105 |   // Update the factor graph with the transformation (need to increment the counter too)
106 |   gtsam::Pose2 robot_odometry(odom_x, odom_y, odom_theta);
107 |   factor_graph_.add(gtsam::BetweenFactor<gtsam::Pose2> (current_robot_sym_, next_robot_sym, robot_odometry, odom_noise_));
108 | 
109 |   current_robot_sym_ = next_robot_sym;
110 | 
111 |   // Generating the initial estimates
112 |   if (robot_pose_counter_ == 1)
113 |   {
114 |     // Estimate is just odom b/c prior is origin
115 |     init_est_.insert(current_robot_sym_, robot_odometry);
116 |     est_robot_pose_ = robot_odometry;
117 |   }
118 | 
119 |   else 
120 |   {
121 |     // Estimate is global pose
122 | 
123 |     gtsam::Pose2 est(global_x, global_y, global_theta);
124 |     init_est_.insert(current_robot_sym_, est);
125 |     est_robot_pose_ = est;
126 |   }
127 | }
128 | 
129 | // Adds/stores a landmark measurement to iSAM2 and returns whether the factor graph can be optimized
130 | bool Localization::add_landmark_measurement(int landmark_id, double land_rel_x, double land_rel_y, double land_rel_theta, 
131 |   double land_glob_x, double land_glob_y, double land_glob_theta)
132 | {
133 |   // Check if the landmark has been observed before
134 |   if (landmark_id_map_.count(landmark_id) == 1)
135 |   {
136 |     // Get the landmark info
137 |     Localization::LandmarkInfo landmark_info = landmark_id_map_.at(landmark_id);
138 | 
139 |     int num_obs = landmark_info.num_obs;
140 |     gtsam::Symbol land_sym = landmark_info.land_sym;
141 | 
142 |     // If the landmark was observed only once, put the previous landmark measurement into the factor graph and add the initial estimate of the landmark
143 |     if (num_obs == 1)
144 |     {
145 |       // Add the previous landmark measurement to the factor graph from the robot pose symbol
146 |       // factor_graph_.add(gtsam::BetweenFactor<gtsam::Pose2> (landmark_info.robot_pose_sym, land_sym, landmark_info.robot_T_land, land_obs_noise_));
147 |       
148 |       // Add the previous landmark measurement to the factor graph from the robot pose symbol
149 |       double x = landmark_info.robot_T_land.x();
150 |       double y = landmark_info.robot_T_land.y();
151 |       double range = sqrt(pow(x, 2) + pow(y, 2));
152 |       factor_graph_.add(gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2>(
153 |         landmark_info.robot_pose_sym, land_sym, landmark_info.robot_T_land.theta(), range, land_obs_noise_));
154 |       
155 |       // Add the initial estimate
156 |       // init_est_.insert(land_sym, landmark_info.world_T_land);
157 |       init_est_.insert(land_sym, gtsam::Point2(landmark_info.world_T_land.x(), landmark_info.world_T_land.y()));
158 |     }
159 | 
160 |     // Construct the current landmark measurement
161 |     // gtsam::Pose2 curr_land_meas(land_rel_x, land_rel_y, land_rel_theta);
162 |     // Add the current measurement
163 |     // factor_graph_.add(gtsam::BetweenFactor<gtsam::Pose2> (current_robot_sym_, land_sym, curr_land_meas, land_obs_noise_));
164 |     double range = sqrt(pow(land_rel_x, 2) + pow(land_rel_y, 2));
165 |     factor_graph_.add(gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2>(
166 |       current_robot_sym_, land_sym, land_rel_theta, range, land_obs_noise_));
167 | 
168 |     // Update the dictionary (only the num_obs needs to be updated)
169 |     // Increase the number of times the landmark was observed
170 |     landmark_info.num_obs = landmark_info.num_obs + 1;
171 | 
172 |     // Update the dictionary entry
173 |     landmark_id_map_[landmark_id] = landmark_info;
174 | 
175 |     // Can optimize the factor graph
176 |     return true;
177 |   }
178 | 
179 |   // Case where the landmark has not been observed before
180 |   else
181 |   {
182 |     landmark_obs_counter_ += 1;
183 | 
184 |     // Creating the new landmark symbol and putting it in the dictionary
185 |     gtsam::Symbol next_landmark_sym = gtsam::Symbol('l', landmark_obs_counter_);
186 | 
187 |     // Create the landmark entry 
188 |     Localization::LandmarkInfo landmark_info;
189 | 
190 |     landmark_info.land_sym = next_landmark_sym;
191 |     landmark_info.num_obs = 1;
192 |     landmark_info.robot_pose_sym = current_robot_sym_;
193 |     // Construct the current landmark measurement
194 |     gtsam::Pose2 curr_land_meas(land_rel_x, land_rel_y, land_rel_theta);
195 |     landmark_info.robot_T_land = curr_land_meas;
196 |     // Construct the global landmark measurement
197 |     gtsam::Pose2 glob_land_meas(land_glob_x, land_glob_y, land_glob_theta);
198 |     landmark_info.world_T_land = glob_land_meas;
199 | 
200 |     landmark_id_map_[landmark_id] = landmark_info;
201 |     return false;
202 |   }
203 | }
204 | 
205 | // Optimizes the factor graph
206 | void Localization::optimize_factor_graph()
207 | {
208 |   // Update iSAM with the new factors
209 |   isam2_.update(factor_graph_, init_est_);
210 |   isam2_.update();
211 |   // // TODO: may want to call update for more accuracy but more time
212 | 
213 |   // Get the current iSAM2 estimate
214 |   est_state_ = isam2_.calculateEstimate();
215 |   // Update the current estimated robot pose
216 |   est_robot_pose_ = est_state_.at<gtsam::Pose2>(current_robot_sym_);
217 |   opt_robot_pose_ = est_robot_pose_;
218 |   // Identity since the optimized pose is the current robot pose
219 |   last_opt_robot_T_robot_now_ = gtsam::Pose2();
220 | 
221 |   // Clear the factor graph and values for the next iteration
222 |   factor_graph_.resize(0);
223 |   init_est_.clear();
224 | }
225 | 
226 | // Returns the estimated robot pose
227 | Localization::Pose2D Localization::get_est_robot_pose()
228 | {
229 |   Localization::Pose2D pose2D;
230 |   pose2D.x = est_robot_pose_.x();
231 |   pose2D.y = est_robot_pose_.y();
232 |   pose2D.theta = est_robot_pose_.theta();
233 | 
234 |   return pose2D;
235 | }
236 | }  // namespace slam


--------------------------------------------------------------------------------