分类： math

GNU libc Rounding Functions

yuiwongComputer science

GNU libc Rounding Functions

See also http://www.gnu.org/software/libc/manual/html_node/Rounding-Functions.html

The functions listed here perform operations such as rounding and truncation
of floating-point values. Some of these functions convert floating point
numbers to integer values. They are all declared in math.h.

You can also convert floating-point numbers to integers simply by casting
them to int. This discards the fractional part, effectively rounding towards
zero.
However, this only works if the result can actually be represented as an
int-for very large numbers, this is impossible.
The functions listed here return the result as a double instead to get around
this problem.

The fromfp functions use the following macros, from TS 18661-1:2014, to
specify the direction of rounding.
These correspond to the rounding directions defined in IEEE 754-2008.

trunc

The trunc functions round x towards zero to the nearest integer
(returned in floating-point format). Thus,
trunc (1.5) is 1.0 and trunc (-1.5) is -1.0 (丢掉小数部分, x 大于等于 0 的时候结果
等于 floor(x), x 小于等于 0 的时候结果等于 ceil(x)).

ceil

These functions round x upwards to the nearest integer, returning that value
as a double. Thus, ceil (1.5) is 2.0 (ceil: 天花板 — 向上(大)取整).

floor

These functions round x downwards to the nearest integer, returning that value
as a double. Thus, floor (1.5) is 1.0 and floor (-1.5) is -2.0 (
floor: 地板 — 向下(小)取整).

round

These functions are similar to rint, but they round halfway cases(一半的情况)
away from zero instead of to the nearest integer
(or other current rounding mode).
I.e. round to nearest integer, away from zero (返回 四舍五入整数).

rint

These functions round x to an integer value according to the current rounding
mode, usually 1, example 2.23-0ubuntu11 libc, gcc 5.4.0: FLT_ROUNDS is 1:

/usr/lib/gcc/x86_64-linux-gnu/5/include/float.h:128:

/* Addition rounds to 0: zero, 1: nearest, 2: +inf, 3: -inf, -1: unknown.  */
/* ??? This is supposed to change with calls to fesetround in <fenv.h>.  */
#undef FLT_ROUNDS
#define FLT_ROUNDS 1

And:

BUGS
       C99 specifies that the value of FLT_ROUNDS should reflect changes
       to the current rounding mode, as set by fesetround().  Currently,
       this does not occur: FLT_ROUNDS always has the value 1.

See also http://www.gnu.org/software/libc/manual/html_node/Floating-Point-Parameters.html#Floating-Point-Parameters

Round to nearest integer (Like round, but maybe NOT good, better use round
when nearest integer, especially “四舍五入”, rint 返回 最接近参数的整数,
如果有两个数同样接近, 则会返回偶数的那个 !!).

Bad rint examples

EXPECT_DOUBLE_EQ(-2.0, ::rint(-1.5));
EXPECT_DOUBLE_EQ(-2.0, ::rint(-2.5));// Here we want -3.0!
EXPECT_DOUBLE_EQ(0.0, ::rint(0.5));// Here we want 1.0!
EXPECT_DOUBLE_EQ(0.0, ::rint(-0.5));// Here we want -1.0!

Good round examples

EXPECT_DOUBLE_EQ(-2.0, ::round(-1.5));
EXPECT_DOUBLE_EQ(-3.0, ::round(-2.5));// Here we want -3.0!
EXPECT_DOUBLE_EQ(1.0, ::round(0.5));// Here we want 1.0!
EXPECT_DOUBLE_EQ(-1.0, ::round(-0.5));// Here we want -1.0!

对比

value round rint floor ceil trunc
0.1 0.0 0.0 0.0 1.0 0.0
0.5 1.0 0.0 0.0 1.0 0.0
0.6 1.0 1.0 0.0 1.0 0.0
1.1 1.0 1.0 1.0 2.0 1.0
1.5 2.0 2.0 1.0 2.0 1.0
2.3 2.0 2.0 2.0 3.0 2.0
2.5 3.0 2.0 2.0 3.0 2.0
3.8 4.0 4.0 3.0 4.0 3.0
5.5 6.0 6.0 5.0 6.0 5.0
-0.5 -1.0 -0.0 -1.0 -0.0 -0.0
-1.1 -1.0 -1.0 -2.0 -1.0 -1.0
-1.5 -2.0 -2.0 -2.0 -1.0 -1.0
-2.3 -2.0 -2.0 -3.0 -2.0 -2.0
-2.5 -3.0 -2.0 -3.0 -2.0 -2.0
-3.8 -4.0 -4.0 -4.0 -3.0 -3.0
-5.5 -6.0 -6.0 -6.0 -5.0 -5.0

See also

坐标变换

yuiwongmath

坐标变换

  • 位姿变换(位姿在不同坐标系中变换, 更常用): PosesT or T,
    • 关键在于同一个位姿（实际位姿固定不变），但是在不同坐标系有不同的 表 示！
  • 坐标系变换(坐标系本身变换), 并且 CoordsT: CoordsT = T.inverse().

注意: ROS tf 发布的变换是坐标系变换 CoordsT, 包括通过
tf2_ros static_transform_publisher, TransformBroadcaster.sendTransform()等
发布. 但是获取到的变换却是位姿变换 PosesT, 例如 Transformer.lookupTransform() !!

计算位姿变换 T

  • 某个位姿在 a 坐标系中为: Pa, e.g. (0, 0, 0) * RI,
    该位姿在 b 坐标系中表示为 Pb, e.g. (-1, 0, 0) * Rb,
    那么 a 到 b 的位姿变换 Ta2b = Pb * Pa.inverse(), e.g. (-1, 0, 0) * RT!!
  • 即 a 原点位姿在 b 中的位姿值, 就是 a 到 b 的位姿变换!!

计算 CoordsT

  • 注意: 以同一个公共坐标系计算的是 CoordsT, e.g. 都在 a 坐标系中,
    a 的原点为 P1, b 的原点为 P2, 那么 CoordsT = P2 * P1.inverse(), and
    Ta2b = CoordsT.inverse()    !
  • 又或者都在 world 坐标系中, a 的原点为 Pw1 , b 的原点为 Pw2, 那么 CoordsT =
    Pw2 * Pw1.inverse(), and Ta2b = CoordsT.inverse()    !

See also geometry: MakeTransform:

/**
 * Make a transform: from @a from pose to @a to pose.
 *
 * If T is PosesT, then @a from usually should be (0, 0, 0) * RI,
 * then @a to should be same real pose with @a from, but value is its value
 * in coordinate of @a to.
 *
 * If T is CoordsT, then @a from pose and @a to pose should be values in the
 * same coordinate, e.g. world or coordinate of @a from or coordinate of @a
 * to or some other.
 *
 * And NOTE: CoordsT = T.inverse()!!
 *
 * @param from from pose to build transform.
 * @param to to pose to build transform.
 * @return the @a from to @a to transform (from --transform--> to).
 */
static inline Eigen::Affine3d MakeTransform(
    Eigen::Affine3d const& from, Eigen::Affine3d const& to) noexcept
{
    return to * from.inverse();
}

通过位姿变换 T 计算位姿

已知某个位姿在坐标系 a 中为: Pa, a 中位姿到 b 中位姿变换为 Ta2b, 那么,
Pa 在坐标系 b 中位姿 Pb = Ta2b * Pa.

已知某个位姿在坐标系 a 中为: Pa, b 中位姿到 a 中位姿变换为 Tb2a, 那么,
Pa 在坐标系 b 中位姿 Pb = Tb2a.inverse() * Pa
(Ta2b = Tb2a.inverse()).

坐标变换矩阵

r00 r01 r02 tx
r10 r11 r12 ty
r20 r21 r22 tz
0   0   0   1

仿射

表示位姿或者变换:

  • Affine3d = Translation3d(x, y, z) * Quaterniond(w, x, y, z)
  • Affine2d = Translation2d(x, y) * Rotation2D<double>(angle))

https://yuiwong.org/gitlab/math/geometry

euler-angle-quaternion

yuiwongmath

euler-angle-quaternion conversion

for euler angle and quaternion:
kdl conversion better than “yuiwongcppbase angle.hpp” conversion (maybe / not)
both ok. and sometimes maybe eigen conversion cannot work??
see “yuiwongtransformdemo” test and “yuiwongcppbase angle test”

#

by kdl

# ros package: `tf_conversions` when ros

# cmake
find_package(orocos_kdl REQUIRED)
find_package(kdl_conversions REQUIRED)

${orocos_kdl_LIBRARIES}
${kdl_conversions_LIBRARIES}

euler angle r. p. y. to quaternion

Quaternion Quaternion::fromRpy(
    double const roll, double const pitch, double const yaw)
{
    tf::Quaternion tfq;
    tfq.setRPY(roll, pitch, yaw);
    return Quaternion::fromTfQuaternion(tfq);
}

quaternion to r. p. y.

std::tuple<double, double, double> Quaternion::getRpy() const
{
    KDL::Rotation k;
    tf::quaternionMsgToKDL(*this, k);
    double roll, pitch, yaw;
    k.GetRPY(roll, pitch, yaw);
    return std::tuple<double, double, double>(roll, pitch, yaw);
}

conversion

euler angle r. p. y. to quaternion

Eigen::Quaterniond RpyToQuaternion(
    double const roll, double const pitch, double const yaw)
{
    /* Abbreviations for the various angular functions */
    double const cy = ::cos(yaw * 0.5);
    double const sy = ::sin(yaw * 0.5);
    double const cr = ::cos(roll * 0.5);
    double const sr = ::sin(roll * 0.5);
    double const cp = ::cos(pitch * 0.5);
    double const sp = ::sin(pitch * 0.5);
    Eigen::Quaterniond q;
    q.w() = cy * cr * cp + sy * sr * sp;
    q.x() = cy * sr * cp - sy * cr * sp;
    q.y() = cy * cr * sp + sy * sr * cp;
    q.z() = sy * cr * cp - cy * sr * sp;
    return q;
}

quaternion to r. p. y.

std::tuple<double, double, double> QuaternionToRpy(
    Eigen::Quaterniond const& q)
{
    /* roll (x-axis rotation) */
    double const sinr = +2.0 * (q.w() * q.x() + q.y() * q.z());
    double const cosr = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
    double const roll = ::atan2(sinr, cosr);
    /* pitch (y-axis rotation) */
    double const sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
    double pitch;
    if (DoubleCompare(::fabs(sinp), 1.0) > 0) {
        /* use 90 degrees if out of range */
        pitch = ::copysign(M_PI / 2.0, sinp);
    } else {
        pitch = ::asin(sinp);
    }
    /* yaw (z-axis rotation) */
    double const siny = +2.0 * (q.w() * q.z() + q.x() * q.y());
    double const cosy = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
    double const yaw = ::atan2(siny, cosy);
    return std::tuple<double, double, double>(roll, pitch, yaw);
}

by eigen

see
https://yuiwong.org/gitlab/cpp/cppcomm/blob/master/yuiwongcppbase/include/yuiwong/angle.hpp

see also