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.