diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 7d53f10c3..f8cbb7cc4 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -105,7 +105,7 @@ TEST( Point2, norm ) { EXPECT(assert_equal(expectedH,actualH)); actual = x2.norm(actualH); - EXPECT_DOUBLES_EQUAL(sqrt(2), actual, 1e-9); + EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9); expectedH = numericalDerivative11(norm_proxy, x2); EXPECT(assert_equal(expectedH,actualH)); } diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index 47e1dc2b5..dc6fd52e6 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -387,16 +387,16 @@ TEST( Rot3, RQ) // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] CHECK(assert_equal(expected,R.xyz(),1e-6)); - CHECK(assert_equal((Vec(3) <<0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); + CHECK(assert_equal((Vector)(Vec(3) <<0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal((Vec(3) <<0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal((Vec(3) <<0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); + CHECK(assert_equal((Vector)(Vec(3) <<0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal((Vector)(Vec(3) <<0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal((Vec(3) <<0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal((Vec(3) <<0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal((Vec(3) <<0.0,0.0,0.1),Rot3::roll (0.1).ypr())); + CHECK(assert_equal((Vector)(Vec(3) <<0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); + CHECK(assert_equal((Vector)(Vec(3) <<0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); + CHECK(assert_equal((Vector)(Vec(3) <<0.0,0.0,0.1),Rot3::roll (0.1).ypr())); // Try RQ to recover calibration from 3*3 sub-block of projection matrix Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index cc1f641e0..b2012f3bf 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -118,8 +118,8 @@ namespace gtsam { /* ************************************************************************* */ template - template - JunctionTree::JunctionTree(const ETREE& eliminationTree) + template + JunctionTree::JunctionTree(const EliminationTree& eliminationTree) { gttic(JunctionTree_FromEliminationTree); // Here we rely on the BayesNet having been produced by this elimination tree, such that the @@ -130,7 +130,7 @@ namespace gtsam { // Traverse the elimination tree, doing symbolic elimination and merging nodes as we go. Gather // the created junction tree roots in a dummy Node. - typedef typename ETREE::Node ETreeNode; + typedef typename EliminationTree::Node ETreeNode; ConstructorTraversalData rootData(0); rootData.myJTNode = boost::make_shared(); // Make a dummy node to gather the junction tree roots treeTraversal::DepthFirstForest(eliminationTree, rootData, diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h index b470ae95f..cdf38b97c 100644 --- a/gtsam/inference/JunctionTree.h +++ b/gtsam/inference/JunctionTree.h @@ -24,6 +24,9 @@ namespace gtsam { + // Forward declarations + template class EliminationTree; + /** * A JunctionTree is a ClusterTree, i.e., a set of variable clusters with factors, arranged * in a tree, with the additional property that it represents the clique tree associated @@ -64,8 +67,8 @@ namespace gtsam { static This FromEliminationTree(const ETREE& eliminationTree) { return This(eliminationTree); } /** Build the junction tree from an elimination tree. */ - template - JunctionTree(const ETREE& eliminationTree); + template + JunctionTree(const EliminationTree& eliminationTree); /// @} diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp index 0d54103a3..2d5c32e25 100644 --- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp @@ -165,7 +165,7 @@ TEST( GaussianFactorGraph, gradient ) // Construct expected gradient // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] - VectorValues expected = map_list_of + VectorValues expected = map_list_of (1, (Vec(2) << 5.0, -12.5)) (2, (Vec(2) << 30.0, 5.0)) (0, (Vec(2) << -25.0, 17.5)); @@ -239,7 +239,7 @@ TEST( GaussianFactorGraph, multiplyHessianAdd ) { GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - VectorValues x = map_list_of + VectorValues x = map_list_of (0, (Vec(2) << 1,2)) (1, (Vec(2) << 3,4)) (2, (Vec(2) << 5,6)); @@ -277,7 +277,7 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 ) Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450; EXPECT(assert_equal(Y,AtA*X)); - VectorValues x = map_list_of + VectorValues x = map_list_of (0, (Vec(2) << 1,2)) (1, (Vec(2) << 3,4)) (2, (Vec(2) << 5,6)); diff --git a/gtsam/linear/tests/testHessianFactorUnordered.cpp b/gtsam/linear/tests/testHessianFactorUnordered.cpp index f2793afbe..52c85dbf6 100644 --- a/gtsam/linear/tests/testHessianFactorUnordered.cpp +++ b/gtsam/linear/tests/testHessianFactorUnordered.cpp @@ -441,7 +441,7 @@ TEST(HessianFactor, gradientAtZero) HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); // test gradient at zero - VectorValues expectedG = pair_list_of(0, -g1) (1, -g2); + VectorValues expectedG = pair_list_of(0, -g1) (1, -g2); Matrix A; Vector b; boost::tie(A,b) = factor.jacobian(); FastVector keys; keys += 0,1; EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys))); diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index f6face900..6dd9dd1b5 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -112,13 +112,13 @@ public: message); else beliefAtKey = - make_shared( + boost::make_shared( (*beliefAtKey) * (*boost::dynamic_pointer_cast( message))); } if (starGraphs_.at(key).unary) - beliefAtKey = make_shared( + beliefAtKey = boost::make_shared( (*beliefAtKey) * (*starGraphs_.at(key).unary)); if (debug) beliefAtKey->print("New belief at key: "); // normalize belief @@ -133,7 +133,7 @@ public: sumFactorTable = (boost::format("%s %f") % sumFactorTable % sum).str(); DecisionTreeFactor sumFactor(allDiscreteKeys.at(key), sumFactorTable); if (debug) sumFactor.print("denomFactor: "); - beliefAtKey = make_shared((*beliefAtKey) / sumFactor); + beliefAtKey = boost::make_shared((*beliefAtKey) / sumFactor); if (debug) beliefAtKey->print("New belief at key normalized: "); beliefs->push_back(beliefAtKey); allMessages[key] = messages; @@ -184,7 +184,7 @@ private: prodOfUnaries = boost::dynamic_pointer_cast( graph.at(factorIdx)); else - prodOfUnaries = make_shared( + prodOfUnaries = boost::make_shared( *prodOfUnaries * (*boost::dynamic_pointer_cast( graph.at(factorIdx)))); diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index 22dbbc3d2..afbf03ffd 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -37,7 +37,7 @@ namespace gtsam { /// Exception thrown by triangulateDLT when SVD returns rank < 3 -class GTSAM_UNSTABLE_EXPORT TriangulationUnderconstrainedException: public std::runtime_error { +class TriangulationUnderconstrainedException: public std::runtime_error { public: TriangulationUnderconstrainedException() : std::runtime_error("Triangulation Underconstrained Exception.") { @@ -45,7 +45,7 @@ public: }; /// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras -class GTSAM_UNSTABLE_EXPORT TriangulationCheiralityException: public std::runtime_error { +class TriangulationCheiralityException: public std::runtime_error { public: TriangulationCheiralityException() : std::runtime_error( diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 4ab77c8d8..096e0427f 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -92,27 +92,13 @@ void ConcurrentBatchFilter::PrintLinearFactorGraph(const GaussianFactorGraph& fa } } -/* ************************************************************************* */ -template<> -void ConcurrentBatchFilter::PrintKeys(const Values& values, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { - FastList keys = values.keys(); - PrintKeys(keys, indent, title, keyFormatter); -} - -/* ************************************************************************* */ -template<> -void ConcurrentBatchFilter::PrintKeys(const NonlinearFactorGraph& graph, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) { - FastSet keys = graph.keys(); - PrintKeys(keys, indent, title, keyFormatter); -} - /* ************************************************************************* */ void ConcurrentBatchFilter::print(const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s; PrintNonlinearFactorGraph(factors_, " ", "Factors:"); - PrintKeys(theta_, " ", "Values:"); + PrintKeys(theta_.keys(), " ", "Values:"); PrintNonlinearFactorGraph(smootherFactors_, " ", "Cached Smoother Factors:"); - PrintKeys(smootherValues_, " ", "Cached Smoother Values:"); + PrintKeys(smootherValues_.keys(), " ", "Cached Smoother Values:"); } /* ************************************************************************* */ @@ -586,8 +572,8 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { if(debug) { PrintNonlinearFactorGraph(removedFactors, "ConcurrentBatchFilter::synchronize ", "Removed Factors:", DefaultKeyFormatter); PrintNonlinearFactorGraph(smootherShortcut_, "ConcurrentBatchFilter::synchronize ", "Old Shortcut:", DefaultKeyFormatter); - PrintKeys(smootherShortcut_, "ConcurrentBatchFilter::moveSeparator ", "Old Shortcut Keys:", DefaultKeyFormatter); - PrintKeys(separatorValues_, "ConcurrentBatchFilter::moveSeparator ", "Previous Separator Keys:", DefaultKeyFormatter); + PrintKeys(smootherShortcut_.keys(), "ConcurrentBatchFilter::moveSeparator ", "Old Shortcut Keys:", DefaultKeyFormatter); + PrintKeys(separatorValues_.keys(), "ConcurrentBatchFilter::moveSeparator ", "Previous Separator Keys:", DefaultKeyFormatter); } // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 745631fac..fce28b214 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -236,8 +236,7 @@ private: /** Print just the nonlinear keys contained inside a container */ template - static void PrintKeys(const Container& keys, const std::string& indent = "", - const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); + static void PrintKeys(const Container& keys, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter = DefaultKeyFormatter); }; // ConcurrentBatchFilter diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 15f8ba9ce..45b640999 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -70,8 +70,8 @@ std::pair AHRS::initialize(double g_e) // Calculate Omega_T, formula 2.80 in Farrell08book double cp = cos(mech0_.bRn().inverse().pitch()); double sp = sin(mech0_.bRn().inverse().pitch()); - double cy = cos(0); - double sy = sin(0); + double cy = cos(0.0); + double sy = sin(0.0); Matrix Omega_T = Matrix_(3, 3, cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0); // Calculate Jacobian of roll/pitch/yaw wrpt (g1,g2,g3), see doc/ypr.nb diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index c416a7d52..385bab9ac 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -53,8 +53,8 @@ TEST( SmartRangeFactor, addRange ) { TEST( SmartRangeFactor, scenario ) { DOUBLES_EQUAL(10, r1, 1e-9); - DOUBLES_EQUAL(sqrt(100+25), r2, 1e-9); - DOUBLES_EQUAL(sqrt(50), r3, 1e-9); + DOUBLES_EQUAL(sqrt(100.0+25.0), r2, 1e-9); + DOUBLES_EQUAL(sqrt(50.0), r3, 1e-9); } /* ************************************************************************* */ @@ -89,7 +89,7 @@ TEST( SmartRangeFactor, unwhitenedError ) { Vector actual4 = f.unwhitenedError(values, H); // with H now ! EXPECT(assert_equal((Vec(1) << 0.0), actual4)); CHECK(assert_equal(Matrix_(1,3, 0.0,-1.0,0.0), H.front())); - CHECK(assert_equal(Matrix_(1,3, sqrt(2)/2,-sqrt(2)/2,0.0), H.back())); + CHECK(assert_equal(Matrix_(1,3, sqrt(2.0)/2,-sqrt(2.0)/2,0.0), H.back())); // Test clone NonlinearFactor::shared_ptr clone = f.clone(); @@ -109,7 +109,7 @@ TEST( SmartRangeFactor, optimization ) { initial.insert(2, pose2); initial.insert(3, Pose2(5, 6, 0)); // does not satisfy range measurement Vector actual5 = f.unwhitenedError(initial); - EXPECT(assert_equal(Vector_(1,sqrt(25+16)-sqrt(50)), actual5)); + EXPECT(assert_equal(Vector_(1,sqrt(25.0+16.0)-sqrt(50.0)), actual5)); // Create Factor graph NonlinearFactorGraph graph;