From 7083de35a4aea261ab5a2e16585a94307b890c23 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 7 Oct 2021 15:54:56 -0400 Subject: [PATCH 01/10] add failing unit test on axisAngle for Rot3 in c++ --- gtsam/geometry/tests/testRot3.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 34f90c8cc..5f0187ed9 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -122,6 +122,21 @@ TEST( Rot3, AxisAngle) CHECK(assert_equal(expected,actual3,1e-5)); } +/* ************************************************************************* */ +TEST( Rot3, AxisAngle2) +{ + // constructor from a rotation matrix, as doubles in *row-major* order. + Rot3 R1(-0.999957, 0.00922903, 0.00203116, 0.00926964, 0.999739, 0.0208927, -0.0018374, 0.0209105, -0.999781); + + Unit3 actualAxis; + double actualAngle; + // convert Rot3 to quaternion using GTSAM + std::tie(actualAxis, actualAngle) = R1.axisAngle(); + + double expectedAngle = 3.1396582; + CHECK(assert_equal(expectedAngle, actualAngle, 1e-7)); +} + /* ************************************************************************* */ TEST( Rot3, Rodrigues) { From bb87dcf759964f7d4aa5dffa751e8c729129bf8e Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 7 Oct 2021 17:20:47 -0400 Subject: [PATCH 02/10] add python unit test for Rot3 --- python/gtsam/tests/test_Rot3.py | 41 +++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 python/gtsam/tests/test_Rot3.py diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py new file mode 100644 index 000000000..273e0e210 --- /dev/null +++ b/python/gtsam/tests/test_Rot3.py @@ -0,0 +1,41 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +See LICENSE for the license information +Rot3 unit tests. +Author: John Lambert +""" +# pylint: disable=no-name-in-module + +import unittest + +import numpy as np + +import gtsam +from gtsam import Rot3 +from gtsam.utils.test_case import GtsamTestCase + + +class TestRot3(GtsamTestCase): + """Test selected Rot3 methods.""" + + def test_axisangle(self) -> None: + """Test .axisAngle() method.""" + # fmt: off + R = np.array( + [ + [ -0.999957, 0.00922903, 0.00203116], + [ 0.00926964, 0.999739, 0.0208927], + [ -0.0018374, 0.0209105, -0.999781] + ]) + # fmt: on + + # get back angle in radians + _, actual_angle = Rot3(R).axisAngle() + expected_angle = 3.1396582; + self.gtsamAssertEquals(actual_angle, expected_angle, 1e-7) + + +if __name__ == "__main__": + unittest.main() From 0b0897d465f5121f2cfad0888fc3bf045d9bf3ac Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 7 Oct 2021 17:21:22 -0400 Subject: [PATCH 03/10] fix typo --- python/gtsam/tests/test_Rot3.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py index 273e0e210..d22182433 100644 --- a/python/gtsam/tests/test_Rot3.py +++ b/python/gtsam/tests/test_Rot3.py @@ -33,7 +33,7 @@ class TestRot3(GtsamTestCase): # get back angle in radians _, actual_angle = Rot3(R).axisAngle() - expected_angle = 3.1396582; + expected_angle = 3.1396582 self.gtsamAssertEquals(actual_angle, expected_angle, 1e-7) From 225ac77f2f524ef4828cb7728628deb03d1aeddf Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 7 Oct 2021 21:04:58 -0400 Subject: [PATCH 04/10] fix assert --- python/gtsam/tests/test_Rot3.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py index d22182433..90c80ab31 100644 --- a/python/gtsam/tests/test_Rot3.py +++ b/python/gtsam/tests/test_Rot3.py @@ -34,7 +34,7 @@ class TestRot3(GtsamTestCase): # get back angle in radians _, actual_angle = Rot3(R).axisAngle() expected_angle = 3.1396582 - self.gtsamAssertEquals(actual_angle, expected_angle, 1e-7) + np.testing.assert_almost_equal(actual_angle, expected_angle, 1e-7) if __name__ == "__main__": From 21279e6d5107f85021ba0788681b0733502a79bb Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 10 Oct 2021 16:20:30 -0400 Subject: [PATCH 05/10] Revert "Revert "replace deprecated tbb functionality"" This reverts commit f819b1a03f74f289f50b96125610026618601a2f. --- gtsam/base/treeTraversal-inst.h | 5 +- .../treeTraversal/parallelTraversalTasks.h | 70 +++++++------------ 2 files changed, 29 insertions(+), 46 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 7a88f72eb..30cec3b9a 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -158,9 +158,8 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, // Typedefs typedef typename FOREST::Node Node; - tbb::task::spawn_root_and_wait( - internal::CreateRootTask(forest.roots(), rootData, visitorPre, - visitorPost, problemSizeThreshold)); + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold); #else DepthFirstForest(forest, rootData, visitorPre, visitorPost); #endif diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 87d5b0d4c..dc1b45906 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,7 +22,7 @@ #include #ifdef GTSAM_USE_TBB -#include // tbb::task, tbb::task_list +#include // tbb::task_group #include // tbb::scalable_allocator namespace gtsam { @@ -34,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ template - class PreOrderTask : public tbb::task + class PreOrderTask { public: const boost::shared_ptr& treeNode; @@ -42,28 +42,30 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; bool makeNewTasks; - bool isPostOrderPhase; + // Keep track of order phase across multiple calls to the same functor + mutable bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - bool makeNewTasks = true) + tbb::task_group& tg, bool makeNewTasks = true) : treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold), + tg(tg), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - tbb::task* execute() override + void operator()() const { if(isPostOrderPhase) { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); - return nullptr; } else { @@ -71,14 +73,10 @@ namespace gtsam { { if(!treeNode->children.empty()) { - // Allocate post-order task as a continuation - isPostOrderPhase = true; - recycle_as_continuation(); - bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); - tbb::task* firstChild = 0; - tbb::task_list childTasks; + // If we have child tasks, start subtasks and wait for them to complete + tbb::task_group ctg; for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling @@ -86,37 +84,30 @@ namespace gtsam { // allocated an extra child, this causes a TBB error. boost::shared_ptr childData = boost::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); - tbb::task* childTask = - new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, overThreshold); - if (firstChild) - childTasks.push_back(*childTask); - else - firstChild = childTask; + ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, ctg, overThreshold)); } + ctg.wait(); - // If we have child tasks, start subtasks and wait for them to complete - set_ref_count((int)treeNode->children.size()); - spawn(childTasks); - return firstChild; + // Allocate post-order task as a continuation + isPostOrderPhase = true; + tg.run(*this); } else { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); - return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); - return nullptr; } } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const { for(const boost::shared_ptr& child: node->children) { @@ -131,7 +122,7 @@ namespace gtsam { /* ************************************************************************* */ template - class RootTask : public tbb::task + class RootTask { public: const ROOTS& roots; @@ -139,38 +130,31 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold) : + int problemSizeThreshold, tbb::task_group& tg) : roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold) {} + problemSizeThreshold(problemSizeThreshold), tg(tg) {} - tbb::task* execute() override + void operator()() const { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children - tbb::task_list tasks; for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); - tasks.push_back(*new(allocate_child()) - PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); + tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); } - // Set TBB ref count - set_ref_count(1 + (int) roots.size()); - // Spawn tasks - spawn_and_wait_for_all(tasks); - // Return nullptr - return nullptr; } }; template - RootTask& - CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) { typedef RootTask RootTask; - return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); - } + tbb::task_group tg; + tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); + } } From 1a97b1413825045d7ff8f6e4a15fab64b8bcb7e4 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 12 Oct 2021 19:02:40 -0400 Subject: [PATCH 06/10] Fixed numerical problems near +-pi --- gtsam/geometry/SO3.cpp | 30 ++++++++++++++++++++--------- gtsam/geometry/tests/testRot3.cpp | 32 +++++++++++++++---------------- 2 files changed, 37 insertions(+), 25 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 80c0171ad..a2309e1f4 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -261,25 +261,37 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special - if (tr + 1.0 < 1e-10) { - if (std::abs(R33 + 1.0) > 1e-5) - omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); - else if (std::abs(R22 + 1.0) > 1e-5) - omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); - else + if (tr + 1.0 < 1e-4) { + std::vector diags = {R11, R22, R33}; + size_t max_elem = std::distance(diags.begin(), std::max_element(diags.begin(), diags.end())); + if (max_elem == 2) { + const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0; + const double r = sqrt(2.0 + 2.0 * R33); + const double correction = 1.0 - M_1_PI / r * (R21 - R12); + omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R33)) * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); + } else if (max_elem == 1) { + const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0; + const double r = sqrt(2.0 + 2.0 * R22); + const double correction = 1.0 - M_1_PI / r * (R13 - R31); + omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R22)) * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); + } else { // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit - omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); + const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; + const double r = sqrt(2.0 + 2.0 * R11); + const double correction = 1.0 - M_1_PI / r * (R32 - R23); + omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R11)) * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31); + } } else { double magnitude; const double tr_3 = tr - 3.0; // always negative - if (tr_3 < -1e-7) { + if (tr_3 < -1e-6) { double theta = acos((tr - 1.0) / 2.0); magnitude = theta / (2.0 * sin(theta)); } else { // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) // see https://github.com/borglab/gtsam/issues/746 for details - magnitude = 0.5 - tr_3 / 12.0; + magnitude = 0.5 - tr_3 / 12.0 + tr_3*tr_3/60.0; } omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 5f0187ed9..bb6f1a77a 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -134,7 +134,7 @@ TEST( Rot3, AxisAngle2) std::tie(actualAxis, actualAngle) = R1.axisAngle(); double expectedAngle = 3.1396582; - CHECK(assert_equal(expectedAngle, actualAngle, 1e-7)); + CHECK(assert_equal(expectedAngle, actualAngle, 1e-5)); } /* ************************************************************************* */ @@ -196,13 +196,13 @@ TEST( Rot3, retract) } /* ************************************************************************* */ -TEST(Rot3, log) { +TEST( Rot3, log) { static const double PI = boost::math::constants::pi(); Vector w; Rot3 R; #define CHECK_OMEGA(X, Y, Z) \ - w = (Vector(3) << X, Y, Z).finished(); \ + w = (Vector(3) << (X), (Y), (Z)).finished(); \ R = Rot3::Rodrigues(w); \ EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12)); @@ -234,17 +234,17 @@ TEST(Rot3, log) { CHECK_OMEGA(0, 0, PI) // Windows and Linux have flipped sign in quaternion mode -#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS) +//#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS) w = (Vector(3) << x * PI, y * PI, z * PI).finished(); R = Rot3::Rodrigues(w); EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12)); -#else - CHECK_OMEGA(x * PI, y * PI, z * PI) -#endif +//#else +// CHECK_OMEGA(x * PI, y * PI, z * PI) +//#endif // Check 360 degree rotations #define CHECK_OMEGA_ZERO(X, Y, Z) \ - w = (Vector(3) << X, Y, Z).finished(); \ + w = (Vector(3) << (X), (Y), (Z)).finished(); \ R = Rot3::Rodrigues(w); \ EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R))); @@ -262,15 +262,15 @@ TEST(Rot3, log) { // Rot3's Logmap returns different, but equivalent compacted // axis-angle vectors depending on whether Rot3 is implemented // by Quaternions or SO3. - #if defined(GTSAM_USE_QUATERNIONS) - // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees - EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211), +#if defined(GTSAM_USE_QUATERNIONS) + // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees + EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211), + (Vector)Rot3::Logmap(Rlund), 1e-8)); +#else + // SO3 will be approximate because of the non-orthogonality + EXPECT(assert_equal(Vector3(0.264485272, -0.742291088, -3.04136444), (Vector)Rot3::Logmap(Rlund), 1e-8)); - #else - // SO3 does not bound angle resulting in ~180.1 degrees - EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314), - (Vector)Rot3::Logmap(Rlund), 1e-8)); - #endif +#endif } /* ************************************************************************* */ From dae409373c66abd6c41cb851a9c4a54b8e625dec Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 12 Oct 2021 19:03:56 -0400 Subject: [PATCH 07/10] Revert "Revert "Revert "replace deprecated tbb functionality""" This reverts commit 21279e6d5107f85021ba0788681b0733502a79bb. --- gtsam/base/treeTraversal-inst.h | 5 +- .../treeTraversal/parallelTraversalTasks.h | 70 ++++++++++++------- 2 files changed, 46 insertions(+), 29 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 30cec3b9a..7a88f72eb 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -158,8 +158,9 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, // Typedefs typedef typename FOREST::Node Node; - internal::CreateRootTask(forest.roots(), rootData, visitorPre, - visitorPost, problemSizeThreshold); + tbb::task::spawn_root_and_wait( + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold)); #else DepthFirstForest(forest, rootData, visitorPre, visitorPost); #endif diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index dc1b45906..87d5b0d4c 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,7 +22,7 @@ #include #ifdef GTSAM_USE_TBB -#include // tbb::task_group +#include // tbb::task, tbb::task_list #include // tbb::scalable_allocator namespace gtsam { @@ -34,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ template - class PreOrderTask + class PreOrderTask : public tbb::task { public: const boost::shared_ptr& treeNode; @@ -42,30 +42,28 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; - tbb::task_group& tg; bool makeNewTasks; - // Keep track of order phase across multiple calls to the same functor - mutable bool isPostOrderPhase; + bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - tbb::task_group& tg, bool makeNewTasks = true) + bool makeNewTasks = true) : treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold), - tg(tg), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - void operator()() const + tbb::task* execute() override { if(isPostOrderPhase) { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); + return nullptr; } else { @@ -73,10 +71,14 @@ namespace gtsam { { if(!treeNode->children.empty()) { + // Allocate post-order task as a continuation + isPostOrderPhase = true; + recycle_as_continuation(); + bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); - // If we have child tasks, start subtasks and wait for them to complete - tbb::task_group ctg; + tbb::task* firstChild = 0; + tbb::task_list childTasks; for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling @@ -84,30 +86,37 @@ namespace gtsam { // allocated an extra child, this causes a TBB error. boost::shared_ptr childData = boost::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); - ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, ctg, overThreshold)); + tbb::task* childTask = + new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, overThreshold); + if (firstChild) + childTasks.push_back(*childTask); + else + firstChild = childTask; } - ctg.wait(); - // Allocate post-order task as a continuation - isPostOrderPhase = true; - tg.run(*this); + // If we have child tasks, start subtasks and wait for them to complete + set_ref_count((int)treeNode->children.size()); + spawn(childTasks); + return firstChild; } else { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); + return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); + return nullptr; } } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) { for(const boost::shared_ptr& child: node->children) { @@ -122,7 +131,7 @@ namespace gtsam { /* ************************************************************************* */ template - class RootTask + class RootTask : public tbb::task { public: const ROOTS& roots; @@ -130,31 +139,38 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; - tbb::task_group& tg; RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold, tbb::task_group& tg) : + int problemSizeThreshold) : roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold), tg(tg) {} + problemSizeThreshold(problemSizeThreshold) {} - void operator()() const + tbb::task* execute() override { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children + tbb::task_list tasks; for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); - tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); + tasks.push_back(*new(allocate_child()) + PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); } + // Set TBB ref count + set_ref_count(1 + (int) roots.size()); + // Spawn tasks + spawn_and_wait_for_all(tasks); + // Return nullptr + return nullptr; } }; template - void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + RootTask& + CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) { typedef RootTask RootTask; - tbb::task_group tg; - tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); - } + return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); + } } From aa6b4329110368c6cd6985f3d77f6af9e3572bd9 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 12 Oct 2021 20:30:06 -0400 Subject: [PATCH 08/10] Cleanup equation --- gtsam/geometry/SO3.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index a2309e1f4..0cb6c93d1 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -267,19 +267,19 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { if (max_elem == 2) { const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R33); - const double correction = 1.0 - M_1_PI / r * (R21 - R12); - omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R33)) * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); + const double scale = M_PI_2 / r - 0.5 / (r * r) * (R21 - R12); + omega = sgn_w * scale * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); } else if (max_elem == 1) { const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R22); - const double correction = 1.0 - M_1_PI / r * (R13 - R31); - omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R22)) * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); + const double scale = M_PI_2 / r - 0.5 / (r * r) * (R13 - R31); + omega = sgn_w * scale * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); } else { // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R11); - const double correction = 1.0 - M_1_PI / r * (R32 - R23); - omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R11)) * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31); + const double scale = M_PI_2 / r - 0.5 / (r * r) * (R32 - R23); + omega = sgn_w * scale * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31); } } else { double magnitude; From 35061ca1356b081f94712ec5f343fb9592cf0d21 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 13 Oct 2021 13:46:33 -0400 Subject: [PATCH 09/10] simplify logic of biggest diagonal --- gtsam/geometry/SO3.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 0cb6c93d1..37ea3840b 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -262,20 +262,20 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special if (tr + 1.0 < 1e-4) { - std::vector diags = {R11, R22, R33}; - size_t max_elem = std::distance(diags.begin(), std::max_element(diags.begin(), diags.end())); - if (max_elem == 2) { + if (R33 > R22 && R33 > R11) { + // R33 is the largest diagonal const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R33); const double scale = M_PI_2 / r - 0.5 / (r * r) * (R21 - R12); omega = sgn_w * scale * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); - } else if (max_elem == 1) { + } else if (R22 > R11) { + // R22 is the largest diagonal const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R22); const double scale = M_PI_2 / r - 0.5 / (r * r) * (R13 - R31); omega = sgn_w * scale * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); } else { - // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit + // R33 is the largest diagonal const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R11); const double scale = M_PI_2 / r - 0.5 / (r * r) * (R32 - R23); @@ -283,8 +283,9 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { } } else { double magnitude; - const double tr_3 = tr - 3.0; // always negative + const double tr_3 = tr - 3.0; // could be non-negative if the matrix is off orthogonal if (tr_3 < -1e-6) { + // this is the normal case -1 < trace < 3 double theta = acos((tr - 1.0) / 2.0); magnitude = theta / (2.0 * sin(theta)); } else { From 48cd9794daaa87517d217bd714118202bda1f427 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 13 Oct 2021 13:48:41 -0400 Subject: [PATCH 10/10] Fix typo --- gtsam/geometry/SO3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 37ea3840b..890bcd206 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -275,7 +275,7 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { const double scale = M_PI_2 / r - 0.5 / (r * r) * (R13 - R31); omega = sgn_w * scale * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); } else { - // R33 is the largest diagonal + // R11 is the largest diagonal const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R11); const double scale = M_PI_2 / r - 0.5 / (r * r) * (R32 - R23);