diff --git a/CMakeLists.txt b/CMakeLists.txt
index 947ec901fc91899617a35ce9a5885d9ef15b8d5d..d86286f103b61f77bfed474e450c52e037f57a04 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,6 +10,8 @@ find_package(catkin REQUIRED COMPONENTS iri_base_algorithm iri_adc_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
+find_package(Eigen3 REQUIRED)
+find_package(Ceres REQUIRED)
# ********************************************************************
# Add system and labrobotica dependencies here
@@ -76,6 +78,8 @@ catkin_package(
# ********************************************************************
include_directories(include)
include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${EIGEN3_INCLUDE_DIR})
+include_directories(${CERES_INCLUDE_DIRS})
# include_directories(${<dependency>_INCLUDE_DIRS})
## Declare a cpp library
@@ -88,6 +92,7 @@ add_executable(${PROJECT_NAME} src/adc_landmarks_slam_solver_alg.cpp src/adc_lan
# Add the libraries
# ********************************************************************
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${CERES_LIBRARIES})
# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARIES})
# ********************************************************************
diff --git a/include/adc_landmarks_slam_solver_alg.h b/include/adc_landmarks_slam_solver_alg.h
index 786fb6ad7716c8f97e46bd78179e283ac53fdefd..1a8e569bfa7b804056ac2d66a33564758a3dad33 100644
--- a/include/adc_landmarks_slam_solver_alg.h
+++ b/include/adc_landmarks_slam_solver_alg.h
@@ -26,9 +26,41 @@
#define _adc_landmarks_slam_solver_alg_h_
#include <iri_adc_landmarks_slam_solver/AdcLandmarksSlamSolverConfig.h>
+#include <Eigen/Eigen>
+#include "ceres/ceres.h"
+#include "ceres/loss_function.h"
+
+#include "estimated_pose_residual_2D.h"
+#include "odom_residual_2D.h"
+#include "landmark_residual_2D.h"
+#include "sensor_pose_residual_2D.h"
//include adc_landmarks_slam_solver_alg main library
+/**
+ * \struct LandmarkResidualData.
+ *
+ * \brief A struct with the necessary information to create a LandmarkErr object.
+ *
+ */
+typedef struct {
+ double r; ///< Range at detection
+ double theta; ///< Angle at detection
+}LandmarkResidualData;
+
+/**
+ * \struct FrameData.
+ *
+ * \brief A struct with all the Ceres problem information.
+ *
+ */
+typedef struct {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Eigen::Vector3d robot_state; ///< X and y position and orientation.
+ ros::Time ts; ///< Time stamp
+ std::vector<LandmarkResidualData> landmark_residuals_data; ///< The LandmarkResidualData information at that frame.
+}FrameData;
+
/**
* \brief IRI ROS Specific Driver Class
*
@@ -36,6 +68,12 @@
*/
class AdcLandmarksSlamSolverAlgorithm
{
+ private:
+ ceres::Solver::Options solv_opt_; ///< Ceres solver options.
+ ceres::Problem problem_; ///< Ceres problem.
+ ceres::Covariance::Options cov_opt_; ///< Ceres covariance options.
+
+ std::list<FrameData> frames_data_; ///< Frames information.
protected:
/**
* \brief define config type
@@ -48,6 +86,46 @@ class AdcLandmarksSlamSolverAlgorithm
// private attributes and methods
public:
+ /**
+ * \brief Function to solve the problem.
+ *
+ * \param _print_summary Flag to print ceres summary (default false).
+ *
+ */
+ void solve_problem(bool _print_summary = false);
+
+ /**
+ * \brief Function to set solver options.
+ *
+ * \param _solv_opt Options to be set.
+ *
+ */
+ void set_ceres_solver_options(const ceres::Solver::Options& _solv_opt);
+
+ /**
+ * \brief Function to get solver options.
+ *
+ * \param _solv_opt Solver options returned.
+ *
+ */
+ void get_ceres_solver_options(ceres::Solver::Options& _solv_opt);
+
+ /**
+ * \brief Function to set covariance options.
+ *
+ * \param _cov_opt Options to be set.
+ *
+ */
+ void set_covariance_options(const ceres::Covariance::Options& _cov_opt);
+
+ /**
+ * \brief Function to get covariance options.
+ *
+ * \param _cov_opt Covariance options returned.
+ *
+ */
+ void get_covariance_options(ceres::Covariance::Options& _cov_opt);
+
/**
* \brief define config type
*
diff --git a/include/estimated_pose_residual_2D.h b/include/estimated_pose_residual_2D.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e5720c0c3f4c166491e2678efb5e846ef0b6e73
--- /dev/null
+++ b/include/estimated_pose_residual_2D.h
@@ -0,0 +1,62 @@
+#include "ceres/ceres.h"
+#include <math.h>
+#include <Eigen/Eigen>
+
+
+/**
+ * \brief Class to define the residuals when adding an estimated pose.
+ *
+ */
+class EstimatedPoseResidual2D {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ /**
+ * \brief The constructor.
+ *
+ * \param _estimated_pose Vector with estimated x, y and yaw.
+ *
+ * \param _sigma_x Standard deviation of X coordenate.
+ *
+ * \param _sigma_y Standard deviation of Y coordenate.
+ *
+ * \param _sigma_yaw Standard deviation of orientation.
+ */
+ EstimatedPoseResidual2D(const Eigen::Vector3d& _estimated_pose, const double& _sigma_x, const double& _sigma_y, const double& _sigma_yaw)
+ {
+ x_ = _estimated_pose(0);
+ y_ = _estimated_pose(1);
+ yaw_ = _estimated_pose(2);
+ sigma_x_ = _sigma_x;
+ sigma_y_ = _sigma_y;
+ sigma_yaw_ = _sigma_yaw;
+ }
+
+ template <typename T>
+ /**
+ * \brief Function to calculate the error for Ceres optimization.
+ *
+ * \return True if all is ok.
+ */
+ bool operator()(const T* const _estimated_robot_pose,
+ T* residual) const {
+
+ T angle_res = T(yaw_) - _estimated_robot_pose[2];
+ while (angle_res >= M_PI)
+ angle_res -= 2*M_PI;
+ while (angle_res < -M_PI)
+ angle_res += 2*M_PI;
+
+ residual[0] = (T(x_) - _estimated_robot_pose[0])/T(sigma_x_);
+ residual[1] = (T(y_) - _estimated_robot_pose[1])/T(sigma_y_);
+ residual[2] = angle_res/T(sigma_yaw_);
+ return true;
+ }
+
+ private:
+ double x_; ///< The estimated x.
+ double y_; ///< The estimated y.
+ double yaw_; ///< The estimated yaw.
+ double sigma_x_; ///< Estimate sigma in X direction.
+ double sigma_y_; ///< Estimate sigma in Y direction.
+ double sigma_yaw_; ///< Estimate angular sigma.
+};
\ No newline at end of file
diff --git a/include/landmark_residual_2D.h b/include/landmark_residual_2D.h
new file mode 100644
index 0000000000000000000000000000000000000000..384ce5d3ab622d8553644c7cf3f1b14969fed440
--- /dev/null
+++ b/include/landmark_residual_2D.h
@@ -0,0 +1,84 @@
+#include "ceres/ceres.h"
+#include <math.h>
+#include <Eigen/Eigen>
+
+#define MAX_ROLL 0.2
+/**
+ * \brief Class to define the residuals between a robot pose and a landmark detected.
+ *
+ */
+class LandmarkResidual2D {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ /**
+ * \brief The constructor.
+ *
+ * \param _r Detection range.
+ *
+ * \param _theta Detection angle.
+ *
+ * \param _sigma_r Sensor radial standard deviation.
+ *
+ * \param _sigma_th Sensor angular standard deviation.
+ *
+ * \param _sensor_roll Sensor roll to know if it is inverted.
+ *
+ */
+ LandmarkResidual2D(const double& _r, const double& _theta, const double& _sigma_r, const double& _sigma_th, const double& _sensor_roll)
+ {
+ r_ = _r;
+ theta_ = _theta;
+ sensor_sigma_r_ = _sigma_r;
+ sensor_sigma_th_ = _sigma_th;
+ sensor_inverted_ = (std::fabs(_sensor_roll) > MAX_ROLL);
+ }
+
+ template <typename T>
+ /**
+ * \brief Function to calculate the error for Ceres optimization.
+ *
+ * \return True if all is ok.
+ */
+ bool operator()(const T* const _robot_pose,
+ const T* const _landmark_pos,
+ const T* const _tf_base_sensor,
+ T* residual) const {
+
+ Eigen::Matrix<T,2,1> dist;
+ Eigen::Matrix<T,3,1> laser_local, laser_global, landmark_global, landmark_local;
+ Eigen::Matrix<T,3,1> trans;
+ Eigen::Matrix<T,3,3> rot;
+
+ rot << cos(_robot_pose[2]), -sin(_robot_pose[2]), T(0.0),
+ sin(_robot_pose[2]), cos(_robot_pose[2]), T(0.0),
+ T(0.0), T(0.0), T(1.0);
+ trans << _robot_pose[0], _robot_pose[1], _robot_pose[2];
+ laser_local << _tf_base_sensor[0], _tf_base_sensor[1], _tf_base_sensor[2];
+ laser_global = rot*laser_local + trans;
+
+ landmark_global << _landmark_pos[0], _landmark_pos[1], T(0.0);
+ trans << laser_global[0], laser_global[1], laser_global[2];
+ rot << cos(laser_global[2]), sin(laser_global[2]), T(0.0),
+ -sin(laser_global[2]), cos(laser_global[2]), T(0.0),
+ T(0.0), T(0.0), T(1.0);
+ landmark_local = rot*(landmark_global - trans);
+
+ dist << landmark_local(0), landmark_local(1)*(sensor_inverted_? T(-1): T(1));
+
+ T angle_res = T(theta_) - T(atan2(dist(1), dist(0)));
+ while (angle_res >= M_PI)
+ angle_res -= 2*M_PI;
+ while (angle_res < -M_PI)
+ angle_res += 2*M_PI;
+
+ residual[0] = (T(r_) - dist.norm())/T(sensor_sigma_r_);
+ residual[1] = angle_res/T(sensor_sigma_th_);
+ return true;
+ }
+ private:
+ double r_; ///< The measured distance to the reflector center.
+ double theta_; ///< The measured angle to the reflector.
+ double sensor_sigma_r_; ///< Sensor radial sigma.
+ double sensor_sigma_th_; ///< Sensor angular sigma.
+ bool sensor_inverted_; ///<If the sensor is inverted
+};
\ No newline at end of file
diff --git a/include/odom_residual_2D.h b/include/odom_residual_2D.h
new file mode 100644
index 0000000000000000000000000000000000000000..09494b5c99eb7a90c81cbd288f310fc51018d9f8
--- /dev/null
+++ b/include/odom_residual_2D.h
@@ -0,0 +1,104 @@
+#include "ceres/ceres.h"
+#include <math.h>
+#include <Eigen/Eigen>
+
+/**
+ * \brief Class to define the residuals between a two consecutives robot poses.
+ *
+ */
+class OdomResidual2D {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ /**
+ * \brief The constructor.
+ *
+ * \param _last_odom Vector with last robot x, y and yaw values from odometry.
+ *
+ * \param _new_odom Vector with new robot x, y and yaw values from odometry.
+ *
+ * \param _fxy Linear factor to set linear standard deviation.
+ *
+ * \param _fth Angular factor to set angular standard deviation.
+ *
+ * \param _fxyth linear factor to increase angular standard deviation.
+ *
+ * \param _sigma_min Minimum standard deviation.
+ *
+ */
+ OdomResidual2D(const Eigen::Vector3d& last_odom, const Eigen::Vector3d& new_odom, const double& _fxy, const double& _fth, const double& _fxyth, const double& _sigma_min)
+ {
+ Eigen::Matrix<double,3,1> inc;
+ Eigen::Matrix<double,3,3> rot;
+
+ inc(0) = new_odom(0) - last_odom(0);
+ inc(1) = new_odom(1) - last_odom(1);
+ inc(2) = new_odom(2) - last_odom(2);
+
+ while (inc(2) >= M_PI)
+ inc(2) -= 2*M_PI;
+ while (inc(2) < -M_PI)
+ inc(2) += 2*M_PI;
+
+ rot << cos(-last_odom[2]), -sin(-last_odom[2]), 0.0,
+ sin(-last_odom[2]), cos(-last_odom[2]), 0.0,
+ 0.0, 0.0, 1.0;
+
+ inc = rot*inc;
+
+ x_inc_ = inc(0);
+ y_inc_ = inc(1);
+ yaw_inc_ = inc(2);
+
+ sigma_x_ = std::fabs(x_inc_)*_fxy + _sigma_min;
+ sigma_y_ = std::fabs(y_inc_)*_fxy + _sigma_min;
+ sigma_th_ = std::fabs(yaw_inc_)*_fth + _sigma_min + std::sqrt(x_inc_*x_inc_ + y_inc_*y_inc_)*_fxyth;
+ }
+
+ template <typename T>
+ /**
+ * \brief Function to calculate the error for Ceres optimization.
+ *
+ * \return True if all is ok.
+ */
+ bool operator()(const T* const _old_robot_pose,
+ const T* const _new_robot_pose,
+ T* residual) const {
+
+ Eigen::Matrix<T,3,1> inc;
+ Eigen::Matrix<T,3,3> rot;
+
+ inc(0) = (_new_robot_pose[0] - _old_robot_pose[0]);
+ inc(1) = (_new_robot_pose[1] - _old_robot_pose[1]);
+ inc(2) = (_new_robot_pose[2] - _old_robot_pose[2]);
+
+ while (inc(2) >= M_PI)
+ inc(2) -= 2*M_PI;
+ while (inc(2) < -M_PI)
+ inc(2) += 2*M_PI;
+
+ rot << cos(-_old_robot_pose[2]), -sin(-_old_robot_pose[2]), T(0.0),
+ sin(-_old_robot_pose[2]), cos(-_old_robot_pose[2]), T(0.0),
+ T(0.0), T(0.0), T(1.0);
+
+ inc = rot*inc;
+
+ T angle_res = T(yaw_inc_) - inc(2);
+ while (angle_res >= M_PI)
+ angle_res -= 2*M_PI;
+ while (angle_res < -M_PI)
+ angle_res += 2*M_PI;
+
+ residual[0] = (T(x_inc_) - inc(0))/T(sigma_x_);
+ residual[1] = (T(y_inc_) - inc(1))/T(sigma_y_);
+ residual[2] = angle_res/T(sigma_th_);
+ return true;
+ }
+
+ private:
+ double x_inc_; ///< The measured x increment.
+ double y_inc_; ///< The measured y increment.
+ double yaw_inc_; ///< The measured yaw increment.
+ double sigma_x_; ///< Odometry sigma in X direction.
+ double sigma_y_; ///< Odometry sigma in Y direction.
+ double sigma_th_; ///< Odometry angular sigma.
+};
\ No newline at end of file
diff --git a/include/sensor_pose_residual_2D.h b/include/sensor_pose_residual_2D.h
new file mode 100644
index 0000000000000000000000000000000000000000..d794da6e7140875777ae4c5fc3a444d845943817
--- /dev/null
+++ b/include/sensor_pose_residual_2D.h
@@ -0,0 +1,57 @@
+#include "ceres/ceres.h"
+#include <math.h>
+#include <Eigen/Eigen>
+
+/**
+ * \brief Class to define the residuals of the laser pose.
+ *
+ */
+class SensorPoseResidual2D {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ /**
+ * \brief The constructor.
+ *
+ * \param _measured_pose The measured sensor pose from robot base link
+ *
+ * \param _sigma_xy Linear standard deviation of the measured pose.
+ *
+ * \param _sigma_th Angular standard deviation of the measured pose.
+ */
+ SensorPoseResidual2D(const Eigen::Vector3d& _measured_pose, const double& _sigma_xy, const double& _sigma_th)
+ {
+ x_ = _measured_pose(0);
+ y_ = _measured_pose(1);
+ yaw_ = _measured_pose(2);
+ sigma_xy_ = _sigma_xy;
+ sigma_th_ = _sigma_th;
+ }
+
+ template <typename T>
+ /**
+ * \brief Function to calculate the error for Ceres optimization.
+ *
+ * \return True if all is ok.
+ */
+ bool operator()(const T* const _tf_base_sensor,
+ T* residual) const {
+
+ T angle_res = T(yaw_) - _tf_base_sensor[2];
+ while (angle_res >= M_PI)
+ angle_res -= 2*M_PI;
+ while (angle_res < -M_PI)
+ angle_res += 2*M_PI;
+
+ residual[0] = (T(x_) - _tf_base_sensor[0])/T(sigma_xy_);
+ residual[1] = (T(y_) - _tf_base_sensor[1])/T(sigma_xy_);
+ residual[2] = angle_res/T(sigma_th_);
+ return true;
+ }
+
+ private:
+ double x_; ///< The measured x.
+ double y_; ///< The measured y.
+ double yaw_; ///< The measured yaw.
+ double sigma_xy_; ///< Measure sigma in XY directions.
+ double sigma_th_; ///< Measure angular sigma.
+};
\ No newline at end of file
diff --git a/src/adc_landmarks_slam_solver_alg.cpp b/src/adc_landmarks_slam_solver_alg.cpp
index d2e3c72a902a2afde45e4df1cd76dac48f7fabae..5689f306df68ec1a8b01fcc24e82a23ff07420b9 100644
--- a/src/adc_landmarks_slam_solver_alg.cpp
+++ b/src/adc_landmarks_slam_solver_alg.cpp
@@ -21,3 +21,30 @@ void AdcLandmarksSlamSolverAlgorithm::config_update(Config& config, uint32_t lev
}
// AdcLandmarksSlamSolverAlgorithm Public API
+void AdcLandmarksSlamSolverAlgorithm::solve_problem(bool _print_summary)
+{
+ ceres::Solver::Summary summary;
+ ceres::Solve(this->solv_opt_, &(this->problem_), &summary);
+ if (_print_summary)
+ std::cout << summary.FullReport() << std::endl;
+}
+
+void AdcLandmarksSlamSolverAlgorithm::set_ceres_solver_options(const ceres::Solver::Options& _solv_opt)
+{
+ this->solv_opt_ = _solv_opt;
+}
+
+void AdcLandmarksSlamSolverAlgorithm::get_ceres_solver_options(ceres::Solver::Options& _solv_opt)
+{
+ _solv_opt = this->solv_opt_;
+}
+
+void AdcLandmarksSlamSolverAlgorithm::set_covariance_options(const ceres::Covariance::Options& _cov_opt)
+{
+ this->cov_opt_ =_cov_opt;
+}
+
+void AdcLandmarksSlamSolverAlgorithm::get_covariance_options(ceres::Covariance::Options& _cov_opt)
+{
+ _cov_opt = this->cov_opt_;
+}