Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Gepetto
aicp_mapping
Commits
7cd5be64
Commit
7cd5be64
authored
Apr 24, 2019
by
Maurice Fallon
Browse files
reject drift of pitch and roll
parent
92589d0d
Changes
2
Hide whitespace changes
Inline
Side-by-side
aicp_core/include/aicp_registration/aligned_cloud.hpp
View file @
7cd5be64
...
...
@@ -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)
...
...
aicp_core/src/registration/aligned_cloud.cpp
View file @
7cd5be64
#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
;
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment