Commit 7cd5be64 authored by Maurice Fallon's avatar Maurice Fallon
Browse files

reject drift of pitch and roll

parent 92589d0d
......@@ -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)
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)
......
#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 (not updated else where)
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 icp estimate roll and pitch
// this is to ensure gravity consistency
// TODO: the cloud_to_reference_ is now 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;
}
......
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