ch4/example/trajectoryError 运行可执行文件出现错误

[chaksw]

高老师、张老师您好，
我在尝试编译trajectoryError程序过程中，在编译无错误并成功生成可执行文件后，运行可执行文件时遇见了一下问题。
machine:/mnt/hgfs/slambook2/ch4/example/build$ cmake ..
-- Configuring done
-- Generating done
-- Build files have been written to: /mnt/hgfs/slambook2/ch4/example/build
machine:/mnt/hgfs/slambook2/ch4/example/build$ make
[100%] Built target trajectoryError
machine:/mnt/hgfs/slambook2/ch4/example/build$ ./trajectoryError
Sophus ensure failed in function 'void Sophus::SO3Base::normalize() [with Derived = Sophus::SO3]', file '/usr/local/include/sophus/so3.hpp', line 275.
Quaternion (6.95256e-310 6.94798e-310 6.95256e-310 6.94798e-310) should not be close to zero!
Aborted (core dumped)

根据我自己分析，是构造四元数时某个数据值过小（接近零），但翻查路径数据文件时没有发现与错误相似大小的数据，请问出现这个错误是什么原因？ 希望有空可以解答下，谢谢！

[junlin49]

我也出现了这个问题

[gaoxiang12]

应该是某个版本的sophus增加了四元数归一化检查。我觉得在读取时加一句归一化应该能解决这个问题。

[ogtc890215]

我在运行第十章的例子时遇到同样的问题
slambook2/ch10$ ./build/pose_graph_g2o_lie sphere.g2o
Sophus ensure failed in function 'void Sophus::SO3Base::normalize() [with Derived = Sophus::SO3]', file '/usr/local/include/sophus/so3.hpp', line 299.
Quaternion ( 0.706662 4.32706e-17 0.707551 -4.3325e-17) should not be close to zero!
Aborted

定位到的问题发生在构建SE3d的实例中

···
class VertexSE3LieAlgebra : public g2o::BaseVertex<6, SE3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

virtual bool read(istream &is) override {
    double data[7];
    for (int i = 0; i < 7; i++)
        is >> data[i];
    setEstimate(SE3d(
        Quaterniond(data[6], data[3], data[4], data[5]),
        Vector3d(data[0], data[1], data[2])
    ));
}

···

我将四元数和李代数的构建单独提取出来，并对四元数进行归一化，都不能解决上述错误。
从Sophus的代码看，在构造SO3实例时会进行归一化，归一化中有检查，这些检查代码在submodule的节点里已经包含，所以想请教老师们在测试时是如何绕开这个报错的

···
SOPHUS_FUNC void normalize() {
Scalar length = unit_quaternion_nonconst().norm();
SOPHUS_ENSURE(length >= Constants::epsilon(),
"Quaternion (%) should not be close to zero!",
unit_quaternion_nonconst().coeffs().transpose());
unit_quaternion_nonconst().coeffs() /= length;
}

// Constructor from quaternion and translation vector.
//
// Precondition: quaternion must not be close to zero.
//
SOPHUS_FUNC SE3(Eigen::Quaternion<Scalar> const& quaternion,
                Point const& translation)
    : so3_(quaternion), translation_(translation) {}

···

另外我也尝试用宏SOPHUS_DISABLE_ENSURES来关闭检查，但在运行时直接发生段错误。

[YuanwenFu]

@chaksw @junlin49 @gaoxiang12 @ogtc890215 你们好！对于这个问题

Sophus ensure failed in function 'void Sophus::SO3Base<Derived>::normalize() [with Derived = Sophus::SO3<double>]', file '/usr/local/include/sophus/so3.hpp', line 300.
Quaternion (   0.706662 4.32706e-17    0.707551 -4.3325e-17) should not be close to zero!

是由于read函数没有返回true，修改之后即可正确执行！
将

virtual bool read(istream &is) override {
    double data[7];
    for (int i = 0; i < 7; i++)
        is >> data[i];
    setEstimate(SE3d(
        Quaterniond(data[6], data[3], data[4], data[5]),
        Vector3d(data[0], data[1], data[2])
    ));
}

改为，

virtual bool read(istream &is) override {
    double data[7];
    for (int i = 0; i < 7; i++)
        is >> data[i];
    setEstimate(SE3d(
        Quaterniond(data[6], data[3], data[4], data[5]),
        Vector3d(data[0], data[1], data[2])
    ));
   return true;
}

[junlin49]

我现在发现，之所以出现这个问题，是因为我读取数据时有错误，导致四元数都是0；错误出现在我在读取数据时使用了以下方式：
while (!ifs.eof()) { ... }
正确方法：
while (ifs.peek() != EOF) { ... }
由此解决问题

[mnslarcher]

My problem for ch4 was that I had the two input files but they were empty, after replacing them with the correct files everything worked.

[ikkelsen]

我在第四章就遇到了同样的问题，但是没法找到virtual bool read（。。）这个函数该怎么处理呢？
./trajectoryError
Sophus ensure failed in function 'void Sophus::SO3Base::normalize() [with Derived = Sophus::SO3]', file '/usr/local/include/sophus/so3.hpp', line 273.
Quaternion (6.91806e-310 8.12478e-315 0 0) should not be close to zero!
已放弃 (核心已转储)

[ikkelsen]

while (fin.peek() != EOF) {
//while (!fin.eof()) {
double time, tx, ty, tz, qx, qy, qz, qw;
fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
trajectory.push_back(p1);
}
In ch4/example/trajectoryError.cpp, By replacing the function, (you need to find the fun)This can work correctly!