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

reverted modif on z when honolonomic but added modif on yaw depending on the estimated y value

Browse Source
This commit is contained in:
matlabbe
2015-06-16 10:50:49 -04:00
parent 7cd0d0cd53
commit 6a7a9fb9b0
4 changed files with 42 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Parameters.h
+1 -1
View File
@@ -322,7 +322,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
RTABMAP_PARAM_STR(Odom, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
RTABMAP_PARAM(Odom, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw).");
RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing and up/down commands can be issued). If not, y/z values will be estimated from x and rotation values (y=x*tan(yaw), z=x*tan(pitch)).");
RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf).");
RTABMAP_PARAM(Odom, PnPEstimation, bool, false, "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences.");
corelib/src/Odometry.cpp
+20 -17
View File
@@ -213,28 +213,27 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
else
{
x = filters_[0]->filter(x);
y = filters_[1]->filter(y);
yaw = filters_[5]->filter(yaw);
if(_holonomic)
if(!_holonomic)
{
y = filters_[1]->filter(y);
}
else
{
y = x * tan(yaw);
float tmpY = x * tan(yaw);
if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
{
y = tmpY;
}
else
{
yaw = atan(y/x);
}
}
if(!_force2D)
{
z = filters_[2]->filter(z);
roll = filters_[3]->filter(roll);
pitch = filters_[4]->filter(pitch);
if(_holonomic)
{
z = filters_[2]->filter(z);
}
else
{
z = x * tan(pitch);
}
}
}
@@ -245,10 +244,14 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
}
else if(!_holonomic)
{
y = x * tan(yaw);
if(!_force2D)
float tmpY = x * tan(yaw);
if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
{
z = x * tan(pitch);
y = tmpY;
}
else
{
yaw = atan(y/x);
}
}
UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) &&
guilib/src/PreferencesDialog.cpp
+16 -7
View File
@@ -3188,13 +3188,9 @@ Transform PreferencesDialog::getSourceOpenniLocalTransform() const
QString str = _ui->lineEdit_openniLocalTransform->text();
str.replace("PI_2", QString::number(3.141592/2.0));
QStringList list = str.split(' ');
if(list.size() != 6)
if(list.size() == 6 || list.size() == 9)
{
UERROR("Local transform is wrong! must have 6 items (%s)", str.toStdString().c_str());
}
else
{
std::vector<float> numbers(6);
std::vector<float> numbers(list.size());
bool ok = false;
for(int i=0; i<list.size(); ++i)
{
@@ -3208,9 +3204,22 @@ Transform PreferencesDialog::getSourceOpenniLocalTransform() const
}
if(ok)
{
t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
if(list.size() == 6)
{
t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
}
else // 9
{
t = Transform(numbers[0], numbers[1], numbers[2], 0,
numbers[3], numbers[4], numbers[5], 0,
numbers[6], numbers[7], numbers[8], 0);
}
}
}
else
{
UERROR("Local transform is wrong! must have 6 or 9 items (%s)", str.toStdString().c_str());
}
return t;
}
guilib/src/ui/preferencesDialog.ui
+5 -5
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-837</y>
<width>755</width>
<height>1591</height>
</rect>
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>23</number>
<number>3</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -2071,14 +2071,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="1" column="0">
<widget class="QLineEdit" name="lineEdit_openniLocalTransform">
<property name="text">
<string>0 0 0 -PI_2 0 -PI_2</string>
<string>0 0 1 -1 0 0 0 -1 0</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_42">
<property name="text">
<string>Local transform from /base_link to /camera_link. Format (6 values): x y z roll pitch yaw.</string>
<string>Local transform from /base_link to /camera_link. Format (6 values): x y z roll pitch yaw. Format (9 values): r11 r12 r13 r21 r22 r23 r31 r32 r33.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -6976,7 +6976,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<item row="7" column="1">
<widget class="QLabel" name="label_238">
<property name="text">
<string>If the robot is holonomic (strafing and up/down commands can be issued). If not, y/z values will be estimated from x and rotation values (y=x*tan(yaw), z=x*tan(pitch)).</string>
<string>If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).</string>
</property>
<property name="wordWrap">
<bool>true</bool>