Unverified Commit 44dd6119 authored by Maurice Fallon's avatar Maurice Fallon Committed by GitHub
Browse files

Merge pull request #24 from ori-drs/sn-fix-go-back-orientation

Fix orientation of poses in go back path
parents 365f3547 d3cecd94
......@@ -18,6 +18,8 @@ public:
Eigen::Isometry3d prior_pose);
~AlignedCloud();
void removePitchRollCorrection();
void updateCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
bool is_reference);
void updateCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
......@@ -31,6 +33,7 @@ public:
pcl::PointCloud<pcl::PointXYZ>::Ptr getCloud(){ return cloud_; }
int getNbPoints(){ return cloud_->size(); }
Eigen::Isometry3d getOdomPose(){ return world_to_cloud_odom_; }
Eigen::Isometry3d getPriorPose(){ return world_to_cloud_prior_; }
Eigen::Isometry3d getCorrection(){ return cloud_to_reference_; }
Eigen::Isometry3d getCorrectedPose(){ return world_to_cloud_corrected_; }
......@@ -53,6 +56,8 @@ private:
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_; // Cloud (pre-filtered and global coordinates)
Eigen::Isometry3d world_to_cloud_odom_; // odom to base: world -> cloud (global coordinates). this is the unmodified input.
Eigen::Isometry3d world_to_cloud_prior_; // cloud pose prior: world -> cloud (global coordinates)
Eigen::Isometry3d cloud_to_reference_; // relative transform: cloud -> reference (global coordinates)
Eigen::Isometry3d world_to_cloud_corrected_; // cloud pose corrected: world -> cloud (global coordinates)
......
......@@ -22,6 +22,10 @@
//convert formats in Eigen
Eigen::Isometry3d fromMatrix4fToIsometry3d(Eigen::Matrix4f matrix);
//compute angle between two vecotrs (returns degrees)
double angleBetweenVectors2d(const Eigen::Vector2d& v1,
const Eigen::Vector2d& v2);
//swapping two values.
template<typename T>
inline bool swap_if_gt(T& a, T& b) {
......@@ -45,3 +49,4 @@ Eigen::VectorXf get_random_gaussian_variable(float mean, float std_deviation, in
void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& yaw);
Eigen::Quaterniond euler_to_quat(double roll, double pitch, double yaw);
......@@ -39,6 +39,20 @@ public:
std::string name,
int64_t utime = -1) = 0;
virtual void publishOdomPoses(Eigen::Isometry3d pose_,
int param,
std::string name,
int64_t utime = -1) = 0;
virtual void publishPriorPoses(Eigen::Isometry3d pose_,
int param,
std::string name,
int64_t utime = -1) = 0;
virtual void publishOdomToMapPose(Eigen::Isometry3d pose_,
int64_t utime = -1) = 0;
// Gets
virtual const PathPoses& getPath() = 0;
......
#include "aicp_registration/aligned_cloud.hpp"
#include "aicp_utils/common.hpp"
namespace aicp {
......@@ -9,6 +10,8 @@ AlignedCloud::AlignedCloud(int64_t utime,
utime_ = utime;
cloud_ = cloud;
world_to_cloud_odom_ = prior_pose; // prior pose. This is the original odom-to-base and is not updated elsewhere
world_to_cloud_prior_ = prior_pose; // prior pose
cloud_to_reference_ = Eigen::Isometry3d::Identity(); // correction
world_to_cloud_corrected_ = world_to_cloud_prior_; // corrected pose (set equal to prior pose when correction not available yet)
......@@ -21,6 +24,35 @@ AlignedCloud::~AlignedCloud()
{
}
// Take the roll and pitch from the odom and use it to replace the roll and pitch estimated by ICP
// this is to ensure gravity consistency
// TODO: the cloud_to_reference_ is currently inconsistent with these two frames.
void AlignedCloud::removePitchRollCorrection()
{
Eigen::Quaterniond q_odom = Eigen::Quaterniond(world_to_cloud_odom_.rotation());
double r_odom, p_odom, y_odom;
quat_to_euler(q_odom, r_odom, p_odom, y_odom);
Eigen::Quaterniond q_corr = Eigen::Quaterniond(world_to_cloud_corrected_.rotation());
double r_corr, p_corr, y_corr;
quat_to_euler(q_corr, r_corr, p_corr, y_corr);
//std::cout << r_odom*180/M_PI << " "
// << p_odom*180/M_PI << " "
// << y_odom*180/M_PI << " odom\n";
//std::cout << r_corr*180/M_PI << " "
// << p_corr*180/M_PI << " "
// << y_corr*180/M_PI << " corr\n";
Eigen::Isometry3d world_to_cloud_corrected_fixed = Eigen::Isometry3d::Identity();
world_to_cloud_corrected_fixed.translation() = world_to_cloud_corrected_.translation();
world_to_cloud_corrected_fixed.rotate( euler_to_quat(r_odom, p_odom, y_corr) );
world_to_cloud_corrected_ = world_to_cloud_corrected_fixed;
}
void AlignedCloud::updateCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
bool is_reference)
{
......@@ -34,6 +66,9 @@ void AlignedCloud::updateCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
cloud_ = cloud;
cloud_to_reference_ = correction;
world_to_cloud_corrected_ = cloud_to_reference_ * world_to_cloud_prior_;
removePitchRollCorrection();
is_reference_ = is_reference;
its_reference_id_ = its_reference_id;
}
......
......@@ -401,10 +401,31 @@ void App::operator()() {
Eigen::Isometry3d relative_motion = vis_->getPath().back().inverse() *
aligned_clouds_graph_->getLastCloud()->getCorrectedPose();
double dist = relative_motion.translation().norm();
if (dist > 1.0)
if (dist > 1.0)//(1==1)// use threshold to reduce number of nearby markers. this shouldnt be done here
{
vis_->publishPoses(aligned_clouds_graph_->getLastCloud()->getCorrectedPose(), 0, "",
cloud->getUtime());
vis_->publishPriorPoses(aligned_clouds_graph_->getLastCloud()->getPriorPose(), 0, "",
cloud->getUtime());
vis_->publishOdomPoses(aligned_clouds_graph_->getLastCloud()->getOdomPose(), 0, "",
cloud->getUtime());
std::cout << "odom_to_map publish <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n";
Eigen::Isometry3d odom_to_base = aligned_clouds_graph_->getLastCloud()->getOdomPose();
Eigen::Isometry3d map_to_base = aligned_clouds_graph_->getLastCloud()->getCorrectedPose();
Eigen::Isometry3d odom_to_map = (map_to_base * odom_to_base.inverse()).inverse();
vis_->publishOdomToMapPose(odom_to_map, cloud->getUtime());
Eigen::Quaterniond q_corr = Eigen::Quaterniond(odom_to_map.rotation());
double r_corr, p_corr, y_corr;
quat_to_euler(q_corr, r_corr, p_corr, y_corr);
std::cout << odom_to_map.translation().transpose() << " odom_to_map publish\n";
std::cout << r_corr*180/M_PI << " "
<< p_corr*180/M_PI << " "
<< y_corr*180/M_PI << " rpy\n";
}
// Store aligned map and VISUALIZE
......
......@@ -22,6 +22,11 @@ Eigen::Isometry3d fromMatrix4fToIsometry3d(Eigen::Matrix4f matrix){
return isometry;
}
double angleBetweenVectors2d(const Eigen::Vector2d &v1,
const Eigen::Vector2d &v2){
return atan2(v1(0)*v2(1) - v1(1)*v2(0), v1(0)*v2(0) + v1(1)*v2(1)) * 180.0 / M_PI;
}
//extract integers from a string.
std::string extract_ints(std::ctype_base::mask category, std::string str, std::ctype<char> const& facet)
{
......@@ -71,3 +76,30 @@ void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& ya
pitch = asin(2*(q0*q2-q3*q1));
yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
}
Eigen::Quaterniond euler_to_quat(double roll, double pitch, double yaw) {
// This conversion function introduces a NaN in Eigen Rotations when:
// roll == pi , pitch,yaw =0 ... or other combinations.
// cos(pi) ~=0 but not exactly 0
if ( ((roll==M_PI) && (pitch ==0)) && (yaw ==0)){
return Eigen::Quaterniond(0,1,0,0);
}else if( ((pitch==M_PI) && (roll ==0)) && (yaw ==0)){
return Eigen::Quaterniond(0,0,1,0);
}else if( ((yaw==M_PI) && (roll ==0)) && (pitch ==0)){
return Eigen::Quaterniond(0,0,0,1);
}
double sy = sin(yaw*0.5);
double cy = cos(yaw*0.5);
double sp = sin(pitch*0.5);
double cp = cos(pitch*0.5);
double sr = sin(roll*0.5);
double cr = cos(roll*0.5);
double w = cr*cp*cy + sr*sp*sy;
double x = sr*cp*cy - cr*sp*sy;
double y = cr*sp*cy + sr*cp*sy;
double z = cr*cp*sy - sr*sp*cy;
return Eigen::Quaterniond(w,x,y,z);
}
This diff is collapsed.
......@@ -5,6 +5,7 @@
#include "aicp_utils/visualizer.hpp"
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Path.h>
#include <eigen_conversions/eigen_msg.h>
......@@ -50,8 +51,24 @@ public:
void publishPoses(PathPoses poses,
int param, std::string name, int64_t utime);
void publishOdomPoses(Eigen::Isometry3d pose,
int param, std::string name, int64_t utime);
void publishOdomPoses(PathPoses poses,
int param, std::string name, int64_t utime);
void publishPriorPoses(Eigen::Isometry3d pose,
int param, std::string name, int64_t utime);
void publishPriorPoses(PathPoses poses,
int param, std::string name, int64_t utime);
void publishOdomToMapPose(Eigen::Isometry3d pose, int64_t utime);
// Publish tf from fixed_frame to odom
void publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_base_eigen, ros::Time msg_time);
void publishFixedFrameToOdomTF(const Eigen::Isometry3d& fixed_frame_to_base_eigen,
ros::Time msg_time);
void publishFixedFrameToOdomPose(const Eigen::Isometry3d& fixed_frame_to_base_eigen,
ros::Time msg_time);
// Gets
const PathPoses& getPath(){
......@@ -64,17 +81,32 @@ private:
ros::Publisher prior_map_pub_;
ros::Publisher aligned_map_pub_;
ros::Publisher pose_pub_;
ros::Publisher odom_pose_pub_;
ros::Publisher prior_pose_pub_;
ros::Publisher fixed_to_odom_pub_;
ros::Publisher odom_to_map_pub_;
// Duplicates the list in collections renderer. assumed to be 3xN colors
std::vector<double> colors_;
// Path (vector of poses)
PathPoses path_;
PathPoses odom_path_;
PathPoses prior_path_;
std::string fixed_frame_; // map or map_test
std::string odom_frame_;
std::string base_frame_;
std::string fixed_to_odom_prefix_ = "/localization_manager/";
// TF listener and broadcaster
tf::TransformListener tf_listener_;
tf::TransformBroadcaster tf_broadcaster_;
geometry_msgs::PoseWithCovarianceStamped fixed_to_odom_msg_;
tf::Pose temp_tf_pose_;
void computeFixedFrameToOdom(const Eigen::Isometry3d &fixed_frame_to_base_eigen,
Eigen::Isometry3d& fixed_frame_to_odom_eigen);
};
}
......@@ -31,6 +31,10 @@
<param name="working_mode" value="debug" />
<!-- Fixed frame (default: map, debug: map_test -->
<param name="fixed_frame" value="$(arg fixed_frame)" />
<!-- default is /aicp/pose_corrected -->
<param name="output_channel" value="/aicp/pose_corrected" />
<param name="lidar_channel" value="/point_cloud_filter/velodyne/point_cloud_filtered" />
<!-- Prior map info -->
<param name="load_map_from_file" value="$(arg load_map_from_file)" />
<param name="map_from_file_path" value="$(arg map_from_file_path)" />
......
<?xml version="1.0" encoding="utf-8"?>
<launch>
<arg name="rviz" default="true"/>
<arg name="rviz_minimal" default="true"/>
<arg name="quadruped_name" default="anymal"/>
<arg name="quadruped_setup" default="minimal_mb_inspection_v_1_2"/>
<arg name="description_name" default="quadruped_description"/>
<arg name="world" default="empty"/>
<param name="use_sim_time" value="true" />
<!-- Load robot description -->
<include file="$(find anymal_boxy_description)/launch/load.launch">
<arg name="description_name" value="$(arg description_name)"/>
<arg name="simulation" value="false"/>
<arg name="velodyne" value="true"/>
<arg name="realsense" value="true"/>
</include>
<arg name="rviz_config_path" default="$(find aicp_ros)/config/rviz/config_aicp.rviz"/>
<!-- rviz -->
<group if="$(arg rviz)">
<include file="$(find anymal_rviz_visualization)/launch/rviz.launch">
<arg name="rviz_world_path" value="$(arg world)"/>
<arg name="rviz_config_path" value="$(arg rviz_config_path)"/>
<arg name="tf_minimal" value="false"/>
</include>
</group>
</launch>
#!/usr/bin/env python
# pass aicp's correction to tf
# this can be used instead of localization manager.
import rospy
from geometry_msgs.msg import TransformStamped
from geometry_msgs.msg import *
from tf2_msgs.msg import TFMessage
from visualization_msgs.msg import Marker
import tf
import math
import numpy
import time
pub = rospy.Publisher("/tf", TFMessage, queue_size=10)
vis_pub = rospy.Publisher("/visualization_marker", Marker, queue_size=10)
rospy.loginfo("aicp_tf_bridge")
def handle_odom_to_map(msg):
print "x"
#
tfm = TFMessage()
this_t = TransformStamped()
this_t.header = msg.header
this_t.header.frame_id = "odom"
this_t.child_frame_id = "map"
this_t.transform.translation = msg.pose.pose.position
this_t.transform.rotation = msg.pose.pose.orientation
tfm.transforms.append(this_t)
pub.publish(tfm)
rospy.init_node('aicp_tf_bridge')
rospy.Subscriber('/icp_tools/map_pose',
PoseWithCovarianceStamped,
handle_odom_to_map)
rospy.spin()
#!/usr/bin/env python
# when playing back a bag, filter out odom-to-map from tf
# Note: need to replay the bag while remapping /tf to /tf_old
# rosbag play filename.bag --pause --clock /tf:=/tf_old
import rospy
from tf2_msgs.msg import TFMessage
import tf
import math
import numpy
import time
tf_new_pub = rospy.Publisher("/tf", TFMessage, queue_size=10)
def handle_odom_to_map(msg):
for t in msg.transforms:
if (t.child_frame_id == "map"):
print "y"
return
tf_new_pub.publish(msg)
rospy.init_node('aicp_tf_filter')
rospy.Subscriber('/tf_old',
TFMessage,
handle_odom_to_map)
rospy.spin()
......@@ -90,7 +90,8 @@ void AppROS::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedCon
// Publish fixed_frame to odom tf
ros::Time msg_time(pose_msg_in->header.stamp.sec, pose_msg_in->header.stamp.nsec);
vis_ros_->publishFixedFrameToOdomTF(corrected_pose_, msg_time);
// vis_ros_->publishFixedFrameToOdomTF(corrected_pose_, msg_time);
// vis_ros_->publishFixedFrameToOdomPose(corrected_pose_, msg_time);
// Publish /aicp/pose_corrected
tf::poseEigenToTF(corrected_pose_, temp_tf_pose_);
......@@ -179,7 +180,7 @@ void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_ms
// vis_->publishCloud(accumulated_cloud, 10, "/aicp/accumulated_cloud", accu_->getFinishedTime());
// Push this cloud onto the work queue (mutex safe)
const int max_queue_size = 100;
const int max_queue_size = 3;
{
std::unique_lock<std::mutex> lock(data_mutex_);
......@@ -193,6 +194,8 @@ void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_ms
cloud_queue_.push_back(current_cloud);
if (cloud_queue_.size() > max_queue_size) {
cout << "|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||\n";
cout << "|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||\n";
cout << "[App ROS] WARNING: dropping " <<
(cloud_queue_.size()-max_queue_size) << " clouds." << endl;
}
......
#include "aicp_ros/talker_ros.hpp"
//#include <eigen_conversions/eigen_msg.h>
#include "aicp_utils/common.hpp"
namespace aicp {
......@@ -46,11 +45,27 @@ void ROSTalker::reversePath(PathPoses& path){
std::reverse(path.begin(),path.end());
for (size_t i = 0; i < path.size(); ++i){
// Turn 90 degrees about z-axis
double angle = 180.0;
// Compute orientation along trajectory line
if (i != path.size()-1)
{
// angle between x-axis of current frame
Eigen::Vector2d v1;
v1 = path[i].matrix().block<2,1>(0,0);
// and trajectory line
Eigen::Vector2d v2;
v2 << path[i+1].translation()(0, 0) - path[i].translation()(0, 0),
path[i+1].translation()(1, 0) - path[i].translation()(1, 0);
angle = angleBetweenVectors2d(v1, v2);
}
// Turn about z-axis
Eigen::Matrix3d rot;
rot = Eigen::AngleAxisd(0*M_PI/180, Eigen::Vector3d::UnitX())
* Eigen::AngleAxisd(0*M_PI/180, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(180*M_PI/180, Eigen::Vector3d::UnitZ());
* Eigen::AngleAxisd(angle*M_PI/180, Eigen::Vector3d::UnitZ());
path[i].linear() = rot * path[i].rotation();
}
......
......@@ -18,6 +18,13 @@ ROSVisualizer::ROSVisualizer(ros::NodeHandle& nh, string fixed_frame) : nh_(nh),
prior_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/aicp/prior_map", 10);
aligned_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/aicp/aligned_map", 10);
pose_pub_ = nh_.advertise<nav_msgs::Path>("/aicp/poses",100);
odom_pose_pub_ = nh_.advertise<nav_msgs::Path>("/aicp/odom_poses",100);
prior_pose_pub_ = nh_.advertise<nav_msgs::Path>("/aicp/prior_poses",100);
fixed_to_odom_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(fixed_to_odom_prefix_ + fixed_frame_ + "_to_odom", 10);
odom_to_map_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/icp_tools/map_pose", 10);
colors_ = {
51/255.0, 160/255.0, 44/255.0, //0
166/255.0, 206/255.0, 227/255.0,
......@@ -152,7 +159,87 @@ void ROSVisualizer::publishPoses(PathPoses poses, int param, string name, int64_
pose_pub_.publish(path_msg);
}
void ROSVisualizer::publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_base_eigen, ros::Time msg_time)
void ROSVisualizer::publishOdomPoses(Eigen::Isometry3d pose, int param, std::string name, int64_t utime)
{
odom_path_.push_back(pose);
publishOdomPoses(odom_path_, param, name, utime);
}
void ROSVisualizer::publishOdomPoses(PathPoses poses, int param, string name, int64_t utime){
nav_msgs::Path path_msg;
int secs = utime*1E-6;
int nsecs = (utime - (secs*1E6))*1E3;
path_msg.header.stamp = ros::Time(secs, nsecs);
path_msg.header.frame_id = fixed_frame_;
for (size_t i = 0; i < poses.size(); ++i){
geometry_msgs::PoseStamped m;
m.header.stamp = ros::Time(secs, nsecs);
m.header.frame_id = fixed_frame_;
tf::poseEigenToMsg(poses[i], m.pose);
path_msg.poses.push_back(m);
}
odom_pose_pub_.publish(path_msg);
}
void ROSVisualizer::publishPriorPoses(Eigen::Isometry3d pose, int param, std::string name, int64_t utime)
{
prior_path_.push_back(pose);
publishPriorPoses(odom_path_, param, name, utime);
}
void ROSVisualizer::publishPriorPoses(PathPoses poses, int param, string name, int64_t utime){
nav_msgs::Path path_msg;
int secs = utime*1E-6;
int nsecs = (utime - (secs*1E6))*1E3;
path_msg.header.stamp = ros::Time(secs, nsecs);
path_msg.header.frame_id = fixed_frame_;
for (size_t i = 0; i < poses.size(); ++i){
geometry_msgs::PoseStamped m;
m.header.stamp = ros::Time(secs, nsecs);
m.header.frame_id = fixed_frame_;
tf::poseEigenToMsg(poses[i], m.pose);
path_msg.poses.push_back(m);
}
prior_pose_pub_.publish(path_msg);
}
void ROSVisualizer::publishOdomToMapPose(Eigen::Isometry3d pose, int64_t utime)
{
int secs = utime*1E-6;
int nsecs = (utime - (secs*1E6))*1E3;
geometry_msgs::PoseWithCovarianceStamped msg;
msg.header.stamp = ros::Time(secs, nsecs);
msg.header.frame_id = "odom";
tf::poseEigenToMsg(pose, msg.pose.pose);
odom_to_map_pub_.publish(msg);
}
void ROSVisualizer::publishFixedFrameToOdomPose(const Eigen::Isometry3d &fixed_frame_to_base_eigen,
ros::Time msg_time)
{
Eigen::Isometry3d fixed_frame_to_odom_eigen;
this->computeFixedFrameToOdom(fixed_frame_to_base_eigen, fixed_frame_to_odom_eigen);
tf::poseEigenToTF(fixed_frame_to_odom_eigen, temp_tf_pose_);
tf::poseTFToMsg(temp_tf_pose_, fixed_to_odom_msg_.pose.pose);
fixed_to_odom_msg_.header.stamp = msg_time;
fixed_to_odom_msg_.header.frame_id = odom_frame_;
fixed_to_odom_pub_.publish(fixed_to_odom_msg_);
}
void ROSVisualizer::computeFixedFrameToOdom(const Eigen::Isometry3d &fixed_frame_to_base_eigen,
Eigen::Isometry3d& fixed_frame_to_odom_eigen)
{
// TF listener
tf::StampedTransform base_to_odom_tf;
......@@ -171,8 +258,14 @@ void ROSVisualizer::publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_
tf::transformTFToEigen(base_to_odom_tf, base_to_odom_eigen);
// Multiply
Eigen::Isometry3d fixed_frame_to_odom_eigen;
fixed_frame_to_odom_eigen = fixed_frame_to_base_eigen * base_to_odom_eigen.inverse();
}
void ROSVisualizer::publishFixedFrameToOdomTF(const Eigen::Isometry3d& fixed_frame_to_base_eigen,
ros::Time msg_time)
{
Eigen::Isometry3d fixed_frame_to_odom_eigen;
this->computeFixedFrameToOdom(fixed_frame_to_base_eigen, fixed_frame_to_odom_eigen);
// Convert to TF
tf::StampedTransform fixed_frame_to_odom_tf;
......
Markdown is supported
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