i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update behavior of Odom/AlignWithGround when odmetry is already aligned with gravity (just update z value).

Browse Source
This commit is contained in:
matlabbe
2024-06-15 17:24:00 -07:00
parent 28fd696c18
commit 0d221a806f
1 changed files with 22 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
corelib/src/Odometry.cpp
View File

@@ -491,7 +491,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
} }
// Ground alignment // Ground alignment
if(_pose.isIdentity() && _alignWithGround) if(_pose.x() == 0 && _pose.y() == 0 && _pose.z() == 0 && this->framesProcessed() == 0 && _alignWithGround)
{ {
if(data.depthOrRightRaw().empty()) if(data.depthOrRightRaw().empty())
{ {
@@ -507,6 +507,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
if(indices->size()) if(indices->size())
{ {
cloud = util3d::voxelize(cloud, indices, 0.01); cloud = util3d::voxelize(cloud, indices, 0.01);
if(!_pose.isIdentity())
{
// In case we are already aligned with gravity
cloud = util3d::transformPointCloud(cloud, _pose);
}
util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true); util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
if(ground->size()) if(ground->size())
{ {
@@ -535,11 +540,22 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
//get rotation from z to n; //get rotation from z to n;
Eigen::Matrix3f R; Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z); R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation( if(_pose.r11() == 1.0f && _pose.r22() == 1.0f && _pose.r33() == 1.0f)
R(0,0), R(0,1), R(0,2), 0, {
R(1,0), R(1,1), R(1,2), 0, Transform rotation(
R(2,0), R(2,1), R(2,2), coefficients.values.at(3)); R(0,0), R(0,1), R(0,2), 0,
this->reset(rotation); R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
this->reset(rotation);
}
else
{
// Rotation is already set (e.g., from IMU/gravity), just update Z
UWARN("Rotation was already initialized, just offseting z to %f", coefficients.values.at(3));
Transform pose = _pose;
pose.z() = coefficients.values.at(3);
this->reset(pose);
}
success = true; success = true;
} }
} }