Commit 622c6ab4 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Added markers roll and pitch offset to improve orientation filter

parent 30d6c4df
......@@ -55,7 +55,8 @@
#define GLOBAL_LOC_TYPE "1001"
#define GLOBAL_LOC_MARKER 3
#define GLOBAL_LOC_SIGN_ID_OFFSET 200
#define SIGN_MARKER_ANGLE_OFFSET M_PI/2
#define SIGN_MARKER_YAW_OFFSET M_PI/2
#define SIGN_MARKER_ROLL_OFFSET M_PI/2
//include adc_landmarks_slam_solver_alg main library
......@@ -115,6 +116,8 @@ class AdcLandmarksSlamSolverAlgorithm
std::list<FrameData> frames_data_; ///< Frames information.
std::map<double, LandmarkInfo> landmarks_; ///< Landmarks.
double landmarks_marker_roll_offset_; ///< Marker's roll ofset for the landmarks.
double landmarks_marker_pitch_offset_; ///< Marker's pitch ofset for the landmarks.
protected:
/**
* \brief define config type
......@@ -196,10 +199,24 @@ class AdcLandmarksSlamSolverAlgorithm
/**
* \brief Function to get landmarks size.
*
* \return The actual size.
* \return The current size.
*/
unsigned int get_landmarks_size(void);
/**
* \brief Function to get landmarks marker roll offset.
*
* \return The current offset.
*/
double get_landmarks_marker_roll_offset(void);
/**
* \brief Function to get landmarks marker pitch offset.
*
* \return The current offset.
*/
double get_landmarks_marker_pitch_offset(void);
/**
* \brief Function to load landmarks map from a txt file.
*
......
......@@ -80,6 +80,8 @@ bool AdcLandmarksSlamSolverAlgorithm::load_landmarks(const std::string& _landmar
l.fixed = _fixed;
this->landmarks_[id] = l;
}
this->landmarks_marker_roll_offset_ = SIGN_MARKER_ROLL_OFFSET;
this->landmarks_marker_pitch_offset_ = 0.0;
// std::cout << "landmarks size = " << (int) this->global_landmarks.size() << std::endl;
landmarks_file.close();
return true;
......@@ -119,7 +121,7 @@ bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmar
loc_yaw = M_PI + LOC_SIGN_TRIANGLE_ANGLE/2;
l.pose(0) = world.x + loc_x*std::cos(world.heading) - loc_y*std::sin(world.heading);
l.pose(1) = world.y + loc_x*std::sin(world.heading) + loc_y*std::cos(world.heading);
l.pose(2) = loc_yaw + world.heading + SIGN_MARKER_ANGLE_OFFSET;
l.pose(2) = loc_yaw + world.heading + SIGN_MARKER_YAW_OFFSET;
id = LOC_SIGN_ID_OFFSET + (sign.get_id()%LOC_SIGN_ID_OFFSET)*2-1;
this->landmarks_[id] = l;
......@@ -129,7 +131,7 @@ bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmar
loc_yaw = -LOC_SIGN_TRIANGLE_ANGLE/2;
l.pose(0) = world.x + loc_x*std::cos(world.heading) - loc_y*std::sin(world.heading);
l.pose(1) = world.y + loc_x*std::sin(world.heading) + loc_y*std::cos(world.heading);
l.pose(2) = loc_yaw + world.heading + SIGN_MARKER_ANGLE_OFFSET;
l.pose(2) = loc_yaw + world.heading + SIGN_MARKER_YAW_OFFSET;
id = LOC_SIGN_ID_OFFSET + (sign.get_id()%LOC_SIGN_ID_OFFSET)*2;
this->landmarks_[id] = l;
......@@ -145,12 +147,11 @@ bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmar
world = sign.get_world_pose();
l.pose(0) = world.x;
l.pose(1) = world.y;
l.pose(2) = world.heading + SIGN_MARKER_ANGLE_OFFSET;
l.pose(2) = world.heading + SIGN_MARKER_YAW_OFFSET;
this->landmarks_[(double)sign.get_id()] = l;
}
}
}
}catch (CException &e){
ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgorithm::load_from_xodr -> [Exception caught] : " << e.what());
return false;
......@@ -160,6 +161,7 @@ bool AdcLandmarksSlamSolverAlgorithm::load_from_xodr(const std::string& _landmar
void AdcLandmarksSlamSolverAlgorithm::transform_landmarks(geometry_msgs::TransformStamped& _tf_map_opendrive)
{
double yaw, pitch, roll;
for (std::map<double, LandmarkInfo>::iterator it = this->landmarks_.begin(); it != this->landmarks_.end(); ++it)
{
geometry_msgs::PoseStamped land, transformed_land;
......@@ -169,12 +171,11 @@ void AdcLandmarksSlamSolverAlgorithm::transform_landmarks(geometry_msgs::Transfo
land.pose.position.x = it->second.pose(0);
land.pose.position.y = it->second.pose(1);
tf2::Quaternion q;
q.setRPY(0.0, 0.0, it->second.pose(2));
q.setRPY(SIGN_MARKER_ROLL_OFFSET, 0.0, it->second.pose(2));
land.pose.orientation = tf2::toMsg(q);
tf2::doTransform(land, transformed_land, _tf_map_opendrive);
double yaw, pitch, roll;
q = tf2::Quaternion(transformed_land.pose.orientation.x, transformed_land.pose.orientation.y, transformed_land.pose.orientation.z, transformed_land.pose.orientation.w);
tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
......@@ -182,6 +183,8 @@ void AdcLandmarksSlamSolverAlgorithm::transform_landmarks(geometry_msgs::Transfo
it->second.pose(1) = transformed_land.pose.position.y;
it->second.pose(2) = yaw;
}
this->landmarks_marker_roll_offset_ = roll;
this->landmarks_marker_pitch_offset_ = pitch;
}
bool AdcLandmarksSlamSolverAlgorithm::write_landmarks_file(void)
......@@ -219,6 +222,16 @@ unsigned int AdcLandmarksSlamSolverAlgorithm::get_landmarks_size(void)
return this->landmarks_.size();
}
double AdcLandmarksSlamSolverAlgorithm::get_landmarks_marker_roll_offset(void)
{
return this->landmarks_marker_roll_offset_;
}
double AdcLandmarksSlamSolverAlgorithm::get_landmarks_marker_pitch_offset(void)
{
return this->landmarks_marker_pitch_offset_;
}
std::map<double, LandmarkInfo>::iterator AdcLandmarksSlamSolverAlgorithm::get_landmarks_begin(void)
{
return this->landmarks_.begin();
......@@ -278,7 +291,7 @@ double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector3d _fe
global_land.pose.position.x = it->second.pose(0);
global_land.pose.position.y = it->second.pose(1);
tf2::Quaternion q;
q.setRPY(0.0, 0.0, it->second.pose(2));//TODO: Must take into acoount not just yaw, duality pitch
q.setRPY(this->landmarks_marker_roll_offset_, this->landmarks_marker_pitch_offset_, it->second.pose(2));
global_land.pose.orientation = tf2::toMsg(q);
tf2::doTransform(global_land, local_land, _tf_sensor_map);
......
......@@ -746,6 +746,9 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature, boo
return true;
double min_dist = 999999999.0;
std::list<LandmarkCandidate>::iterator min_dist_land = this->landmarks_candidates_.end();
double marker_roll_offset, marker_pitch_offset;
marker_roll_offset = this->alg_.get_landmarks_marker_roll_offset();
marker_pitch_offset = this->alg_.get_landmarks_marker_pitch_offset();
for (auto it = this->landmarks_candidates_.begin(); it != this->landmarks_candidates_.end(); ++it)
{
if(_feature.type == it->type)
......@@ -760,7 +763,7 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature, boo
global_land.pose.position.x = it->pose(0);
global_land.pose.position.y = it->pose(1);
tf2::Quaternion q;
q.setRPY(0.0, 0.0, it->pose(2));
q.setRPY(marker_roll_offset, marker_pitch_offset, it->pose(2));
global_land.pose.orientation = tf2::toMsg(q);
tf2::doTransform(global_land, local_land, _tf_sensor_map);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment