diff --git a/include/landmark_residual_2D.h b/include/landmark_residual_2D.h
index 0b6fe634f45022cd59f6025e960d624900697329..92dd1ac9216fd67237cabde11829eb00196afb9c 100644
--- a/include/landmark_residual_2D.h
+++ b/include/landmark_residual_2D.h
@@ -13,21 +13,24 @@ class LandmarkResidual2D {
/**
* \brief The constructor.
*
- * \param _r Detection range.
+ * \param _r Detection range from base link.
*
- * \param _theta Detection angle.
+ * \param _theta Detection angle from base link.
*
* \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)
+ 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>
@@ -45,6 +48,7 @@ class LandmarkResidual2D {
Eigen::Matrix<T,3,1> trans;
Eigen::Matrix<T,3,3> rot;
+
landmark_global << _landmark_pos[0], _landmark_pos[1], T(0.0);
trans << _robot_pose[0], _robot_pose[1], _robot_pose[2];
rot << cos(_robot_pose[2]), sin(_robot_pose[2]), T(0.0),
@@ -52,7 +56,7 @@ class LandmarkResidual2D {
T(0.0), T(0.0), T(1.0);
landmark_local = rot*(landmark_global - trans);
- dist << landmark_local(0), landmark_local(1);
+ 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)
@@ -65,8 +69,9 @@ class LandmarkResidual2D {
return true;
}
private:
- double r_; ///< The measured distance to the landmark center.
- double theta_; ///< The measured angle to the landmark.
+ double r_; ///< The measured distance to the landmark center from base link.
+ double theta_; ///< The measured angle to the landmark from base link.
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/landmark_residual_from_base_2D.h b/include/landmark_residual_from_base_2D.h
index 8d8b289c5e5ac46b8c4f42fdd5c087a876864e10..2e209be5cd6a648e689233bf1131d11e059e86b0 100644
--- a/include/landmark_residual_from_base_2D.h
+++ b/include/landmark_residual_from_base_2D.h
@@ -13,24 +13,21 @@ class LandmarkResidualFromBase2D {
/**
* \brief The constructor.
*
- * \param _r Detection range from base link.
+ * \param _r Detection range.
*
- * \param _theta Detection angle from base link.
+ * \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.
- *
*/
- LandmarkResidualFromBase2D(const double& _r, const double& _theta, const double& _sigma_r, const double& _sigma_th, const double& _sensor_roll)
+ LandmarkResidualFromBase2D(const double& _r, const double& _theta, const double& _sigma_r, const double& _sigma_th)
{
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>
@@ -48,7 +45,6 @@ class LandmarkResidualFromBase2D {
Eigen::Matrix<T,3,1> trans;
Eigen::Matrix<T,3,3> rot;
-
landmark_global << _landmark_pos[0], _landmark_pos[1], T(0.0);
trans << _robot_pose[0], _robot_pose[1], _robot_pose[2];
rot << cos(_robot_pose[2]), sin(_robot_pose[2]), T(0.0),
@@ -56,7 +52,7 @@ class LandmarkResidualFromBase2D {
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));
+ dist << landmark_local(0), landmark_local(1);
T angle_res = T(theta_) - T(atan2(dist(1), dist(0)));
while (angle_res >= M_PI)
@@ -69,9 +65,8 @@ class LandmarkResidualFromBase2D {
return true;
}
private:
- double r_; ///< The measured distance to the landmark center from base link.
- double theta_; ///< The measured angle to the landmark from base link.
+ double r_; ///< The measured distance to the landmark center.
+ double theta_; ///< The measured angle to the landmark.
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/src/adc_landmarks_slam_solver_alg.cpp b/src/adc_landmarks_slam_solver_alg.cpp
index b53a0e55b466b13d6f96aeb369c70a13cf6452ff..2b76cd22500e5f9f6f45b15c68cafc34d0019d81 100644
--- a/src/adc_landmarks_slam_solver_alg.cpp
+++ b/src/adc_landmarks_slam_solver_alg.cpp
@@ -247,7 +247,7 @@ void AdcLandmarksSlamSolverAlgorithm::add_newest_frame_to_ceres(bool _estimated_
std::map<double, LandmarkInfo>::iterator land_it = this->landmarks_.find(land_res_it->landmark_key);
if (land_it != this->landmarks_.end())
{
- ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<LandmarkResidual2D, 2, 3, 2>(new LandmarkResidual2D(land_res_it->r, land_res_it->theta, this->config_.sensor_sigma_r, this->config_.sensor_sigma_th));
+ ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<LandmarkResidualFromBase2D, 2, 3, 2>(new LandmarkResidualFromBase2D(land_res_it->r, land_res_it->theta, this->config_.sensor_sigma_r, this->config_.sensor_sigma_th));
this->problem_.AddResidualBlock(cost_function, new ceres::CauchyLoss(1.0), this->frames_data_.back().robot_state.data(), land_it->second.pose.data());
if(this->config_.landmarks_pos_fixed)