i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-05 01:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Removed parameter "OdomF2M/FixedMapPath". GUI: added odometry disabled option. Rtabmap can now localize even if odometry input is null (only in localization mode).

Browse Source
This commit is contained in:
matlabbe
2016-11-27 14:30:11 -05:00
parent 693f623e5e
commit 121446d648
15 changed files with 695 additions and 763 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Odometry.cpp
+54 -47
View File
@@ -206,56 +206,63 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
// Ground alignment
if(_pose.isIdentity() && _alignWithGround)
{
UTimer alignTimer;
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr ground, obstacles;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get());
cloud = util3d::voxelize(cloud, indices, 0.01);
bool success = false;
if(cloud->size())
if(data.depthOrRightRaw().empty())
{
util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
if(ground->size())
{
pcl::ModelCoefficients coefficients;
util3d::extractPlane(cloud, ground, 0.02, 100, &coefficients);
if(coefficients.values.at(3) >= 0)
{
UWARN("Ground detected! coefficients=(%f, %f, %f, %f) time=%fs",
coefficients.values.at(0),
coefficients.values.at(1),
coefficients.values.at(2),
coefficients.values.at(3),
alignTimer.ticks());
}
else
{
UWARN("Ceiling detected! coefficients=(%f, %f, %f, %f) time=%fs",
coefficients.values.at(0),
coefficients.values.at(1),
coefficients.values.at(2),
coefficients.values.at(3),
alignTimer.ticks());
}
Eigen::Vector3f n(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2));
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
_pose *= rotation;
success = true;
}
UWARN("\"%s\" is true but the input has no depth information, ignoring alignment with ground...", Parameters::kOdomAlignWithGround().c_str());
}
if(!success)
else
{
UERROR("Odometry failed to detect the ground. You have this "
"error because parameter \"Odom/AlignWithGround\" is true. "
"Make sure the camera is seeing the ground (e.g., tilt ~30 "
"degrees toward the ground).");
UTimer alignTimer;
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr ground, obstacles;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get());
cloud = util3d::voxelize(cloud, indices, 0.01);
bool success = false;
if(cloud->size())
{
util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
if(ground->size())
{
pcl::ModelCoefficients coefficients;
util3d::extractPlane(cloud, ground, 0.02, 100, &coefficients);
if(coefficients.values.at(3) >= 0)
{
UWARN("Ground detected! coefficients=(%f, %f, %f, %f) time=%fs",
coefficients.values.at(0),
coefficients.values.at(1),
coefficients.values.at(2),
coefficients.values.at(3),
alignTimer.ticks());
}
else
{
UWARN("Ceiling detected! coefficients=(%f, %f, %f, %f) time=%fs",
coefficients.values.at(0),
coefficients.values.at(1),
coefficients.values.at(2),
coefficients.values.at(3),
alignTimer.ticks());
}
Eigen::Vector3f n(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2));
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
_pose *= rotation;
success = true;
}
}
if(!success)
{
UERROR("Odometry failed to detect the ground. You have this "
"error because parameter \"%s\" is true. "
"Make sure the camera is seeing the ground (e.g., tilt ~30 "
"degrees toward the ground).", Parameters::kOdomAlignWithGround().c_str());
}
}
}