assert(dot >= 0) failed

[clemense]

I'm trying to insert elements into an SE3 tree but at some point I get:

nigh/impl/kdtree_batch/traversal_so3.hpp:177: unc::robotics::nigh::impl::kdtree_batch::Traversal<Tree, Key, unc::robotics::nigh::metric::SO3, Get>::Node* unc::robotics::nigh::impl::kdtree_batch::Traversal<Tree, Key, unc::robotics::nigh::metric::SO3, Get>::split(Tree&, const Space&, unc::robotics::nigh::impl::kdtree_batch::Traversal<Tree, Key, unc::robotics::nigh::metric::SO3, Get>::Leaf*, const Key&, unsigned int, Traversal&) [with Traversal = unc::robotics::nigh::impl::kdtree_batch::Traversal<unc::robotics::nigh::Nigh<MyNode, unc::robotics::nigh::metric::Space<std::tuple<Eigen::Quaternion<double, 0>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >, unc::robotics::nigh::metric::Cartesian<unc::robotics::nigh::metric::SO3, unc::robotics::nigh::metric::LP<2> >, void>, MyNodeKey, unc::robotics::nigh::Concurrent, unc::robotics::nigh::KDTreeBatch<> >, std::tuple<Eigen::Quaternion<double, 0>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >, unc::robotics::nigh::metric::Cartesian<unc::robotics::nigh::metric::SO3, unc::robotics::nigh::metric::LP<2> >, unc::robotics::nigh::impl::RootGet>; Tree = unc::robotics::nigh::Nigh<MyNode, unc::robotics::nigh::metric::Space<std::tuple<Eigen::Quaternion<double, 0>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >, unc::robotics::nigh::metric::Cartesian<unc::robotics::nigh::metric::SO3, unc::robotics::nigh::metric::LP<2> >, void>, MyNodeKey, unc::robotics::nigh::Concurrent, unc::robotics::nigh::KDTreeBatch<> >; Key = Eigen::Quaternion<double>; Get = unc::robotics::nigh::impl::CartesianGet<unc::robotics::nigh::impl::RootGet, 0>; unc::robotics::nigh::impl::kdtree_batch::Traversal<Tree, Key, unc::robotics::nigh::metric::SO3, Get>::Node = unc::robotics::nigh::impl::kdtree_batch::Node<MyNode, unc::robotics::nigh::metric::Space<std::tuple<Eigen::Quaternion<double, 0>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >, unc::robotics::nigh::metric::Cartesian<unc::robotics::nigh::metric::SO3, unc::robotics::nigh::metric::LP<2> >, void>, unc::robotics::nigh::Concurrent>; unc::robotics::nigh::impl::kdtree_batch::Traversal<Tree, Key, unc::robotics::nigh::metric::SO3, Get>::Space = unc::robotics::nigh::metric::Space<Eigen::Quaternion<double>, unc::robotics::nigh::metric::SO3, void>; unc::robotics::nigh::impl::kdtree_batch::Traversal<Tree, Key, unc::robotics::nigh::metric::SO3, Get>::Leaf = unc::robotics::nigh::impl::kdtree_batch::Leaf<MyNode, unc::robotics::nigh::metric::Space<std::tuple<Eigen::Quaternion<double, 0>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >, unc::robotics::nigh::metric::Cartesian<unc::robotics::nigh::metric::SO3, unc::robotics::nigh::metric::LP<2> >, void>, unc::robotics::nigh::Concurrent, 8>]: Assertion `dot >= 0' failed.

What exactly does that mean?
I checked the uniqueness of that element I would like to enter, but that's ok.

[jeffi]

That's strange. My initial guess would be that its a duplicate value in the tree (which is currently not supported). Do you have test input that reproduces the assertion?

[clemense]

It only happens when I do set(CMAKE_CXX_FLAGS_RELEASE "-O3").
I explicitly checked the uniqueness of each element of the input. In case it helps, that's the input:

-0.1613302401 -0.0386662401 -0.0376202401 0.0499525 0.578929 0.285496 0.762127
-0.1613302401 -0.0386662401 -0.0376202401 0.762127 -0.285496 0.578929 -0.0499526
-0.1613302401 -0.0336662401 -0.0526202401 -0.106574 0.576114 0.0377605 0.809511
-0.1613302401 -0.0336662401 -0.0526202401 4.5e-09 0.566257 0.112635 0.816497
-0.1613302401 -0.0336662401 -0.0526202401 0.809512 -0.0377605 0.576114 0.106574
-0.1613302401 -0.0336662401 -0.0526202401 0.816496 -0.112635 0.566257 -3.7e-09
-0.1613302401 -0.0336662401 -0.0476202401 0.149003 0.536711 0.358619 0.749087
-0.1613302401 -0.0336662401 -0.0476202401 0.749087 -0.358619 0.536711 -0.149003
-0.1613302401 -0.0336662401 -0.0426202401 0.72323 0.207488 0.611241 0.245503
-0.1613302401 -0.0336662401 -0.0426202401 -0.245503 0.611241 -0.207488 0.72323
-0.1613302401 -0.0336662401 -0.0376202401 0.749087 0.12593 0.633094 0.149003
-0.1613302401 -0.0336662401 -0.0376202401 -0.149003 0.633094 -0.12593 0.749087
-0.1613302401 -0.0286662401 -0.0576202401 4.5e-09 0.566257 0.112635 0.816497
-0.1613302401 -0.0286662401 -0.0576202401 0.816496 -0.112635 0.566257 -3.7e-09
-0.1613302401 -0.0286662401 -0.0526202401 -0.106574 0.576114 0.0377605 0.809511
-0.1613302401 -0.0286662401 -0.0526202401 4.5e-09 0.566257 0.112635 0.816497
-0.1613302401 -0.0286662401 -0.0526202401 0.788675 0.0377605 0.576114 0.211325
-0.1613302401 -0.0286662401 -0.0526202401 0.809512 -0.0377605 0.576114 0.106574
-0.1613302401 -0.0286662401 -0.0526202401 0.816496 -0.112635 0.566257 -3.7e-09
-0.1613302401 -0.0286662401 -0.0526202401 -0.211325 0.576114 -0.0377604 0.788675
-0.1613302401 -0.0286662401 -0.0476202401 0.72323 0.207488 0.611241 0.245503
-0.1613302401 -0.0286662401 -0.0476202401 -0.245503 0.611241 -0.207488 0.72323
-0.1613302401 -0.0286662401 -0.0476202401 0.149003 0.536711 0.358619 0.749087
-0.1613302401 -0.0286662401 -0.0476202401 0.749087 -0.358619 0.536711 -0.149003
-0.1613302401 -0.0286662401 -0.0426202401 0.72323 0.207488 0.611241 0.245503
-0.1613302401 -0.0286662401 -0.0426202401 0.749087 0.12593 0.633094 0.149003
-0.1613302401 -0.0286662401 -0.0426202401 -0.245503 0.611241 -0.207488 0.72323
-0.1613302401 -0.0286662401 -0.0426202401 -0.149003 0.633094 -0.12593 0.749087
-0.1613302401 -0.0286662401 -0.0376202401 0.749087 0.12593 0.633094 0.149003

The format is [x, y, z, qw, qx, qy, qz]. The assertion fails after the last input. KDTreeBatch size is 8.

[jeffi]

Just to check off one possible issue, are you using the SE3 space alias? The default ones put SO(3) before R3, so the tuple must be populated [qw, qx, qy, qz, x, y, z]. (We do this to get better memory alignment by having the 4 element quaternion before the 3 element vector).

Also, please take a look at test/issue_1_test.cpp. I attempted to reproduce the issue therein, but was unsuccessful in getting the assertion. Does the code match what you are doing when you get the assertion?